diff --git a/config/o30_three_camera_extrinsics_20260923_181333.yaml b/config/o30_three_camera_extrinsics_20260923_181333.yaml new file mode 100644 index 0000000..da7e1e8 --- /dev/null +++ b/config/o30_three_camera_extrinsics_20260923_181333.yaml @@ -0,0 +1,60 @@ +schema_version: 1 +reference_view: front +cameras: + front: + serial_number: DB2163742 + width: 1624 + height: 1240 + intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07 + side: + serial_number: DB2163749 + width: 1624 + height: 1240 + intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655 + top: + serial_number: DB2163739 + width: 1624 + height: 1240 + intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446 +front_from_view: + front: + translation_xyz_m: + - 0.0 + - 0.0 + - 0.0 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + side: + translation_xyz_m: + - -0.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 diff --git a/src/linkerhand_calibration/O30_AUTOMATIC_URDF_FIX.md b/src/linkerhand_calibration/O30_AUTOMATIC_URDF_FIX.md new file mode 100644 index 0000000..0ac85bd --- /dev/null +++ b/src/linkerhand_calibration/O30_AUTOMATIC_URDF_FIX.md @@ -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/` +分别保存来源核验、加载、精度、冻结输入和软件测试记录。 +本次未放宽精度/可观测性门限，未替换仿真默认模型，未发布正式版本。 diff --git a/src/linkerhand_calibration/O30_CURRENT_FLOW_REVIEW.md b/src/linkerhand_calibration/O30_CURRENT_FLOW_REVIEW.md new file mode 100644 index 0000000..1833754 --- /dev/null +++ b/src/linkerhand_calibration/O30_CURRENT_FLOW_REVIEW.md @@ -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`。 diff --git a/src/linkerhand_calibration/O30_OFFLINE_LOGIC_REVIEW.md b/src/linkerhand_calibration/O30_OFFLINE_LOGIC_REVIEW.md new file mode 100644 index 0000000..9fe765c --- /dev/null +++ b/src/linkerhand_calibration/O30_OFFLINE_LOGIC_REVIEW.md @@ -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`。本轮仅复核证据及更正说明，未修改生产代码或模型。 diff --git a/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md b/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md index 09ff7d2..1d976f7 100644 --- a/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md +++ b/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md @@ -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`。 diff --git a/src/linkerhand_calibration/O30_REMAINING_ISSUES.md b/src/linkerhand_calibration/O30_REMAINING_ISSUES.md new file mode 100644 index 0000000..4d8f5e9 --- /dev/null +++ b/src/linkerhand_calibration/O30_REMAINING_ISSUES.md @@ -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`，便于新批次做同项对照。 diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index e876153..890171d 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -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 基准指令下的像素/反馈差异及不可见情况 +列为“待离线复核”，不将回位姿态差异直接判成标签重装，不据此暂停采集。 +固定基准、配置身份及运动保护仍须通过。前后安装参考、差异和原补采预算均保留； +未核实差异只允许进入未认证修正草稿，正式发布仍拒绝未复核的安装关系。 +标签必须仍在原配置连杆上；若移到了其他指节，先修正连杆绑定。 diff --git a/src/linkerhand_calibration/TESTING.md b/src/linkerhand_calibration/TESTING.md index a1fafdd..1d8f192 100644 --- a/src/linkerhand_calibration/TESTING.md +++ b/src/linkerhand_calibration/TESTING.md @@ -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`。 diff --git a/src/linkerhand_calibration/config/o30_right_product.yaml b/src/linkerhand_calibration/config/o30_right_product.yaml index b6bd29f..86df987 100644 --- a/src/linkerhand_calibration/config/o30_right_product.yaml +++ b/src/linkerhand_calibration/config/o30_right_product.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/profiles/o30_right_18.yaml b/src/linkerhand_calibration/config/profiles/o30_right_18.yaml index 6370e31..fbb535a 100644 --- a/src/linkerhand_calibration/config/profiles/o30_right_18.yaml +++ b/src/linkerhand_calibration/config/profiles/o30_right_18.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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py index b8acd7c..7a31511 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py @@ -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): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py index 49526bf..2a81cc6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_support.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_support.py new file mode 100644 index 0000000..8ecaf6d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_support.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_draft.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_draft.py new file mode 100644 index 0000000..8bf3294 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_draft.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_uncertainty.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_uncertainty.py index cab6a8b..8093541 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_uncertainty.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_uncertainty.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_update.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_update.py new file mode 100644 index 0000000..28274e4 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_update.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_validation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_validation.py index 4367340..ad2b410 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_validation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/raw_joint_validation.py @@ -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, diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/finger_chain_images.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/finger_chain_images.py index d512e85..18285bc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/finger_chain_images.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/finger_chain_images.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py index 7c6a044..d749121 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py @@ -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(): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/urdf_command_images.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/urdf_command_images.py index 69062b9..02687df 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/urdf_command_images.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/urdf_command_images.py @@ -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]), diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py index 1c24ca7..7a056bc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/linear_drive.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/linear_drive.py new file mode 100644 index 0000000..51cabfc --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/linear_drive.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py index d945ce9..39783a9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py index 740b999..7027605 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py @@ -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, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py index 0e43a78..0fa6f26 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py @@ -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", {}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py index 65371c4..f99095a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_draft.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_draft.py new file mode 100644 index 0000000..0f4dd21 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_draft.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py index 9c2059b..1eb2b97 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_report.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_report.py index e258799..6bca261 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_report.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_report.py @@ -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, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py index 7949c85..cf2bd47 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py index cb982ad..1acb950 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py @@ -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') diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py index bc4a849..fddd212 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py index dd73ba3..54b64c8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/direction_progress.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/direction_progress.py new file mode 100644 index 0000000..9276235 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/direction_progress.py @@ -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}') diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py index 6369092..c29b52a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py index 7d94551..c165c9b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_coverage.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_coverage.py index 4e18b2b..3026e2c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_coverage.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_coverage.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_completion.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_completion.py new file mode 100644 index 0000000..4849d9f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_completion.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_evidence.py index 0ab2c91..428a09b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_evidence.py @@ -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): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_quality.py index a56cc6e..692e615 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_quality.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_quality.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_remount.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_remount.py new file mode 100644 index 0000000..bb497f7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_remount.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py index 9b6bc1b..ff781bc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py @@ -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'}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py index fec47cc..9cb4a58 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -11,6 +11,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 秒，已停止。', diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 2618553..905e1b5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -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, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py index 3a2fc15..d1bb45c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py index 5a16c6c..f8936a6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py @@ -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"]) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py index 2c14926..a22462a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py index 64e0569..5406fed 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py @@ -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', diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py index 5a70a6a..681ee17 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py @@ -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") diff --git a/src/linkerhand_calibration/test/fixtures/o30_recorded_motion_initialization.json.gz b/src/linkerhand_calibration/test/fixtures/o30_recorded_motion_initialization.json.gz new file mode 100644 index 0000000..4e65ba3 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_recorded_motion_initialization.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_recorded_root_directions.json.gz b/src/linkerhand_calibration/test/fixtures/o30_recorded_root_directions.json.gz new file mode 100644 index 0000000..394626a Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_recorded_root_directions.json.gz differ diff --git a/src/linkerhand_calibration/test/o30_capture_fixture.py b/src/linkerhand_calibration/test/o30_capture_fixture.py index 4b03891..956065d 100644 --- a/src/linkerhand_calibration/test/o30_capture_fixture.py +++ b/src/linkerhand_calibration/test/o30_capture_fixture.py @@ -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)) diff --git a/src/linkerhand_calibration/test/test_acquisition.py b/src/linkerhand_calibration/test/test_acquisition.py deleted file mode 100644 index fa340d0..0000000 --- a/src/linkerhand_calibration/test/test_acquisition.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_adaptive_command_sampling.py b/src/linkerhand_calibration/test/test_adaptive_command_sampling.py deleted file mode 100644 index dc8194d..0000000 --- a/src/linkerhand_calibration/test/test_adaptive_command_sampling.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_adaptive_training.py b/src/linkerhand_calibration/test/test_adaptive_training.py deleted file mode 100644 index 97bc697..0000000 --- a/src/linkerhand_calibration/test/test_adaptive_training.py +++ /dev/null @@ -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()) diff --git a/src/linkerhand_calibration/test/test_affine_mimic_export.py b/src/linkerhand_calibration/test/test_affine_mimic_export.py deleted file mode 100644 index 994b3c9..0000000 --- a/src/linkerhand_calibration/test/test_affine_mimic_export.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_alignment_view.py b/src/linkerhand_calibration/test/test_alignment_view.py deleted file mode 100644 index 0fa8e82..0000000 --- a/src/linkerhand_calibration/test/test_alignment_view.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_all_model_artifact_pipeline.py b/src/linkerhand_calibration/test/test_all_model_artifact_pipeline.py deleted file mode 100644 index afbe185..0000000 --- a/src/linkerhand_calibration/test/test_all_model_artifact_pipeline.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_all_view_image_acceptance.py b/src/linkerhand_calibration/test/test_all_view_image_acceptance.py deleted file mode 100644 index aabe1f9..0000000 --- a/src/linkerhand_calibration/test/test_all_view_image_acceptance.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_all_view_image_capture.py b/src/linkerhand_calibration/test/test_all_view_image_capture.py deleted file mode 100644 index cc7c8c1..0000000 --- a/src/linkerhand_calibration/test/test_all_view_image_capture.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py deleted file mode 100644 index 366a6c0..0000000 --- a/src/linkerhand_calibration/test/test_architecture.py +++ /dev/null @@ -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}) diff --git a/src/linkerhand_calibration/test/test_artifact_publication.py b/src/linkerhand_calibration/test/test_artifact_publication.py deleted file mode 100644 index c1a733f..0000000 --- a/src/linkerhand_calibration/test/test_artifact_publication.py +++ /dev/null @@ -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('', 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('', 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('') - 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() diff --git a/src/linkerhand_calibration/test/test_axis_line_gauge.py b/src/linkerhand_calibration/test/test_axis_line_gauge.py deleted file mode 100644 index 5be1367..0000000 --- a/src/linkerhand_calibration/test/test_axis_line_gauge.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_bounded_mimic_approximation.py b/src/linkerhand_calibration/test/test_bounded_mimic_approximation.py deleted file mode 100644 index 9889515..0000000 --- a/src/linkerhand_calibration/test/test_bounded_mimic_approximation.py +++ /dev/null @@ -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)) diff --git a/src/linkerhand_calibration/test/test_branch_initialization.py b/src/linkerhand_calibration/test/test_branch_initialization.py deleted file mode 100644 index cfa0a35..0000000 --- a/src/linkerhand_calibration/test/test_branch_initialization.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_branch_worker.py b/src/linkerhand_calibration/test/test_branch_worker.py deleted file mode 100644 index a785f3c..0000000 --- a/src/linkerhand_calibration/test/test_branch_worker.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_cad_image_geometry.py b/src/linkerhand_calibration/test/test_cad_image_geometry.py deleted file mode 100644 index 2120210..0000000 --- a/src/linkerhand_calibration/test/test_cad_image_geometry.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_calibrated_joint_state_bridge.py b/src/linkerhand_calibration/test/test_calibrated_joint_state_bridge.py deleted file mode 100644 index 5ca160b..0000000 --- a/src/linkerhand_calibration/test/test_calibrated_joint_state_bridge.py +++ /dev/null @@ -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" - ) diff --git a/src/linkerhand_calibration/test/test_calibrated_motion_limits.py b/src/linkerhand_calibration/test/test_calibrated_motion_limits.py deleted file mode 100644 index 2e17bc7..0000000 --- a/src/linkerhand_calibration/test/test_calibrated_motion_limits.py +++ /dev/null @@ -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") diff --git a/src/linkerhand_calibration/test/test_calibration_coordinator.py b/src/linkerhand_calibration/test/test_calibration_coordinator.py deleted file mode 100644 index 2b0f0e0..0000000 --- a/src/linkerhand_calibration/test/test_calibration_coordinator.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_camera_preflight.py b/src/linkerhand_calibration/test/test_camera_preflight.py deleted file mode 100644 index 3c06c92..0000000 --- a/src/linkerhand_calibration/test/test_camera_preflight.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_camera_timing.py b/src/linkerhand_calibration/test/test_camera_timing.py deleted file mode 100644 index 1fa8191..0000000 --- a/src/linkerhand_calibration/test/test_camera_timing.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_capture_journal.py b/src/linkerhand_calibration/test/test_capture_journal.py deleted file mode 100644 index e708b60..0000000 --- a/src/linkerhand_calibration/test/test_capture_journal.py +++ /dev/null @@ -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)] diff --git a/src/linkerhand_calibration/test/test_capture_metadata_concurrency.py b/src/linkerhand_calibration/test/test_capture_metadata_concurrency.py deleted file mode 100644 index f87795c..0000000 --- a/src/linkerhand_calibration/test/test_capture_metadata_concurrency.py +++ /dev/null @@ -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} diff --git a/src/linkerhand_calibration/test/test_capture_plan.py b/src/linkerhand_calibration/test/test_capture_plan.py deleted file mode 100644 index c8be5eb..0000000 --- a/src/linkerhand_calibration/test/test_capture_plan.py +++ /dev/null @@ -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)} diff --git a/src/linkerhand_calibration/test/test_capture_provenance.py b/src/linkerhand_calibration/test/test_capture_provenance.py deleted file mode 100644 index d38b066..0000000 --- a/src/linkerhand_calibration/test/test_capture_provenance.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_capture_runtime_index.py b/src/linkerhand_calibration/test/test_capture_runtime_index.py deleted file mode 100644 index 01cb1fe..0000000 --- a/src/linkerhand_calibration/test/test_capture_runtime_index.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_chain_candidate_audit.py b/src/linkerhand_calibration/test/test_chain_candidate_audit.py deleted file mode 100644 index 5436bce..0000000 --- a/src/linkerhand_calibration/test/test_chain_candidate_audit.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_chain_capture.py b/src/linkerhand_calibration/test/test_chain_capture.py deleted file mode 100644 index f8e39b1..0000000 --- a/src/linkerhand_calibration/test/test_chain_capture.py +++ /dev/null @@ -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, ()) diff --git a/src/linkerhand_calibration/test/test_chain_capture_runtime.py b/src/linkerhand_calibration/test/test_chain_capture_runtime.py deleted file mode 100644 index b8a926b..0000000 --- a/src/linkerhand_calibration/test/test_chain_capture_runtime.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_checkpoint_extension.py b/src/linkerhand_calibration/test/test_checkpoint_extension.py deleted file mode 100644 index a81b5cc..0000000 --- a/src/linkerhand_calibration/test/test_checkpoint_extension.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_checkpoint_observer_upgrade.py b/src/linkerhand_calibration/test/test_checkpoint_observer_upgrade.py deleted file mode 100644 index d289b8a..0000000 --- a/src/linkerhand_calibration/test/test_checkpoint_observer_upgrade.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_checkpoint_startup.py b/src/linkerhand_calibration/test/test_checkpoint_startup.py deleted file mode 100644 index 3c3af53..0000000 --- a/src/linkerhand_calibration/test/test_checkpoint_startup.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_command_motion.py b/src/linkerhand_calibration/test/test_command_motion.py deleted file mode 100644 index 8e2417b..0000000 --- a/src/linkerhand_calibration/test/test_command_motion.py +++ /dev/null @@ -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()) diff --git a/src/linkerhand_calibration/test/test_compact_calibration_json.py b/src/linkerhand_calibration/test/test_compact_calibration_json.py deleted file mode 100644 index 74ae63a..0000000 --- a/src/linkerhand_calibration/test/test_compact_calibration_json.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_conditioned_witness.py b/src/linkerhand_calibration/test/test_conditioned_witness.py deleted file mode 100644 index 91c7d04..0000000 --- a/src/linkerhand_calibration/test/test_conditioned_witness.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_config.py b/src/linkerhand_calibration/test/test_config.py deleted file mode 100644 index 040e06a..0000000 --- a/src/linkerhand_calibration/test/test_config.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_core.py b/src/linkerhand_calibration/test/test_core.py deleted file mode 100644 index 056a1ef..0000000 --- a/src/linkerhand_calibration/test/test_core.py +++ /dev/null @@ -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 - ) diff --git a/src/linkerhand_calibration/test/test_declared_byte_direction.py b/src/linkerhand_calibration/test/test_declared_byte_direction.py deleted file mode 100644 index 59da0b5..0000000 --- a/src/linkerhand_calibration/test/test_declared_byte_direction.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_declared_witness_path.py b/src/linkerhand_calibration/test/test_declared_witness_path.py deleted file mode 100644 index 0f40015..0000000 --- a/src/linkerhand_calibration/test/test_declared_witness_path.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_detection_dispatch.py b/src/linkerhand_calibration/test/test_detection_dispatch.py deleted file mode 100644 index 828accc..0000000 --- a/src/linkerhand_calibration/test/test_detection_dispatch.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_diagnostic_analysis.py b/src/linkerhand_calibration/test/test_diagnostic_analysis.py deleted file mode 100644 index b76ce91..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_analysis.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_diagnostic_coordinator.py b/src/linkerhand_calibration/test/test_diagnostic_coordinator.py deleted file mode 100644 index f8f2fe5..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_coordinator.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_diagnostic_observation_scope.py b/src/linkerhand_calibration/test/test_diagnostic_observation_scope.py deleted file mode 100644 index a54e3f5..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_observation_scope.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_diagnostic_presets_cli.py b/src/linkerhand_calibration/test/test_diagnostic_presets_cli.py deleted file mode 100644 index 87f0257..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_presets_cli.py +++ /dev/null @@ -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 diff --git a/src/linkerhand_calibration/test/test_diagnostic_provenance.py b/src/linkerhand_calibration/test/test_diagnostic_provenance.py deleted file mode 100644 index 68fc51e..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_provenance.py +++ /dev/null @@ -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('', 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 diff --git a/src/linkerhand_calibration/test/test_diagnostic_quality.py b/src/linkerhand_calibration/test/test_diagnostic_quality.py deleted file mode 100644 index 9a6579c..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_quality.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_diagnostic_reference_flow.py b/src/linkerhand_calibration/test/test_diagnostic_reference_flow.py deleted file mode 100644 index 7992245..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_reference_flow.py +++ /dev/null @@ -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() diff --git a/src/linkerhand_calibration/test/test_diagnostic_report.py b/src/linkerhand_calibration/test/test_diagnostic_report.py deleted file mode 100644 index 4ed44e2..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_report.py +++ /dev/null @@ -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) diff --git a/src/linkerhand_calibration/test/test_diagnostic_session.py b/src/linkerhand_calibration/test/test_diagnostic_session.py deleted file mode 100644 index 1f7490b..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_session.py +++ /dev/null @@ -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()}) diff --git a/src/linkerhand_calibration/test/test_diagnostic_thumb_cli.py b/src/linkerhand_calibration/test/test_diagnostic_thumb_cli.py deleted file mode 100644 index 06cb29c..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_thumb_cli.py +++ /dev/null @@ -1,246 +0,0 @@ -"""Exercise the short capture through the real CLI and process boundary, offline.""" - -from dataclasses import replace -import importlib.util -import json -from pathlib import Path -from types import SimpleNamespace - -import pytest -import yaml - -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.runtime import runner -from linkerhand_calibration.runtime.ros.parameters import load_runtime_parameters, parameter_defaults -from linkerhand_calibration.runtime.runner_support import launch_command, protected_inputs - - -PACKAGE = Path(__file__).resolve().parents[1] - - -def product(model="o6"): - return load_product_config(PACKAGE / f"config/{model}_right_product.yaml", - workspace=PACKAGE.parents[1], check_can=False) - - -@pytest.mark.parametrize("flag,mode", [ - ("--diagnostic-thumb", "o6_thumb"), ("--diagnostic-yaw-hold", "o6_yaw_hold"), -]) -def test_cli_selects_diagnostic_capture_and_disables_resume(monkeypatch, flag, mode): - calls = [] - config = product() - monkeypatch.setattr(runner, "load_product_config", lambda *args, **kwargs: config) - monkeypatch.setattr(runner, "run_online", lambda *args, **kwargs: calls.append(kwargs) or 0) - with pytest.raises(SystemExit) as result: - runner.main([flag, "--record-bag"]) - assert result.value.code == 0 - assert calls == [{"record_bag": True, "commands_enabled": True, - "allow_resume": False, "diagnostic_capture": mode}] - - -@pytest.mark.parametrize("arguments", [ - ["--offline-raw", "old.jsonl"], ["--offline-output", "output"], - ["--publish-offline"], ["--commands-disabled"], -]) -@pytest.mark.parametrize("flag", ["--diagnostic-thumb", "--diagnostic-yaw-hold"]) -def test_cli_rejects_incompatible_modes_before_loading_devices(monkeypatch, arguments, flag): - monkeypatch.setattr(runner, "load_product_config", lambda *args, **kwargs: pytest.fail("loaded hardware config")) - with pytest.raises(SystemExit) as result: - runner.main([flag, *arguments]) - assert result.value.code == 2 - - -@pytest.mark.parametrize("model", ["l6", "o12", "g20"]) -@pytest.mark.parametrize("flag", ["--diagnostic-thumb", "--diagnostic-yaw-hold"]) -def test_thumb_cli_rejects_other_models_without_starting_stack(monkeypatch, model, flag): - config = product(model) - monkeypatch.setattr(runner, "load_product_config", lambda *args, **kwargs: config) - monkeypatch.setattr(runner, "run_online", lambda *args, **kwargs: pytest.fail("started stack")) - with pytest.raises(SystemExit) as result: - runner.main([flag, "--validate-only"]) - assert result.value.code == 2 - - -@pytest.mark.parametrize("flag,mode", [ - ("--diagnostic-thumb", "o6_thumb"), ("--diagnostic-yaw-hold", "o6_yaw_hold"), -]) -def test_validate_only_exposes_diagnostic_scope_without_starting_stack(monkeypatch, capsys, flag, mode): - monkeypatch.setattr(runner, "run_online", lambda *args, **kwargs: pytest.fail("started stack")) - runner.main(["--config", str(PACKAGE / "config/o6_right_product.yaml"), - "--workspace", str(PACKAGE.parents[1]), flag, "--validate-only"]) - output = capsys.readouterr().out - report, _ = json.JSONDecoder().raw_decode(output) - assert report["diagnostic_capture"]["mode"] == mode - assert report["diagnostic_capture"]["publication_allowed"] is False - if mode == "o6_yaw_hold": - assert report["diagnostic_capture"]["hold_command_values"] == [64., 128., 192.] - assert report["diagnostic_capture"]["hold_seconds"] == 10. - - -def test_cli_diagnostic_presets_are_mutually_exclusive_before_device_loading(monkeypatch): - monkeypatch.setattr(runner, "load_product_config", lambda *a, **kw: pytest.fail("loaded devices")) - with pytest.raises(SystemExit) as result: - runner.main(["--diagnostic-thumb", "--diagnostic-yaw-hold"]) - assert result.value.code == 2 - - -def test_runner_recognizes_diagnostic_completion_as_separate_terminal(): - status = {"state": "DIAGNOSTIC_COMPLETE"} - monitor = SimpleNamespace(status=status) - result = runner.drive_session(monitor, None, spin=lambda: None, ok=lambda: True, - request_start=lambda: pytest.fail("unexpected start")) - assert result == status - - -@pytest.mark.parametrize("terminal,report_change,expected,analysis_outcome", [ - ("DIAGNOSTIC_COMPLETE", {}, 0, "complete"), - ("DIAGNOSTIC_COMPLETE", {}, 0, "failed"), - ("DIAGNOSTIC_COMPLETE", {}, 130, "cancelled"), - ("DIAGNOSTIC_COMPLETE", {"missing_joint_zero_references": ["rh_thumb_ip"], - "accepted_pose_quality_incomplete_attempts": 2}, 0, "complete"), - ("DIAGNOSTIC_COMPLETE", {"mode": "other"}, 2, "complete"), - ("DIAGNOSTIC_COMPLETE", {"publication_allowed": True}, 2, "complete"), - ("DIAGNOSTIC_COMPLETE", {"publication_allowed": 0}, 2, "complete"), - ("DIAGNOSTIC_COMPLETE", {"result": "COMPLETE"}, 2, "complete"), - ("DIAGNOSTIC_COMPLETE", None, 2, "complete"), - ("COMPLETE", {}, 3, "complete"), - ("PAUSED", {}, 3, "complete"), -]) -@pytest.mark.parametrize("diagnostic_mode", ["o6_thumb", "o6_yaw_hold"]) -def test_online_diagnostic_never_enters_artifact_completion( - monkeypatch, tmp_path, capsys, terminal, report_change, expected, analysis_outcome, diagnostic_mode, -): - import rclpy - import linkerhand_calibration.hikrobot_camera as camera - from linkerhand_calibration.runtime import diagnostic_analysis, resume, runner_support - from linkerhand_calibration.runtime.artifacts import completion - - config = replace(product(), output_root=tmp_path) - session = tmp_path / "session" - session.mkdir() - if report_change is not None: - report = {"mode": diagnostic_mode, "result": "DIAGNOSTIC_COMPLETE", - "publication_allowed": False, **report_change} - (session / "diagnostic_capture.json").write_text(json.dumps(report)) - now = [0.0] - launched = [] - stopped = [] - analyzed = [] - monitor = SimpleNamespace( - count_publishers=lambda topic: 0, - status_topic="/offline/status", - destroy_node=lambda: stopped.append("monitor"), - ) - monkeypatch.setattr(rclpy, "ok", lambda: True) - monkeypatch.setattr(rclpy, "spin_once", lambda *args, **kwargs: None) - - def clock(): - now[0] += 1.0 - return now[0] - - monkeypatch.setattr(runner.time, "monotonic", clock) - monkeypatch.setattr(runner_support, "create_session_directory", lambda root: session) - monkeypatch.setattr(runner_support, "CalibrationMonitor", lambda **kwargs: monitor) - monkeypatch.setattr(runner_support, "stop_stack", lambda process: stopped.append("stack")) - def analyze_after_shutdown(profile, directory): - assert stopped == ["stack"] - assert directory == session - analyzed.append(profile.key.profile_id) - if analysis_outcome == "cancelled": - raise KeyboardInterrupt - return {"status": analysis_outcome, "accuracy_validated": False, "publication_allowed": False} - monkeypatch.setattr(diagnostic_analysis, "finish_diagnostic_analysis", analyze_after_shutdown) - monkeypatch.setattr(camera, "configure_fastdds_large_image_transport", lambda: None) - monkeypatch.setattr(resume, "discover_resume_candidate", - lambda *args, **kwargs: pytest.fail("diagnostic reused old samples")) - monkeypatch.setattr(completion, "finish_online_artifacts", - lambda *args, **kwargs: pytest.fail("diagnostic attempted publication")) - monkeypatch.setattr(runner.subprocess, "Popen", lambda command, **kwargs: launched.append(command) or object()) - monkeypatch.setattr(runner, "drive_session", lambda *args, **kwargs: {"state": terminal}) - - assert runner.run_online(config, diagnostic_capture=diagnostic_mode) == expected - assert len(launched) == 1 - assert f"diagnostic_capture:={diagnostic_mode}" in launched[0] - assert not any(arg.startswith("resume_raw_samples_path:=") for arg in launched[0]) - assert stopped == ["stack", "monitor"] - assert analyzed == ([config.profile_key.profile_id] if expected in {0, 130} else []) - if expected in {0, 130}: - output = capsys.readouterr().out - assert "诊断数据已采完" in output - assert "本次未进行精度验收，未发布 JSON 或修正 URDF" in output - if diagnostic_mode == "o6_yaw_hold": - assert "拇指yaw方向差保持诊断" in output - if report_change.get("missing_joint_zero_references"): - assert "零位未确认，仅保留原始观测：rh_thumb_ip" in output - assert "未生成这些关节的零位或角度样本" in output - assert "2 次方向采集的可信姿态覆盖不足" in output - if expected == 130: - assert "诊断采集已完成；已取消后续分析" in output - assert "已请求中止" not in output - assert not (session / "runner_diagnostic.json").exists() - else: - assert (session / "runner_diagnostic.json").is_file() - - -def runtime_values(config, tmp_path): - profile = config.calibration_contract.typed_profile - declared = next(iter(yaml.safe_load(config.calibration_config.read_text()).values()))["ros__parameters"] - return {**parameter_defaults(profile), **declared, - "serial_number": "OFFLINE_DIAGNOSTIC", "session_dir": str(tmp_path), - "source_urdf_path": str(config.source_urdf), - "camera_extrinsics_file": str(config.camera_extrinsics), - **{key.replace("_sha256", "_expected_sha256"): value - for key, value in protected_inputs(config).items()}} - - -@pytest.mark.parametrize("mode", ["o6_thumb", "o6_yaw_hold"]) -def test_ros_parameters_load_explicit_diagnostic_mode(tmp_path, mode): - config = product() - values = runtime_values(config, tmp_path) - assert values["diagnostic_capture"] == "" - values["diagnostic_capture"] = mode - resolved = load_runtime_parameters(config.calibration_contract.typed_profile, values.__getitem__) - assert resolved.diagnostic_capture == mode - assert resolved.resume_raw_samples_path is None - - -@pytest.mark.parametrize("extra", [ - {"commands_enabled": False}, {"resume_raw_samples_path": "old.jsonl"}, - {"diagnostic_capture": "unknown"}, -]) -@pytest.mark.parametrize("mode", ["o6_thumb", "o6_yaw_hold"]) -def test_direct_ros_entry_cannot_bypass_diagnostic_constraints(tmp_path, extra, mode): - config = product() - values = {**runtime_values(config, tmp_path), "diagnostic_capture": mode, **extra} - with pytest.raises(ValueError): - load_runtime_parameters(config.calibration_contract.typed_profile, values.__getitem__) - - -@pytest.mark.parametrize("mode", ["o6_thumb", "o6_yaw_hold"]) -def test_launch_forwards_diagnostic_string_to_owner_and_suppresses_resume(tmp_path, monkeypatch, mode): - from launch import LaunchContext - from launch.actions import DeclareLaunchArgument - from launch_ros.actions import Node - from launch_ros.utilities import evaluate_parameters - from ros2launch.api.api import parse_launch_arguments - - monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path / "ros_logs")) - config = product() - arguments = dict(parse_launch_arguments(launch_command(config, tmp_path / "session", - record_bag=False, commands_enabled=True, resume_from=tmp_path / "old", - diagnostic_capture=mode)[4:])) - assert "resume_raw_samples_path" not in arguments - assert arguments["diagnostic_capture"] == mode - spec = importlib.util.spec_from_file_location("diagnostic_launch", - PACKAGE / "launch/three_camera_calibration.launch.py") - module = importlib.util.module_from_spec(spec) - spec.loader.exec_module(module) - context = LaunchContext() - context.launch_configurations.update(arguments) - for action in module.generate_launch_description().entities: - if isinstance(action, DeclareLaunchArgument): - action.execute(context) - nodes = [action for action in module._launch_stack(context) if isinstance(action, Node)] - owner = next(node for node in nodes if node.node_executable == "three_camera_calibration_node") - resolved = evaluate_parameters(context, owner._Node__parameters) - assert any(isinstance(item, dict) and item.get("diagnostic_capture") == mode for item in resolved) diff --git a/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_coordinator.py b/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_coordinator.py deleted file mode 100644 index a9bb1fc..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_coordinator.py +++ /dev/null @@ -1,139 +0,0 @@ -"""The real coordinator holds six declared poses without publishing artifacts. - -Projected image inputs exercise the public geometry/capture path. This test -checks schedule duration, raw observation gates and publication rules; its -synthetic Tags do not constitute a hardware accuracy certificate. -""" - -from collections import defaultdict -import hashlib -import json - -import numpy as np -import pytest - -from linkerhand_calibration.acquisition import StateSample -from linkerhand_calibration.runtime.diagnostic_capture import diagnostic_hold_timing -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from runtime_host_fixture import coordinator_fixture, ready -from test_diagnostic_coordinator import advance, assert_terminal_is_idle, TERMINAL_PHASES - - -def test_hold_timing_uses_only_this_segment_commands_at_or_before_image(): - target = (255., 192.) - history = [StateSample(90, target), StateSample(100, (255., 220.)), - StateSample(115, target), StateSample(120, target), StateSample(140, target)] - early = diagnostic_hold_timing(history, target=target, image_stamp_ns=110, - session_epoch=2, motion_version=7, motion_started_ns=100, feedback_stable_since_ns=None) - assert early == {"hold_id": "2:7", "target_first_command_stamp_ns": None, - "feedback_stable_since_ns": None} - current = diagnostic_hold_timing(history, target=target, image_stamp_ns=125, - session_epoch=2, motion_version=7, motion_started_ns=100, feedback_stable_since_ns=120) - assert current["target_first_command_stamp_ns"] == 115 - assert current["feedback_stable_since_ns"] == 120 - next_segment = diagnostic_hold_timing(history, target=target, image_stamp_ns=145, - session_epoch=2, motion_version=8, motion_started_ns=130, feedback_stable_since_ns=None) - assert next_segment["hold_id"] == "2:8" and next_segment["target_first_command_stamp_ns"] == 140 - - -@pytest.mark.parametrize("stable_stamp", [90, 126]) -def test_hold_timing_does_not_import_stability_from_previous_segment_or_future(stable_stamp): - result = diagnostic_hold_timing([StateSample(110, (255., 192.))], target=(255., 192.), - image_stamp_ns=125, session_epoch=2, motion_version=7, motion_started_ns=100, - feedback_stable_since_ns=stable_stamp) - assert result["target_first_command_stamp_ns"] == 110 - assert result["feedback_stable_since_ns"] is None - - -def test_yaw_hold_completes_six_full_holds_and_keeps_pre_stable_raw_images(tmp_path, monkeypatch): - host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_yaw_hold") - ready(host, clock) - assert set(host.required_observation_views) == {"top"} - 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 - intervals, order, cursor, checked_three = {}, [], 0, set() - accepted = defaultdict(int) - previous = None - try: - for _ in range(9000): - if host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE: - break - if host.execution.session.phase in TERMINAL_PHASES: - pytest.fail(f"premature diagnostic exit: {host.state}: {host.reason}") - advance(host, clock) - motion = host._motion - key = None - if motion is not None and motion.phase == "steady": - key = (motion.direction, motion.steady_index, motion.target[motion.command_index]) - if key not in intervals: - order.append(key) - intervals[key] = {"start": clock.now, "target_first": None, "end": None} - if tuple(host.last_command) == motion.target and intervals[key]["target_first"] is None: - intervals[key]["target_first"] = clock.now - if previous is not None and previous != key: - intervals[previous]["end"] = clock.now - previous = key - for row in host.raw_records[cursor:]: - if row.get("kind") != "joint_sample" or row.get("sample_phase") != "steady": - continue - sample_key = (row["direction"], row["steady_index"], row["steady_target"]) - accepted[sample_key] += 1 - if accepted[sample_key] == 3: - # Three good frames must not terminate a ten-second hold. - assert key == sample_key - assert host.segment is not None and not host.segment.minimum_hold_complete(clock.now) - checked_three.add(sample_key) - cursor = len(host.raw_records) - else: - pytest.fail(f"virtual yaw-hold capture timed out: {host.state}: {host.reason}") - assert host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE - expected = [("decreasing", i, value) for i, value in enumerate((192., 128., 64.))] - expected += [("increasing", i, value) for i, value in enumerate((64., 128., 192.))] - assert order == expected - assert checked_three == set(expected) - for key in expected: - interval = intervals[key] - assert interval["target_first"] is not None and interval["end"] is not None - assert interval["end"] - interval["target_first"] >= 10. - 1e-9 - assert accepted[key] > 3 - - holds = defaultdict(list) - for row in host.raw_records: - if row.get("kind") == "diagnostic_observation_frame" and row.get("sample_phase") == "steady": - holds[(row["direction"], row["steady_index"], row["steady_target"])].append(row) - assert set(holds) == set(expected) - hold_ids, versions = set(), set() - for key, rows in holds.items(): - assert {row["minimum_hold_seconds"] for row in rows} == {10.} - assert any(not row["accepted_sample_window_open"] for row in rows) - assert any(row["accepted_sample_window_open"] for row in rows) - assert all(row["is_accepted_joint_sample"] is False for row in rows) - assert all(row["command_vector"] is not None and row["feedback_vector"] is not None for row in rows) - assert len({row["hold_id"] for row in rows}) == 1 - assert len({row["session_epoch"] for row in rows}) == 1 - assert len({row["motion_version"] for row in rows}) == 1 - assert len({row["motion_started_ns"] for row in rows}) == 1 - hold_ids.add(rows[0]["hold_id"]) - versions.add(rows[0]["motion_version"]) - assert all(row["image_stamp_ns"] >= row["motion_started_ns"] for row in rows) - target_stamps = {row["target_first_command_stamp_ns"] for row in rows - if row["target_first_command_stamp_ns"] is not None} - assert len(target_stamps) == 1 - assert max(row["image_stamp_ns"] for row in rows) - next(iter(target_stamps)) >= 9.9e9 - assert any(row["feedback_stable_since_ns"] is None for row in rows) - assert any(row["feedback_stable_since_ns"] is not None for row in rows) - assert len(hold_ids) == len(versions) == 6 - references = [row for row in host.raw_records if row.get("kind") == "joint_zero_reference"] - assert [row["joint"] for row in references] == ["rh_thumb_cmc_yaw"] - assert len(host.execution.completed_units) == 2 - assert not host.finalization.started and not host.final_json and not host.final_urdf - assert not list(tmp_path.rglob("*.urdf")) - assert all(tuple(command[i] for i in (0, 2, 3, 4, 5)) == (255.,) * 5 for command in clock.positions) - report = json.loads((host.parameters.session_dir / "diagnostic_capture.json").read_text()) - assert not report["publication_allowed"] and not report["accuracy_validated"] - assert report["raw_samples_sha256"] == hashlib.sha256(host.raw_path.read_bytes()).hexdigest() - assert_terminal_is_idle(host, clock, monkeypatch) - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_plan.py b/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_plan.py deleted file mode 100644 index 25077c1..0000000 --- a/src/linkerhand_calibration/test/test_diagnostic_yaw_hold_plan.py +++ /dev/null @@ -1,59 +0,0 @@ -"""The directional-hold preset has bounded motion and diagnostic-only scope.""" - -from dataclasses import replace - -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.observation_scope import required_observation_views -from linkerhand_calibration.runtime.status import render_status_zh - - -def test_yaw_hold_scope_and_legacy_serialization(): - profile = load_bundled_hand_profile("o6_right_8") - plan = resolve_diagnostic_capture(profile, "o6_yaw_hold") - assert plan.as_dict() == {"mode": "o6_yaw_hold", "task_keys": ("thumb_yaw_top",), - "cycles": (0,), "hold_command_values": (64., 128., 192.), "hold_seconds": 10.} - assert required_observation_views(profile, plan) == ("top",) - legacy = resolve_diagnostic_capture(profile, "o6_thumb") - assert legacy.hold_command_values == () and legacy.hold_seconds == 0. - assert legacy.as_dict() == {"mode": "o6_thumb", "cycles": (0,), - "task_keys": ("thumb_yaw_top", "thumb_pitch_ip_front")} - text = render_status_zh({"phase": "steady", "diagnostic_capture": plan.as_dict()}) - assert "拇指yaw方向差保持诊断" in text - assert "方向差保持采集（每点至少 10 秒）" in text - assert "不发布标定 JSON/URDF" in text - complete = render_status_zh({"phase": "DIAGNOSTIC_COMPLETE", "diagnostic_capture": plan.as_dict()}) - assert "诊断采集完成（未进行精度验收）" in complete - - -@pytest.mark.parametrize("values,seconds", [ - ((64., 128.), 0.), ((64.,), 10.01), ((64.,), float("nan")), - ((), 10.), ((64., float("inf")), 10.), ((64., 64.), 10.), - ((128., 64.), 10.), (("64",), 10.), -]) -def test_invalid_hold_contract_is_rejected(values, seconds): - with pytest.raises(ValueError, match="diagnostic_hold_"): - DiagnosticCapturePlan("invalid", ("task",), hold_command_values=values, hold_seconds=seconds) - - -@pytest.mark.parametrize("change", ["scan_endpoint", "channel_envelope", "joint_binding"]) -def test_yaw_hold_refuses_changed_profile_motion_contract(change): - profile = load_bundled_hand_profile("o6_right_8") - task = next(task for task in profile.motion.tasks if task.key == "thumb_yaw_top") - if change == "scan_endpoint": - tasks = tuple(replace(item, end_u8=64) if item == task else item for item in profile.motion.tasks) - profile = replace(profile, motion=replace(profile.motion, tasks=tasks)) - elif change == "channel_envelope": - upper = list(profile.command.maximum_values) - upper[task.command_index] = 192. - profile = replace(profile, command=replace(profile.command, upper_bounds=tuple(upper))) - else: - bindings = dict(profile.command.command_index_by_joint) - bindings["rh_thumb_cmc_yaw"] = (task.command_index + 1) % profile.command.command_count - profile = replace(profile, command=replace(profile.command, command_index_by_joint=bindings)) - with pytest.raises(ValueError, match="diagnostic_(hold_outside_task_domain|thumb_task_binding_changed)"): - resolve_diagnostic_capture(profile, "o6_yaw_hold") diff --git a/src/linkerhand_calibration/test/test_diagnostics.py b/src/linkerhand_calibration/test/test_diagnostics.py deleted file mode 100644 index 8f46766..0000000 --- a/src/linkerhand_calibration/test/test_diagnostics.py +++ /dev/null @@ -1,105 +0,0 @@ -from linkerhand_calibration.acquisition import TagQuality -from linkerhand_calibration.diagnostics import ( - build_tag_quality_diagnostics, - render_status_text_zh, - status_guidance_zh, -) - - -def _tag_config() -> dict[str, dict]: - return { - role: {"id": tag_id, "frame": role, "size_m": 0.01} - for role, tag_id in zip(("t0", "t3", "t4", "t5"), range(4)) - } - - -def test_invalid_t0_edge_has_specific_chinese_guidance() -> None: - qualities = { - role: TagQuality( - hamming=0, - decision_margin=100.0, - edge_pixels=29.4 if role == "t0" else 34.0, - ) - for role in _tag_config() - } - diagnostics = build_tag_quality_diagnostics( - _tag_config(), - qualities, - {"t0": "tag_quality_invalid"}, - {}, - maximum_hamming=0, - minimum_decision_margin=30.0, - minimum_edge_pixels=30.0, - maximum_reprojection_error_px=1.5, - ) - - assert diagnostics["t0"]["individual_valid"] is False - assert diagnostics["t0"]["edge_pixels"] == 29.4 - assert "边长29.4px" in diagnostics["t0"]["summary_zh"] - assert diagnostics["t3"]["individual_valid"] is True - - reason_zh, action_zh = status_guidance_zh( - "PAUSED", - "point_capture_failed:invalid_tag_frame", - diagnostics, - ) - assert "掌心T0" in reason_zh - assert "边长29.4px" in reason_zh - assert "相机稍微靠近" in action_zh - assert "resume" in action_zh - text = render_status_text_zh( - "标定已暂停", - reason_zh, - action_zh, - diagnostics, - ) - assert "状态：标定已暂停" in text - assert "掌心T0(ID 0)：异常，边长29.4px" in text - assert "拇指根部T3(ID 1)：正常" in text - - -def test_missing_tag_is_reported_without_manual_topic_parsing() -> None: - diagnostics = build_tag_quality_diagnostics( - _tag_config(), - {}, - {"t5": "tag_not_detected"}, - {}, - maximum_hamming=0, - minimum_decision_margin=30.0, - minimum_edge_pixels=30.0, - maximum_reprojection_error_px=1.5, - ) - - assert diagnostics["t5"]["detected"] is False - assert "未检测到" in diagnostics["t5"]["summary_zh"] - reason_zh, action_zh = status_guidance_zh( - "PAUSED", - "point_capture_failed:invalid_tag_frame", - diagnostics, - ) - assert "拇指末节T5" in reason_zh - assert "四张标签同时可见" in action_zh - - -def test_low_detection_frequency_has_direct_chinese_action() -> None: - reason_zh, action_zh = status_guidance_zh( - "PREFLIGHT", - "detection_hz_too_low:11.29", - {}, - ) - - assert "11.29Hz" in reason_zh - assert "额外订阅" in action_zh - - -def test_synchronised_timeout_explains_resume_not_start() -> None: - reason_zh, action_zh = status_guidance_zh( - "PAUSED", - "continuous_sweep_failed:synchronised_tag_state_timeout", - {}, - ) - - assert "运动过程中连续3秒" in reason_zh - assert "当前画面恢复正常" in reason_zh - assert "resume" in action_zh - assert "不要调用start" in action_zh diff --git a/src/linkerhand_calibration/test/test_directional_command_mapping.py b/src/linkerhand_calibration/test/test_directional_command_mapping.py deleted file mode 100644 index 0a81708..0000000 --- a/src/linkerhand_calibration/test/test_directional_command_mapping.py +++ /dev/null @@ -1,156 +0,0 @@ -"""Recorded major-loop hysteresis cannot be hidden by a single command table.""" -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.core.domain.reference import read_joint_zero_references -from linkerhand_calibration.core.domain.result import CalibrationResult, JointMapping -from linkerhand_calibration.core.fitting.observed_motion import fit_native_observed_rotation -from linkerhand_calibration.core.fitting.command_mapping import fit_command_mappings -from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping -from linkerhand_calibration.core.urdf.measured_acceptance import validate_measured_joint_holdout -from linkerhand_calibration.runtime.acquisition import accepted_joint_records -from linkerhand_calibration.runtime.artifacts.direction import next_input_directions -from linkerhand_calibration.runtime.artifacts.reader import UnifiedCommandMapper -from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import serialize_report, from_report - -JOINT = "rh_thumb_cmc_yaw" - - -@pytest.fixture(scope="module") -def recorded(): - payload = json.loads(gzip.decompress((Path(__file__).parent / "fixtures" / - "o6_yaw_directional_114156.json.gz").read_bytes())) - profile = load_bundled_hand_profile("o6_right_8") - # This recording used nine training nodes; later profiles may sample more. - profile = replace(profile, acquisition=replace(profile.acquisition, steady_training_nodes=9, - command_capture_mode="interleaved", steady_extra_training_nodes={})) - profile = replace(profile, measurement=replace(profile.measurement, - measurements={JOINT: profile.measurement.measurements[JOINT]}, - transferred_motion_sources={}, cross_view_sources={})) - rows = payload["records"] - reference = read_joint_zero_references(profile, rows)[JOINT] - sweep = accepted_joint_records(profile, rows)[JOINT] - steady = accepted_joint_records(profile, rows, sample_phase="steady") - curve, errors = fit_native_observed_rotation(sweep, input_domain="feedback_u8", - input_to_joint_direction=-1, reference_xyzw=reference.quaternion("top")) - mapping = JointMapping(JOINT, 1, "feedback_u8", tuple(curve.circle["input_knots"]), - curve.angle_rad, curve.increasing_rad, curve.decreasing_rad) - fit = CalibrationResult({JOINT: curve}, {JOINT: mapping}, {JOINT: 0.}, - {JOINT: "test_only_geometry"}, {}, {JOINT: errors}, None, - reference_inputs={JOINT: reference.input_reference("top", feedback=True)}, - joint_zero_references={JOINT: reference}) - return profile, fit, steady - - -def directional_payload(recorded): - profile, fit, steady = recorded - fitted = fit_command_mappings(profile, fit, steady) - prepared = SimpleNamespace(mappings=fitted.output_mappings, plan=SimpleNamespace(mimic_output={})) - report = serialize_report(profile, fitted, prepared, serial_number="TEST_ONLY", - protected_inputs={}) - return from_report(report), fitted, report - - -def test_recorded_single_table_fails_both_serialized_directional_branches_pass(recorded): - profile, fit, steady = recorded - single = replace(profile, artifacts=replace(profile.artifacts, directional_command_joints=frozenset())) - with pytest.raises(ValueError, match="steady_command_holdout_failed"): - fit_command_mappings(single, fit, steady) - payload, fitted, _ = directional_payload(recorded) - validation = [row for row in steady[JOINT] if row["cycle"] == 3] - metrics = validate_measured_joint_holdout(payload, fit.curves, validation) - for direction, value in metrics[JOINT].items(): - assert value["mae_deg"] < .3 - assert value["p95_deg"] < 2 - assert value["maximum_deg"] < 3 - mapping = SerializedJointMapping(payload) - baseline = profile.command.baseline_values - for direction in ("", "increasing", "decreasing"): - assert mapping.evaluate(baseline, direction, active_joints=(JOINT,))[JOINT] == 0 - values = list(baseline); values[1] = 128 - with pytest.raises(ValueError, match="requires_direction"): - mapping.evaluate(values, "", active_joints=(JOINT,)) - angles = [mapping.evaluate(values, d, active_joints=(JOINT,))[JOINT] - for d in ("increasing", "decreasing")] - assert np.degrees(abs(angles[0]-angles[1])) > 2 - assert fitted.command_applicability[JOINT]["interior_reversals_validated"] is False - - -def test_bad_branch_or_direction_declaration_cannot_pass(recorded): - payload, fit, _ = directional_payload(recorded) - bad = deepcopy(payload) - bad["joints"][JOINT]["increasing_rad"] = bad["joints"][JOINT]["decreasing_rad"] - with pytest.raises(ValueError, match="serialized_measured_mapping_inaccurate"): - validate_measured_joint_holdout(bad, fit.curves, - [r for r in recorded[2][JOINT] if r["cycle"] == 3]) - for mutation in ("missing", "undeclared", "unknown"): - bad = deepcopy(payload) - if mutation == "missing": del bad["joints"][JOINT]["decreasing_rad"] - if mutation == "undeclared": del bad["directional_command_joints"] - if mutation == "unknown": bad["directional_command_joints"].append("unknown") - with pytest.raises(ValueError): SerializedJointMapping(bad) - - -def test_runtime_direction_initialization_hold_endpoint_turn_and_interior_reversal(): - channels = {0: (255., 0., 255.)} - advance = lambda value, previous, direction: next_input_directions([value], - None if previous is None else [previous], [direction], resolution=.5, channels=channels) - with pytest.raises(ValueError, match="baseline_initialization"): advance(128, None, "") - assert advance(255, None, "") == ["increasing"] - assert advance(128, 255, "increasing") == ["decreasing"] - assert advance(128, 128, "decreasing") == ["decreasing"] - assert advance(128, 0, "decreasing") == ["increasing"] - with pytest.raises(ValueError, match="interior_reversal"): advance(140, 128, "decreasing") - - -def test_reader_uses_recorded_branches_and_commits_state_only_on_success(recorded, tmp_path): - payload, _, report = directional_payload(recorded) - urdf = tmp_path / "reader_only.urdf" - urdf.write_text(f''' - - - - - ''') - mapper = UnifiedCommandMapper(payload, urdf, report=report) - values = list(recorded[0].command.baseline_values) - values[1] = 128 - with pytest.raises(ValueError, match="baseline_initialization"): - mapper.map_positions(values) - assert mapper.previous is None - values[1] = 255 - assert mapper.map_positions(values) == (0.,) - values[1] = 254.6 - assert mapper.map_positions(values) == (0.,) - values[1] = 254.2 - assert mapper.map_positions(values) == (payload["joints"][JOINT]["decreasing_rad"][254],) - assert mapper.directions[1] == "decreasing" - values[1] = 128 - descending = mapper.map_positions(values) - assert descending == (payload["joints"][JOINT]["decreasing_rad"][128],) - assert mapper.map_positions(values) == descending - values[1] = 128.4 - assert mapper.map_positions(values) == descending - values[1] = 128.8 - with pytest.raises(ValueError, match="interior_reversal"): - mapper.map_positions(values) - assert mapper.previous[1] == 128.4 - values[1] = 140 - with pytest.raises(ValueError, match="interior_reversal"): - mapper.map_positions(values) - assert mapper.previous[1] == 128.4 - assert mapper.directions[1] == "decreasing" - values[1] = 0 - mapper.map_positions(values) - values[1] = 128 - ascending = mapper.map_positions(values) - assert ascending == (payload["joints"][JOINT]["increasing_rad"][128],) - assert np.degrees(abs(ascending[0]-descending[0])) > 2 diff --git a/src/linkerhand_calibration/test/test_dual_mapping.py b/src/linkerhand_calibration/test/test_dual_mapping.py deleted file mode 100644 index bbdbf0d..0000000 --- a/src/linkerhand_calibration/test/test_dual_mapping.py +++ /dev/null @@ -1,134 +0,0 @@ -"""Command calibration uses visual truth, never feedback relabelled as command.""" - -from dataclasses import replace -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.domain.measurement import JointCurveFit -from linkerhand_calibration.core.domain.result import CalibrationResult, JointMapping -from linkerhand_calibration.core.fitting.command_mapping import fit_command_mappings -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.steady import steady_targets -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.core.domain.reference import build_joint_zero_reference -from test_joint_zero_runtime import reference_rows - - -def independent_steady_fixture(layout): - profile = load_bundled_hand_profile(layout) - # Explicit legacy partitions: the negative cases alter cycles 1 and 3. - profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="interleaved")) - task = profile.motion.tasks[0] - name = task.joints[0] - channel = task.command_index - lo, hi = sorted((task.start_value, task.end_value)) - native = profile.command.unit - # Deliberately different coordinate systems. The synthetic camera observes - # actual q(command); the SDK returns feedback = 0.8*command+offset. - offset = 10.0 if native == "u8" else .02 - scale = -.002 if native == "u8" else .7*profile.command.joint_directions[channel] - baseline = profile.command.baseline_values[channel] - q = lambda u: scale*(u-baseline) - knots = (.8*lo+offset, .8*hi+offset) - angles = (q(lo), q(hi)) - curve = JointCurveFit(angles, angles, angles, - {"space": "relative_rotation_3d", "reference_quaternion_xyzw": [0, 0, 0, 1], - "axis_xyz": [0, 0, 1], "input_knots": knots}, 0., 0., {}) - feedback = JointMapping(name, channel, f"feedback_{native}", knots, angles, angles, angles) - zero_rows = reference_rows(profile, name) - for row in zero_rows: - state = list(row[f"state_{native}"]) - state[channel] = .8*baseline+offset - row[f"state_{native}"] = state - reference = build_joint_zero_reference(profile, name, zero_rows, session_epoch=1, motion_version=1) - fit = CalibrationResult({name: curve}, {name: feedback}, {name: 0.}, {}, {}, {}, None, - reference_inputs={name: .8*baseline+offset}, joint_zero_references={name: reference}) - rows = [] - for unit in CalibrationEngine(profile).scan_units(): - if unit.task_key != task.key: - continue - for index, command in enumerate(steady_targets(profile, unit)): - for frame in range(3): - vector = list(profile.command.baseline_values) - for i, v in task.auxiliary_commands: - vector[i] = v - vector[channel] = command - rows.append({"joint": name, "task_name": task.key, "view": task.view, - "sample_id": f"{unit.cycle}:{unit.direction}:{index}:{frame}", - "sample_phase": "steady", "steady_target": command, "steady_index": index, - "cycle": unit.cycle, "direction": unit.direction, - f"command_{native}": command, f"feedback_{native}": .8*command+offset, - f"command_vector_{native}": vector, - "relative_quaternion_xyzw": Rotation.from_rotvec([0, 0, q(command)]).as_quat()}) - return profile, fit, name, rows, q - - -@pytest.mark.parametrize("layout", ["l6_right_8", "o12_right_16"]) -def test_command_map_is_measured_not_feedback_relabelled(layout): - profile, fit, name, rows, q = independent_steady_fixture(layout) - result = fit_command_mappings(profile, fit, {name: rows}) - mapping = result.command_mappings[name] - for command in np.linspace(mapping.knots[0], mapping.knots[-1], 30): - if profile.command.unit == "u8": - command = round(command) - assert mapping.evaluate(command, "increasing") == pytest.approx(q(command), abs=1e-9) - assert result.output_mappings[name] is fit.output_mappings[name] - assert result.command_mappings[name].knots != result.output_mappings[name].knots - - -def test_holdout_cannot_update_a_command_fit_or_tag_mount(): - profile, fit, name, rows, _ = independent_steady_fixture("o12_right_16") - for row in rows: - if row["cycle"] == 3: - 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: rows}) - assert not fit.command_mappings - - -def test_missing_steady_nodes_and_changed_auxiliary_are_not_silently_interpolated(): - profile, fit, name, rows, _ = independent_steady_fixture("o12_right_16") - with pytest.raises(ValueError, match="steady_training_incomplete"): - fit_command_mappings(profile, fit, {name: [r for r in rows if r["cycle"] != 1]}) - rows[0]["command_vector_rad"][1] = -.1 - with pytest.raises(ValueError, match="scan_changed_multiple"): - fit_command_mappings(profile, fit, {name: rows}) - - -def test_steady_motion_is_bounded_and_stability_is_not_tracking_equality(): - profile = load_bundled_hand_profile("o12_right_16") - initial = profile.command.baseline_values - target = list(initial); target[0] = .1 - segment = MotionExecution(profile, MotionCommand("steady", tuple(target), .1, - command_index=0, direction="increasing", steady_index=1), initial_command=initial, - initial_feedback=initial, now=0, identity="steady") - ready = False - for index in range(500): - now = index*.02 - command = segment.sample(now) - feedback = list(command); feedback[0] -= .02 - segment.observe(feedback, stamp=index, now=now) - ready |= segment.steady_ready(now) - if segment.arrived(now): - break - assert ready and segment.arrived(now) - assert now-segment.finished_at < 2.03 - - -def test_scan_and_steady_images_have_separate_input_semantics(): - from linkerhand_calibration.runtime.acquisition import accepted_joint_records - profile = load_bundled_hand_profile("l6_right_8") - task = profile.motion.tasks[0] - row = {"joint": task.joints[0], "task_name": task.key, "cycle": 0, "direction": "increasing", - "view": task.view, "image_stamp_ns": 1, "relative_quaternion_xyzw": [0, 0, 0, 1], - "command_u8": 10., "feedback_u8": 1.2} - original = dict(row) - result = accepted_joint_records(profile, [row])[task.joints[0]][0] - assert result["command_u8"] == 10. and result["feedback_u8"] == 1.2 and row == original - # A partially captured new retry must not borrow the old steady attempt. - steady = dict(row, sample_phase="steady", attempt=1) - retry = dict(row, attempt=2, image_stamp_ns=2) - assert not accepted_joint_records(profile, [steady, retry], sample_phase="steady")[task.joints[0]] diff --git a/src/linkerhand_calibration/test/test_extrinsics_node.py b/src/linkerhand_calibration/test/test_extrinsics_node.py deleted file mode 100644 index ef7eae8..0000000 --- a/src/linkerhand_calibration/test/test_extrinsics_node.py +++ /dev/null @@ -1,326 +0,0 @@ -"""Focused tests for checkerboard frame pairing.""" - -import cv2 -import numpy as np -from scipy.spatial.transform import Rotation - -import linkerhand_calibration.extrinsics_node as extrinsics_node -from linkerhand_calibration.extrinsics_node import ( - BoardPose, - StereoCapture, - _fit_stereo_robust, - _individual_reprojection_passes, - _minimum_history_skew_ns, - _pose_is_novel, - _select_latest_synchronised_pair, - _summarize_transform_repeatability, -) - - -def _pose(stamp_ns: int) -> BoardPose: - return BoardPose( - stamp_ns=stamp_ns, - camera_from_board_candidates=(), - reprojection_rms_px=0.1, - ) - - -def test_pairing_uses_recent_history_instead_of_mismatched_latest_frames(): - front = [_pose(1_000_000_000), _pose(1_200_000_000)] - side = [_pose(1_030_000_000), _pose(1_370_000_000)] - - pair = _select_latest_synchronised_pair(front, side, 100_000_000) - - assert pair is not None - selected_front, selected_side, skew = pair - assert selected_front.stamp_ns == 1_000_000_000 - assert selected_side.stamp_ns == 1_030_000_000 - assert skew == 30_000_000 - - -def test_pairing_prefers_newest_valid_common_pair(): - front = [_pose(1_000_000_000), _pose(1_200_000_000)] - side = [_pose(1_010_000_000), _pose(1_240_000_000)] - - pair = _select_latest_synchronised_pair(front, side, 100_000_000) - - assert pair is not None - selected_front, selected_side, skew = pair - assert selected_front.stamp_ns == 1_200_000_000 - assert selected_side.stamp_ns == 1_240_000_000 - assert skew == 40_000_000 - - -def test_pairing_reports_minimum_skew_when_no_pair_passes(): - front = [_pose(1_000_000_000), _pose(1_200_000_000)] - side = [_pose(1_370_000_000)] - - assert _select_latest_synchronised_pair( - front, side, 100_000_000 - ) is None - assert _minimum_history_skew_ns(front, side) == 170_000_000 - - -def _transform(rotation_deg: float, translation_m: float) -> np.ndarray: - value = np.eye(4) - value[:3, :3] = Rotation.from_euler( - "z", rotation_deg, degrees=True - ).as_matrix() - value[0, 3] = translation_m - return value - - -def test_repeatability_summary_accepts_consistent_capture_set(): - captures = [ - [_transform(-0.1, -0.0005)], - [_transform(0.0, 0.0)], - [_transform(0.1, 0.0005)], - ] - - _, selected, rotation_error, translation_error = ( - _summarize_transform_repeatability(captures) - ) - - assert len(selected) == 3 - np.testing.assert_allclose( - np.degrees(rotation_error), 0.1, atol=1.0e-6 - ) - np.testing.assert_allclose(translation_error, 0.0005, atol=1.0e-9) - - -def test_repeatability_summary_exposes_current_capture_outlier(): - captures = [ - [_transform(0.0, 0.0)], - [_transform(0.1, 0.0005)], - [_transform(1.0, 0.008)], - ] - - _, _, rotation_error, translation_error = ( - _summarize_transform_repeatability(captures) - ) - - assert np.degrees(rotation_error) > 0.3 - assert translation_error > 0.0015 - - -def test_each_camera_must_pass_reprojection_gate_independently(): - assert _individual_reprojection_passes(0.8, 1.1, 1.2) - assert not _individual_reprojection_passes(0.5, 1.4, 1.2) - assert _individual_reprojection_passes(0.5, 1.4, 1.5) - assert not _individual_reprojection_passes(0.5, 1.6, 1.5) - assert not _individual_reprojection_passes(float("nan"), 0.5, 1.2) - - -def test_pose_novelty_is_checked_against_all_previous_captures(): - previous = [_transform(0.0, 0.0), _transform(10.0, 0.10)] - - assert not _pose_is_novel( - _transform(10.5, 0.105), - previous, - minimum_rotation_rad=np.deg2rad(2.0), - minimum_translation_m=0.015, - ) - assert _pose_is_novel( - _transform(14.0, 0.13), - previous, - minimum_rotation_rad=np.deg2rad(2.0), - minimum_translation_m=0.015, - ) - - -def test_joint_fit_trims_moderate_bad_views_without_relaxing_gate( - monkeypatch, -): - captures = [ - StereoCapture( - front_points_px=np.zeros((40, 2), dtype=np.float32), - other_points_px=np.zeros((40, 2), dtype=np.float32), - transform_candidates=(np.eye(4),), - pair_reprojection_rms_px=1.0, - skew_ns=1, - ) - for _ in range(20) - ] - - def fake_fit_once( - captures_arg, - indices, - object_points, - front_matrix, - other_matrix, - image_size, - ): - del ( - captures_arg, - object_points, - front_matrix, - other_matrix, - image_size, - ) - errors = np.asarray( - [1.1 if index < 15 else 1.5 for index in indices], - dtype=float, - ) - return float(np.sqrt(np.mean(np.square(errors)))), np.eye(4), errors - - monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once) - result = _fit_stereo_robust( - captures, - np.zeros((40, 3), dtype=np.float32), - np.eye(3), - np.eye(3), - (640, 480), - minimum_inliers=15, - maximum_rms_px=1.2, - maximum_rotation_stability_rad=np.deg2rad(0.3), - maximum_translation_stability_m=0.0015, - ) - - assert result.passed - assert result.stereo_rms_px <= 1.2 - assert len(result.inlier_indices) >= 15 - assert result.rejected_indices - assert set(result.rejected_indices) <= set(range(15, 20)) - - -def test_joint_fit_reports_finite_provisional_metrics_before_minimum( - monkeypatch, -): - captures = [ - StereoCapture( - front_points_px=np.zeros((40, 2), dtype=np.float32), - other_points_px=np.zeros((40, 2), dtype=np.float32), - transform_candidates=(np.eye(4),), - pair_reprojection_rms_px=0.8, - skew_ns=1, - ) - for _ in range(10) - ] - - def fake_fit_once( - captures_arg, - indices, - object_points, - front_matrix, - other_matrix, - image_size, - ): - del ( - captures_arg, - object_points, - front_matrix, - other_matrix, - image_size, - ) - return 0.8, np.eye(4), np.full(len(indices), 0.8) - - monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once) - result = _fit_stereo_robust( - captures, - np.zeros((40, 3), dtype=np.float32), - np.eye(3), - np.eye(3), - (640, 480), - minimum_inliers=15, - maximum_rms_px=1.2, - maximum_rotation_stability_rad=np.deg2rad(0.3), - maximum_translation_stability_m=0.0015, - ) - - assert not result.passed - assert result.stereo_rms_px == 0.8 - assert len(result.inlier_indices) == 10 - - -def test_joint_stereo_fit_recovers_transform_and_rejects_bad_view(): - random = np.random.default_rng(7) - object_points = np.zeros((40, 3), dtype=np.float32) - object_points[:, :2] = ( - np.mgrid[0:8, 0:5].T.reshape(-1, 2) * 0.027 - ) - matrix = np.asarray( - [[1800.0, 0.0, 812.0], [0.0, 1795.0, 620.0], [0.0, 0.0, 1.0]] - ) - other_from_front = np.eye(4) - other_from_front[:3, :3] = Rotation.from_euler( - "xyz", [2.0, 18.0, -1.0], degrees=True - ).as_matrix() - other_from_front[:3, 3] = [0.20, -0.01, 0.04] - expected_front_from_other = np.linalg.inv(other_from_front) - captures = [] - for index in range(21): - front_from_board = np.eye(4) - front_from_board[:3, :3] = Rotation.from_euler( - "xyz", - [ - -8.0 + index * 0.7, - 5.0 + (index % 5) * 2.0, - -5.0 + (index % 4) * 3.0, - ], - degrees=True, - ).as_matrix() - front_from_board[:3, 3] = [ - -0.08 + (index % 5) * 0.035, - -0.04 + (index % 4) * 0.025, - 0.75 + (index % 3) * 0.08, - ] - other_from_board = other_from_front @ front_from_board - front_rvec = Rotation.from_matrix( - front_from_board[:3, :3] - ).as_rotvec() - other_rvec = Rotation.from_matrix( - other_from_board[:3, :3] - ).as_rotvec() - front_points, _ = cv2.projectPoints( - object_points, - front_rvec, - front_from_board[:3, 3], - matrix, - np.zeros(5), - ) - other_points, _ = cv2.projectPoints( - object_points, - other_rvec, - other_from_board[:3, 3], - matrix, - np.zeros(5), - ) - front_points = front_points.reshape(-1, 2) - other_points = other_points.reshape(-1, 2) - front_points += random.normal(0.0, 0.12, front_points.shape) - other_points += random.normal(0.0, 0.12, other_points.shape) - if index == 20: - other_points += random.normal(0.0, 4.0, other_points.shape) - captures.append( - StereoCapture( - front_points_px=front_points.astype(np.float32), - other_points_px=other_points.astype(np.float32), - transform_candidates=(expected_front_from_other.copy(),), - pair_reprojection_rms_px=0.2, - skew_ns=10_000_000, - ) - ) - - result = _fit_stereo_robust( - captures, - object_points, - matrix, - matrix, - (1624, 1240), - minimum_inliers=15, - maximum_rms_px=1.2, - maximum_rotation_stability_rad=np.deg2rad(0.3), - maximum_translation_stability_m=0.0015, - ) - - assert result.passed - assert 20 in result.rejected_indices - assert len(result.inlier_indices) >= 15 - rotation_error = Rotation.from_matrix( - result.front_from_other[:3, :3] - ).inv() * Rotation.from_matrix(expected_front_from_other[:3, :3]) - assert np.degrees(rotation_error.magnitude()) < 0.05 - assert np.linalg.norm( - result.front_from_other[:3, 3] - - expected_front_from_other[:3, 3] - ) < 0.001 diff --git a/src/linkerhand_calibration/test/test_feedback_capture_timing.py b/src/linkerhand_calibration/test/test_feedback_capture_timing.py deleted file mode 100644 index 549978e..0000000 --- a/src/linkerhand_calibration/test/test_feedback_capture_timing.py +++ /dev/null @@ -1,37 +0,0 @@ -"""Timer starvation cannot discard independently received SDK observations.""" - -import pytest - -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from runtime_host_fixture import coordinator_fixture - - -@pytest.mark.parametrize("pulse", [False, True]) -def test_sdk_feedback_between_slow_ticks_defines_the_stable_window(tmp_path, pulse): - host, clock = coordinator_fixture(tmp_path) - try: - profile = host.profile - baseline = profile.command.baseline_values - command = MotionCommand("joint_zero", baseline, 40., command_index=1) - segment = MotionExecution(profile, command, initial_command=baseline, - initial_feedback=baseline, now=clock.now, identity="slow_timer") - host.segment = segment - segment.sample(clock.now) - start = clock.now - for index in range(16): - clock.now = start+index*.02 - feedback = list(baseline) - if pulse and index == 9: - feedback[0] -= 5 # A held channel moves between control ticks. - host.receive_feedback(profile.command.names, feedback, host.ports.clock_ns()) - if index in {0, 15}: - segment.sample(clock.now) - segment.observe(host.latest_feedback, stamp=host.state_history[-1].stamp_ns, now=clock.now) - assert len(segment.stamped_history) == 16 - assert segment.steady_ready(clock.now) is not pulse - # Replayed feedback identities do not manufacture a larger sample set. - before = len(segment.stamped_history) - host.receive_feedback(profile.command.names, baseline, host.ports.clock_ns()) - assert len(segment.stamped_history) == before - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_feedback_reversal_progress.py b/src/linkerhand_calibration/test/test_feedback_reversal_progress.py deleted file mode 100644 index 1e194a9..0000000 --- a/src/linkerhand_calibration/test/test_feedback_reversal_progress.py +++ /dev/null @@ -1,131 +0,0 @@ -"""A delayed reversal must prove directional travel without inventing an endpoint.""" - -from dataclasses import replace -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.adapters import HardwareHealth -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.runtime.safety import SafetyPolicy, SafetySample - - -@pytest.fixture -def profile(): - return load_bundled_hand_profile("o30_right_18", scope="full") - - -def segment(profile, *, increasing=False, references=()): - start = list(profile.command.baseline_values) - target, feedback = start.copy(), start.copy() - start[0], target[0], feedback[0] = (0., 32., 34.) if increasing else (255., 223., 221.) - motion = MotionCommand("sweep", tuple(target), 200., command_index=0, - defer_settling_to_hold=True, feedback_targets=references) - return MotionExecution(profile, motion, initial_command=start, initial_feedback=feedback, - now=0., identity="reversal") - - -def observe(motion, safety, now, value, stamp): - command = motion.sample(now) - feedback = list(motion.start_feedback) - feedback[0] = value - motion.observe(feedback, stamp=stamp, now=now) - return safety.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=motion.motion_expected, motion_id=motion.identity, motion_goals=motion.goals)) - - -def test_recorded_reversal_returns_to_initial_feedback_and_completes(profile): - path = Path(__file__).parent / "fixtures/o30_roll_delayed_feedback.json" - trace = json.loads(path.read_text()) - motion, safety = segment(profile), SafetyPolicy(profile) - arrived_at = None - for stamp, (now, command, feedback) in enumerate(trace["frames"]): - assert observe(motion, safety, now, feedback, stamp).safe - # The independent logged command agrees with the configured trajectory. - assert abs(motion.sample(now)[0]-command) <= 2 - if motion.arrived(now) and arrived_at is None: - arrived_at = now - assert arrived_at is not None and arrived_at < 3. - assert motion.history[-1][0] == motion.start_feedback[0] == 221. - assert motion.feedback_motion_complete() - - -@pytest.mark.parametrize("increasing", [False, True]) -def test_delayed_feedback_reversal_is_symmetric(profile, increasing): - motion, safety = segment(profile, increasing=increasing), SafetyPolicy(profile) - original = motion.start_feedback[0] - direction = 1 if increasing else -1 - for stamp, now in enumerate([0., .3, .6, 1., 2., 3., 4., 5.]): - # Finish the preceding direction, reverse, and settle at the initial - # reading. Net displacement is zero, but requested travel is real. - feedback = original - direction*([0., 26., 18., 10., 0., 0., 0., 0.][stamp]) - assert observe(motion, safety, now, feedback, stamp).safe - assert motion.feedback_motion_complete() and motion.arrived(now) - - -@pytest.mark.parametrize("increasing", [False, True]) -@pytest.mark.parametrize("opposite_only", [False, True]) -def test_no_motion_and_only_opposite_motion_still_stop(profile, increasing, opposite_only): - motion, safety = segment(profile, increasing=increasing), SafetyPolicy(profile) - direction = 1 if increasing else -1 - first_expected = None - for stamp in range(240): - now = stamp/30 - excursion = min(26., now*10) if opposite_only else 0. - result = observe(motion, safety, now, motion.start_feedback[0]-direction*excursion, stamp) - if motion.motion_expected and first_expected is None: - first_expected = now - assert not motion.feedback_motion_complete() - assert not motion.arrived(now) - if not result.safe: - assert result.code == "mechanical_stall" - assert now-first_expected == pytest.approx(profile.acquisition.stall_timeout_seconds, abs=1/30) - break - else: - pytest.fail("the feedback turning point restarted the stall timeout") - - -def test_duplicate_feedback_cannot_supply_a_turning_point(profile): - motion, safety = segment(profile), SafetyPolicy(profile) - assert observe(motion, safety, 0., 221., 1).safe - assert observe(motion, safety, .3, 247., 1).safe - assert observe(motion, safety, 2., 221., 2).safe - assert not motion.feedback_motion_complete() - - -@pytest.mark.parametrize("mapped,reference", [(True, None), (False, 3.)]) -def test_absolute_travel_and_measured_return_contracts_are_preserved(profile, mapped, reference): - profile = replace(profile, acquisition=replace(profile.acquisition, - feedback_travel_matches_command=mapped)) - motion = segment(profile, references=() if reference is None else ((0, reference),)) - safety = SafetyPolicy(profile) - for stamp, value in enumerate([221., 247., 240., 230., 221.]): - observe(motion, safety, stamp*.5, value, stamp) - assert not motion.feedback_motion_complete() - expected = 221.-32.*.8 if mapped else reference - assert motion.goals[0].end_feedback == pytest.approx(expected) - - -def test_other_channel_motion_does_not_release_a_stalled_channel(profile): - first = segment(profile) - target = list(first.command.target) - target[6] = 32. - motion = MotionExecution(profile, replace(first.command, target=tuple(target)), - initial_command=first.start, initial_feedback=first.start_feedback, now=0., identity="group") - safety = SafetyPolicy(profile) - for stamp in range(240): - now = stamp/30 - command = motion.sample(now) - feedback = list(motion.start_feedback) - feedback[0] = 247. if .1 < now < .5 else 221. - motion.observe(feedback, stamp=stamp, now=now) - result = safety.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=motion.motion_expected, motion_id=motion.identity, motion_goals=motion.goals)) - assert not motion.arrived(now) - if not result.safe: - assert result.code == "mechanical_stall" and "channel=6" in result.details - break - else: - pytest.fail("one moving channel released the whole group") diff --git a/src/linkerhand_calibration/test/test_final_image_holdout.py b/src/linkerhand_calibration/test/test_final_image_holdout.py deleted file mode 100644 index 6600365..0000000 --- a/src/linkerhand_calibration/test/test_final_image_holdout.py +++ /dev/null @@ -1,85 +0,0 @@ -"""Final pixels are independent of the intermediate reconstructed Tag poses.""" - -from dataclasses import replace -import json - -import cv2 -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points -from linkerhand_calibration.core.urdf.image_acceptance import TagImageObservation, validate_serialized_image_holdout -from linkerhand_calibration.runtime.artifacts.publisher import FrozenTagArtifactValidator -from test_frozen_tag_replay import fixture - - -def independent_pixels(observations): - camera = ((1200., 0., 640.), (0., 1200., 480.), (0., 0., 1.)) - # Distinct camera view; the generator starts from independent physical poses. - view = np.eye(4) - view[:3, :3] = Rotation.from_euler("xyz", (.08, -.1, .04)).as_matrix() - view[:3, 3] = (.02, -.03, .5) - images = [] - for row in observations: - pose = view @ np.asarray(row.common_from_tag) - corners, _ = cv2.projectPoints(square_object_points(.016), Rotation.from_matrix(pose[:3, :3]).as_rotvec(), - pose[:3, 3], np.asarray(camera), np.zeros(5)) - images.append(TagImageObservation(row.sample_id, row.role, row.cycle, row.sdk_values, - tuple(sorted(row.sdk_directions_by_joint.items())), "independent_view", .016, - tuple(map(tuple, corners.reshape(4, 2))), camera, tuple(map(tuple, view)))) - return tuple(images) - - -@pytest.fixture -def scene(tmp_path): - source, plan, base, _, mounts, holdout, payload = fixture(tmp_path) - written = plan.write(source, tmp_path / "corrected.urdf") - images = independent_pixels(holdout) - kwargs = dict(corrected_urdf=written, payload=payload, common_from_base=base, installations=mounts, - observations=images, independent_observations=holdout, - required_roles=("a", "b"), input_kind="feedback", holdout_cycle=3, minimum_samples=40) - return source, plan, holdout, kwargs - - -def test_written_json_urdf_predict_original_corners_without_pose_refitting(scene): - *_, kwargs = scene - report = validate_serialized_image_holdout(**kwargs) - assert set(report) == {"a", "b"} - assert max(value["maximum_px"] for value in report.values()) < 1e-6 - - -@pytest.mark.parametrize("mutation", ["pixels", "camera", "extrinsics", "size", "overlap", "duplicate", "missing"]) -def test_wrong_raw_pixels_or_camera_cannot_hide_behind_good_3d_poses(scene, mutation): - *_, kwargs = scene - images = kwargs["observations"] - row = images[0] - if mutation == "pixels": - row = replace(row, corners_xy=tuple((x+8, y) for x, y in row.corners_xy)) - elif mutation == "camera": - camera = np.asarray(row.camera_matrix).copy(); camera[0, 0] *= 1.2 - row = replace(row, camera_matrix=tuple(map(tuple, camera))) - elif mutation == "extrinsics": - camera = np.asarray(row.view_from_common).copy(); camera[0, 3] += .01 - row = replace(row, view_from_common=tuple(map(tuple, camera))) - elif mutation == "size": - row = replace(row, tag_size_m=.03) - elif mutation == "overlap": - row = replace(row, cycle=0) - elif mutation == "duplicate": - images += (row,) - elif mutation == "missing": - images = tuple(item for item in images if item.role == "a") - with pytest.raises(ValueError, match="final_image_holdout"): - validate_serialized_image_holdout(**{**kwargs, "observations": (row, *images[1:])}) - - -def test_publisher_requires_pixels_after_3d_passes(scene, tmp_path): - source, plan, holdout, kwargs = scene - calibration = tmp_path / "calibration.json" - calibration.write_text(json.dumps(kwargs["payload"])) - validator = FrozenTagArtifactValidator(kwargs["payload"]["profile_id"], source, plan.source_sha256, - plan.authorized_fields, plan.zero_offsets_rad, kwargs["common_from_base"], kwargs["installations"], - tuple(holdout), ("a", "b"), lambda _: None, require_image_holdout=True) - with pytest.raises(ValueError, match="final_image_holdout_missing"): - validator(calibration, kwargs["corrected_urdf"]) diff --git a/src/linkerhand_calibration/test/test_final_image_sources.py b/src/linkerhand_calibration/test/test_final_image_sources.py deleted file mode 100644 index 1b20a84..0000000 --- a/src/linkerhand_calibration/test/test_final_image_sources.py +++ /dev/null @@ -1,64 +0,0 @@ -"""Actual camera metadata/corners bind to the configured protected file.""" - -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.geometry.extrinsics import transform_matrix -from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.runtime.artifacts.image_evidence import prepare_image_holdout - - -def scene(): - config = load_product_config(Path(__file__).resolve().parents[1]/"config/o6_right_product.yaml", check_can=False) - raw = json.loads(gzip.decompress((Path(__file__).parent/"fixtures/o6_final_image_sources_160040.json.gz").read_bytes())) - assert raw["source_raw_sha256"] == "57ebe3d1987e14ca48911c8db1dddb680133d3edf436e59da7677bbf6931a236" - profile = config.calibration_contract.typed_profile - # This immutable historical capture used 16 mm Tags. Bind its original - # measurement contract instead of relabeling pixels with today's 16.5 mm - # product setting; mismatch rejection remains part of the tests below. - assert {tag['tag_size_m'] for frame in raw['frames'] for tag in frame['roles'].values()} == {.016} - profile = replace(profile, vision=replace(profile.vision, - views=tuple(replace(view, tags=tuple(replace(tag, size_m=.016) for tag in view.tags)) - for view in profile.vision.views))) - config = replace(config, calibration_contract=replace(config.calibration_contract, declarative=profile)) - observations = [] - for row in raw["samples"]: - role = profile.measurement.measurements[row["joint"]].child_role - pose = row["child_pose_common"] - observations.append(TagHoldout(f"{row['view']}:{row['image_stamp_ns']}", role, 3, - tuple(row["state_u8"]), {}, tuple(map(tuple, transform_matrix(pose["translation_xyz_m"], pose["quaternion_xyzw"]))))) - return config, [*raw["models"], *raw["frames"]], observations - - -def test_raw_pixels_bind_without_using_intermediate_tag_poses(): - config, records, observations = scene() - images = prepare_image_holdout(config.calibration_contract.typed_profile, records, observations, - camera_extrinsics_file=config.camera_extrinsics, - protected_inputs={"camera_extrinsics_sha256": config.camera_extrinsics_sha256}) - assert len(images) == len(observations) == 2 - assert {image.view for image in images} == {"top", "front"} - assert all(len(image.corners_xy) == 4 for image in images) - - -@pytest.mark.parametrize("mutation", ["missing", "intrinsics", "matrix", "size", "duplicate", "file_hash"]) -def test_missing_or_changed_camera_identity_cannot_enter_final_check(mutation): - config, records, observations = scene() - records = deepcopy(records) - matrix = next(row for row in records if row.get("kind") == "rectified_camera_model" and row["view"] == "top") - frame = next(row for row in records if row.get("kind") == "pnp_candidate_frame" and row["view"] == "top") - digest = config.camera_extrinsics_sha256 - if mutation == "missing": records.remove(frame) - elif mutation == "intrinsics": matrix["raw_k"][0] += 10 - elif mutation == "matrix": frame["camera_matrix"][0][0] += 10 - elif mutation == "size": next(iter(frame["roles"].values()))["tag_size_m"] = .02 - elif mutation == "duplicate": records.append({**frame, "camera_matrix": [[1, 0, 0], [0, 1, 0], [0, 0, 1]]}) - else: digest = "f"*64 - with pytest.raises(ValueError, match="final_image"): - prepare_image_holdout(config.calibration_contract.typed_profile, records, observations, - camera_extrinsics_file=config.camera_extrinsics, protected_inputs={"camera_extrinsics_sha256": digest}) diff --git a/src/linkerhand_calibration/test/test_finalization_worker.py b/src/linkerhand_calibration/test/test_finalization_worker.py deleted file mode 100644 index ff861ba..0000000 --- a/src/linkerhand_calibration/test/test_finalization_worker.py +++ /dev/null @@ -1,58 +0,0 @@ -from threading import Event - -import pytest - -from linkerhand_calibration.runtime.artifacts.worker import FinalizationWorker - - -def test_fit_does_not_block_control_and_abort_cannot_publish(): - entered, release, finished = Event(), Event(), Event() - def fit(*, publish, cancelled): - assert not publish - entered.set() - assert release.wait(2) - assert cancelled() - finished.set() - return "not publishable" - worker = FinalizationWorker() - commits = [] - try: - worker.start(fit) - assert entered.wait(2) - # A deterministic blocked fit must not block any controller poll. - for _ in range(100): - assert worker.finish_if_ready(commits.append) is None - worker.cancel() - release.set() - assert finished.wait(2) - assert worker.finish_if_ready(commits.append) is None - assert commits == [] - finally: - release.set() - worker.close() - - -def test_success_commits_once_and_failure_never_retries(): - for fail in (False, True): - worker = FinalizationWorker() - commits = [] - try: - def fit(**kwargs): - assert kwargs["publish"] is False - return ("json", "fit", "staged") - worker.start(fit) - worker._future.result(timeout=2) - def commit(value): - commits.append(value) - if fail: - raise ValueError("invalid staged hash") - if fail: - with pytest.raises(ValueError, match="hash"): - worker.finish_if_ready(commit) - else: - assert worker.finish_if_ready(commit) == ("json", "fit", "staged") - assert worker.finish_if_ready(commit) is None - assert len(commits) == 1 - finally: - worker.close() - diff --git a/src/linkerhand_calibration/test/test_finger_chain_diagnostic.py b/src/linkerhand_calibration/test/test_finger_chain_diagnostic.py deleted file mode 100644 index a373676..0000000 --- a/src/linkerhand_calibration/test/test_finger_chain_diagnostic.py +++ /dev/null @@ -1,251 +0,0 @@ -"""Three-stage known-truth recovery without frozen upstream geometry.""" -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose.finger_chain_diagnostic import ( - FingerChainImages, chain_candidate_seeds) -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points - - -@pytest.fixture(scope='module') -def chain_fit(): - return make_chain() - - -def make_chain(*, parallel=False, full_parallel=False, stage_changes=False): - matrix = np.array([[1600., 0, 640.], [0, 1600., 480.], [0, 0, 1.]]) - parent = Rotation.from_euler('xy', [8., -12.], degrees=True) - origin = np.array([.01, .01, .5]) - installation = Rotation.from_euler('xy', [5., 8.], degrees=True) - translation = np.array([.005, -.04, .003]) - axes = np.array([[.04, -.02, 1.], [-.02, .01, 1.], [0., 0., 1.]]) - axes /= np.linalg.norm(axes, axis=1)[:, None] - if parallel: - axes[2] = axes[1] - if full_parallel: - axes[:] = axes[1] - pivots = np.array([[0., .06, 0.], [0., .015, 0.], [0., -.025, 0.]]) - objects = square_object_points(.016) - rows, phases = [], [] - for phase in range(3): - for theta in np.linspace(0., 1.3, 12): - turn = Rotation.from_rotvec(axes[phase]*theta) - proximal, distal = objects, installation.apply(objects)+translation - if phase < 2: - proximal = turn.apply(proximal-pivots[phase])+pivots[phase] - distal = turn.apply(distal-pivots[phase])+pivots[phase] - tags = {} - for role, points in [('parent', proximal), ('child', distal)]: - camera = parent.apply(points)+origin - if stage_changes: - camera = Rotation.from_rotvec([phase*.006, phase*.004, 0.]).apply(camera) - camera += [phase*.0003, 0., phase*.0004] - projected = camera@matrix.T - tags[role] = dict(tag_size_m=.016, - corners_xy=(projected[:, :2]/projected[:, 2:]).tolist()) - rows.append(dict(image_stamp_ns=len(rows)+1, camera_matrix=matrix.tolist(), tags=tags)) - phases.append(phase) - vector = pivots[2]-pivots[1] - distance = np.linalg.norm(vector-axes[1]*(vector@axes[1])) if parallel else None - proximal_vector = pivots[1]-pivots[0] - proximal_distance = (np.linalg.norm(proximal_vector-axes[0]*(proximal_vector@axes[0])) - if full_parallel else None) - bundle = FingerChainImages(rows, phases, ('parent', 'child'), pip_dip_distance_m=distance, - mcp_pip_distance_m=proximal_distance, - independent_stage_references=stage_changes) - fit = bundle.fit((0,)*6) - assert fit.success - return bundle, fit, axes, pivots - - -def test_full_chain_recovers_known_geometry(chain_fit): - bundle, fit, axes, pivots = chain_fit - assert np.max(np.abs(fit.fun)) < 1e-4 - physical = bundle.physical_geometry(fit.x[:24]) - assert np.all(np.abs(np.sum(physical[:, :3]*axes, axis=1)) > .99999) - pivots = pivots-axes*np.sum(axes*pivots, axis=1)[:, None] - assert np.max(np.linalg.norm(physical[:, 3:]-pivots, axis=1)) < 1e-5 - - -def test_chain_covariance_marginalizes_all_angles(chain_fit): - bundle, fit, *_ = chain_fit - jac = fit.jac*bundle.scale[None, :] - expected = np.linalg.inv(jac.T@jac)[:24, :24]*.03**2 - expected *= bundle.scale[:24, None]*bundle.scale[None, :24] - assert np.allclose(bundle.covariance(fit), expected, atol=1e-10, rtol=1e-4) - assert np.all(np.isfinite(bundle.bounds(fit))) - - -def test_validation_does_not_refit_chain_geometry(chain_fit): - bundle, fit, *_ = chain_fit - previous = fit.x.copy() - residual, _ = bundle.validate(fit, bundle) - assert np.max(np.abs(residual)) < 1e-4 - assert np.array_equal(previous, fit.x) - - -def test_cartesian_seed_enumeration_and_phase_contract(chain_fit): - bundle, *_ = chain_fit - assert len(set(chain_candidate_seeds())) == 64 - with pytest.raises(ValueError, match='three_phases_required'): - FingerChainImages(bundle.rows, np.zeros(bundle.count), ('parent', 'child')) - - -def test_every_phase_tag_branch_contributes_to_initial_state(chain_fit): - original, *_ = chain_fit - bundle = FingerChainImages(original.rows, original.phases, ('parent', 'child')) - baseline = bundle.initialize((0,)*6) - for index in range(6): - branches = [0]*6 - branches[index] = 1 - alternate = bundle.initialize(branches) - assert np.linalg.norm(alternate-baseline) > 1e-6 - - -def test_validation_exposes_held_parent_drift(chain_fit): - import copy - bundle, fit, *_ = chain_fit - rows = copy.deepcopy(bundle.rows) - for row, phase in zip(rows, bundle.phases): - if phase == 2: - row['tags']['parent']['corners_xy'] = ( - np.asarray(row['tags']['parent']['corners_xy'])+[4., 0.]).tolist() - other = FingerChainImages(rows, bundle.phases, ('parent', 'child')) - residuals, _ = bundle.validate(fit, other) - errors = np.sqrt(np.mean(np.sum(residuals.reshape(-1, 2, 4, 2)**2, axis=3), axis=2)) - assert np.min(errors[bundle.phases == 2, 0]) > 3.99 - - -def test_parallel_chain_estimates_shared_axis_and_propagates_covariance(): - bundle, fit, axes, pivots = make_chain(parallel=True) - assert bundle.static_width == 21 - assert np.max(np.abs(fit.fun)) < 1e-4 - geometry = bundle.physical_geometry(fit.x[:21]) - assert abs(geometry[1, :3]@geometry[2, :3]) > 1-1e-12 - assert np.all(np.abs(np.sum(geometry[:, :3]*axes, axis=1)) > .99999) - vector = geometry[2, 3:]-geometry[1, 3:] - assert abs(np.linalg.norm(vector)-bundle.pip_dip_distance_m) < 1e-9 - jac = fit.jac*bundle.scale[None, :] - expected = np.linalg.inv(jac.T@jac)[:21, :21]*.03**2 - expected *= bundle.scale[:21, None]*bundle.scale[None, :21] - assert np.allclose(bundle.covariance(fit), expected, atol=1e-10, rtol=1e-4) - assert np.all(np.isfinite(bundle.bounds(fit))) - residuals, _ = bundle.validate(fit, bundle) - assert np.max(np.abs(residuals)) < 1e-4 - - -def test_full_parallel_chain_has_no_redundant_geometry_parameters(): - bundle, fit, axes, _ = make_chain(parallel=True, full_parallel=True) - assert bundle.static_width == 18 - assert np.max(np.abs(fit.fun)) < 1e-4 - geometry = bundle.physical_geometry(fit.x[:18]) - assert np.all(np.abs(np.sum(geometry[:, :3]*axes, axis=1)) > .99999) - for phase in (1, 2): - assert abs(np.linalg.norm(geometry[phase, 3:]-geometry[phase-1, 3:]) - -bundle.distances[phase]) < 1e-9 - assert np.all(np.isfinite(bundle.bounds(fit))) - - -def test_stage_references_are_observed_and_uncertainty_is_marginalized(): - bundle, fit, axes, _ = make_chain(parallel=True, full_parallel=True, stage_changes=True) - assert bundle.static_width == 30 - assert len(bundle.angle_columns) == bundle.count-2 - assert np.max(np.abs(fit.fun)) < 1e-4 - geometry = bundle.physical_geometry(fit.x[:bundle.static_width]) - assert np.all(np.abs(np.sum(geometry[:, :3]*axes, axis=1)) > .99999) - width = bundle.static_width - jac = fit.jac*bundle.scale[None, :] - expected = np.linalg.inv(jac.T@jac)[:width, :width]*.03**2 - expected *= bundle.scale[:width, None]*bundle.scale[None, :width] - assert np.allclose(bundle.covariance(fit), expected, atol=1e-10, rtol=1e-4) - assert np.all(np.isfinite(bundle.bounds(fit))) - - -def test_common_tag_scale_diagnostic_recovers_known_print_size(): - import copy - truth, _, _, _ = make_chain(parallel=True, full_parallel=True) - rows = copy.deepcopy(truth.rows) - for row in rows: - for tag in row['tags'].values(): - tag['tag_size_m'] = .0165 # Images were generated using .016 m. - bundle = FingerChainImages(rows, truth.phases, ('parent', 'child'), - pip_dip_distance_m=truth.distances[2], mcp_pip_distance_m=truth.distances[1], - estimate_tag_scale=True) - fit = bundle.fit((0,)*6) - assert fit.success and np.max(np.abs(fit.fun)) < 1e-4 - assert abs(np.exp(fit.x[bundle.tag_scale_column])*.0165-.016) < 1e-6 - assert np.all(np.isfinite(bundle.bounds(fit))) - with pytest.raises(ValueError, match='requires_two_structural_lengths'): - FingerChainImages(rows, truth.phases, ('parent', 'child'), estimate_tag_scale=True) - - -def test_continuous_angles_do_not_create_false_motion_bins(): - from linkerhand_calibration.core.geometry.tag_pose.finger_chain_diagnostic import continuous_phase_angles - phases = [0, 0, 0, 1, 1, 1] - angles = [.1, .2+2*np.pi, .3, -.1, -.2-2*np.pi, -.3] - assert np.allclose(continuous_phase_angles(angles, phases), [.1, .2, .3, -.1, -.2, -.3]) - - -def test_output_angle_comparison_removes_sign_and_wrap_not_zero(): - from linkerhand_calibration.core.geometry.tag_pose.finger_chain_diagnostic import compare_chain_output_angles - geometry = np.tile([0., 0., 1., 0., 0., 0.], (3, 1)) - other = geometry.copy() - other[1, :3] *= -1 - phases = np.repeat(np.arange(3), 3) - angles = np.tile([.1, .2, .3], 3) - alternative = angles.copy()+.04 - alternative[3:6] *= -1 - alternative[1] += 2*np.pi - differences = compare_chain_output_angles(geometry, angles, other, alternative, phases) - assert np.allclose(differences, .04) - with pytest.raises(ValueError, match='comparison_invalid'): - compare_chain_output_angles(geometry, angles[:-1], other, alternative, phases) - - -def test_validation_angle_covariance_keeps_shared_uncertainty(chain_fit): - import copy - from types import SimpleNamespace - bundle, fit, *_ = chain_fit - rows = copy.deepcopy(bundle.rows) - for row in rows: - row['image_stamp_ns'] += 1000 - other = FingerChainImages(rows, bundle.phases, ('parent', 'child')) - _, angles = bundle.validate(fit, other) - previous = fit.x.copy() - covariance = bundle.validation_angle_covariance(fit, other, angles) - assert np.allclose(covariance, covariance.T) - assert np.linalg.eigvalsh(covariance)[0] > 0 - assert np.max(np.abs(covariance-np.diag(np.diag(covariance)))) > 1e-9 - # Independent finite difference: refit only the held-out angles as each - # frozen training parameter changes, never re-estimate the training fit. - sensitivity = np.empty((other.count, bundle.static_width)) - for column in range(bundle.static_width): - delta = 1e-4*bundle.scale[column] - perturbed = [] - for sign in (1, -1): - vector = fit.x.copy() - vector[column] += sign*delta - _, values = bundle.validate(SimpleNamespace(x=vector), other) - perturbed.append(values) - sensitivity[:, column] = (perturbed[0]-perturbed[1])/(2*delta) - propagated = sensitivity@bundle.covariance(fit)@sensitivity.T - remainder = covariance-propagated - assert np.all(np.diag(remainder) > 0) - assert np.max(np.abs(remainder-np.diag(np.diag(remainder)))) < 1e-9 - assert np.array_equal(previous, fit.x) - with pytest.raises(ValueError, match='requires_independent_images'): - bundle.validation_angle_covariance(fit, bundle, angles) - - -@pytest.mark.parametrize('field', ['matrix', 'objects', 'roles']) -def test_validation_refuses_different_pixel_geometry(chain_fit, field): - import copy - bundle, fit, *_ = chain_fit - other = copy.copy(bundle) - value = getattr(other, field) - setattr(other, field, tuple(reversed(value)) if field == 'roles' else value*1.001) - with pytest.raises(ValueError, match='validation_geometry_changed'): - bundle.validate(fit, other) - with pytest.raises(ValueError, match='validation_geometry_changed'): - bundle.validation_angle_covariance(fit, other, np.zeros(other.count)) diff --git a/src/linkerhand_calibration/test/test_fixed_reference_pose.py b/src/linkerhand_calibration/test/test_fixed_reference_pose.py deleted file mode 100644 index e927bbd..0000000 --- a/src/linkerhand_calibration/test/test_fixed_reference_pose.py +++ /dev/null @@ -1,234 +0,0 @@ -"""Stationarity is checked against live corners, not changing IPPE rankings.""" - -from concurrent.futures import ThreadPoolExecutor -from dataclasses import replace -import json -from pathlib import Path -from threading import Event - -import numpy as np -import pytest - -from linkerhand_calibration.core.geometry.pnp import SquareTagPose, SquareTagPoseTracker -from linkerhand_calibration.core.geometry.tag_pose import tracking -from linkerhand_calibration.core.geometry.tag_pose.branch_confirmation import BranchState -from linkerhand_calibration.core.geometry.tag_pose.fixed_reference import FixedTagReference -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.pose_evidence import reference_branch_revisions -from test_observation_capture import frame, make_capture - - -REAL = json.loads((Path(__file__).parent / 'fixtures' / - 'o6_front_base_20260914_branch_events.json').read_text()) -ROLE = REAL['tag_role'] -STEP_NS = 40_000_000 - - -def recorded_reference(): - pose = REAL['reference_pose'] - return FixedTagReference(SquareTagPose(tuple(pose['rotation_xyzw']), - tuple(pose['translation_xyz_m']), 0.), 1) - - -def seeded_tracker(reference, stamp): - # Successful preparation frames were not logged. Start at the recorded - # frozen reference boundary; this is explicitly not an end-to-end replay. - tracker = SquareTagPoseTracker() - tracker._branches.states[ROLE] = BranchState(observed_ns=stamp, pose=reference.pose, - revision=reference.branch_revision, frozen=True, status='tracking', reason='') - return tracker - - -def observe(tracker, stamp, corners, reference): - return tracker.estimate(ROLE, corners, tag_size_m=REAL['tag_size_m'], - camera_matrix=REAL['camera_matrix'], stamp_ns=stamp, fixed_reference=reference) - - -def test_recorded_id0_conflict_is_resolved_only_with_declared_stationarity(): - reference = recorded_reference() - first = REAL['events'][0]['image_stamp_ns'] - moving_tracker = seeded_tracker(reference, first - STEP_NS) - fixed_tracker = seeded_tracker(reference, first - STEP_NS) - errors = [] - for event in REAL['events']: - stamp = event['image_stamp_ns'] - corners = event['pnp_observation_evidence']['corners_xy'] - rejected, reason = observe(moving_tracker, stamp, corners, None) - assert rejected is None and reason == 'pose_branch_ambiguous' - pose, reason = observe(fixed_tracker, stamp, corners, reference) - assert reason == '' and pose is not None - assert pose.quaternion_xyzw == reference.pose.quaternion_xyzw - assert pose.translation_xyz_m == reference.pose.translation_xyz_m - errors.append(pose.reprojection_error_px) - candidates, diagnostics, corrections = fixed_tracker.observation_snapshot(ROLE, stamp) - assert len(candidates) == 2 and corrections == 0 - assert diagnostics['branch_revision'] == 1 and diagnostics['branch_frozen'] - assert diagnostics['fixed_reference_verified'] is True - assert max(errors) == pytest.approx(.665772, abs=1e-5) - assert not moving_tracker._branches.states[ROLE].conflict - assert not fixed_tracker._branches.states[ROLE].conflict - - -@pytest.mark.parametrize('disturbance', ['translation', 'one_bad_corner', 'invalid_corners']) -def test_bad_fixed_images_are_never_samples_and_cannot_relocate_the_base(disturbance): - reference = recorded_reference() - tracker = seeded_tracker(reference, 1_000_000_000) - good = np.asarray(REAL['reference_corners_xy']) - bad = good.copy() - if disturbance == 'translation': - bad += [6., 0.] - elif disturbance == 'one_bad_corner': - bad[0] += [0., 4.] - else: - bad[0] = np.nan - for index in range(1, 10): - pose, reason = observe(tracker, 1_000_000_000 + index*STEP_NS, bad, reference) - assert pose is None - assert reason == 'fixed_reference_pose_unverified' - assert not tracker.freeze_roles({ROLE: 1}) - # Image inconsistency is missing measurement evidence, not proof of a - # different physical branch. Real corner drift has its own capture gate. - assert not tracker._branches.states[ROLE].conflict - assert observe(tracker, 2_000_000_000, good, reference) == (None, 'pose_branch_unconfirmed') - assert tracker._branches.states[ROLE].pose == reference.pose - assert tracker.branch_correction_counts == {} - - -def test_short_bad_image_discards_sample_then_recovers_without_refreezing(): - reference = recorded_reference() - tracker = seeded_tracker(reference, 1_000_000_000) - corners = np.asarray(REAL['reference_corners_xy']) - assert observe(tracker, 1_040_000_000, corners + 6, reference)[0] is None - assert not tracker.freeze_roles({ROLE: 1}) # Cannot cache the rejected base. - for index in range(1, 6): - pose, reason = observe(tracker, 1_040_000_000 + index*STEP_NS, corners, reference) - if index < 5: - assert pose is None and reason == 'pose_branch_unconfirmed' - assert not tracker.freeze_roles({ROLE: 1}) - else: - assert pose is not None and not reason - assert pose.translation_xyz_m == reference.pose.translation_xyz_m - assert tracker.freeze_roles({ROLE: 1}) - - -def test_recorded_161416_images_remain_rejected_without_latching_a_branch_conflict(): - data = json.loads((Path(__file__).parent / 'fixtures' / - 'o6_front_base_20260914_161416_quality_events.json').read_text()) - payload = data['reference_pose'] - reference = FixedTagReference(SquareTagPose(tuple(payload['rotation_xyzw']), - tuple(payload['translation_xyz_m']), 0.), 1) - first = data['events'][0]['image_stamp_ns'] - tracker = seeded_tracker(reference, first - STEP_NS) - for event in data['events']: - pose, reason = tracker.estimate(ROLE, event['pnp_observation_evidence']['corners_xy'], - tag_size_m=data['tag_size_m'], camera_matrix=data['camera_matrix'], - stamp_ns=event['image_stamp_ns'], fixed_reference=reference) - assert pose is None and reason == 'fixed_reference_pose_unverified' - error = tracker.observation_snapshot(ROLE, event['image_stamp_ns'])[1]['fixed_reference_reprojection_error_px'] - assert error > 1.5 - assert error == pytest.approx(event['pnp_observation_evidence']['candidate_diagnostics'][ - 'fixed_reference_reprojection_error_px'], abs=1e-10) - assert not tracker.freeze_roles({ROLE: 1}) - assert tracker._branches.states[ROLE].pose == reference.pose - assert not tracker._branches.states[ROLE].conflict - assert tracker.branch_correction_counts == {} - - -def test_interrupted_recovery_never_counts_failed_or_duplicate_frames(): - reference = recorded_reference() - tracker = seeded_tracker(reference, 1_000_000_000) - corners = np.asarray(REAL['reference_corners_xy']) - for stamp, bad in ((1, True), (2, False), (3, False), (4, False), (5, True)): - assert observe(tracker, 1_000_000_000 + stamp*STEP_NS, corners + (6 if bad else 0), reference)[0] is None - for _ in range(6): - assert observe(tracker, 1_240_000_000, corners, reference)[0] is None - for index in range(7, 11): - pose, _ = observe(tracker, 1_000_000_000 + index*STEP_NS, corners, reference) - assert (pose is not None) is (index == 10) - - -def test_long_absence_reconfirms_same_frozen_pose_and_never_counts_bad_frames(): - reference = recorded_reference() - tracker = seeded_tracker(reference, 1_000_000_000) - corners = np.asarray(REAL['reference_corners_xy']) - assert observe(tracker, 7_000_000_000, corners + 6, reference)[0] is None - for index in range(1, 6): - pose, reason = observe(tracker, 7_000_000_000 + index*STEP_NS, corners, reference) - if index < 5: - assert pose is None and reason == 'pose_branch_unconfirmed' - else: - assert pose is not None and not reason - assert tracker._branches.states[ROLE].revision == 1 - - -@pytest.mark.parametrize('revision', [True, 2]) -def test_wrong_reference_version_is_rejected(revision): - reference = recorded_reference() - tracker = seeded_tracker(reference, 1_000_000_000) - assert observe(tracker, 1_040_000_000, REAL['reference_corners_xy'], - replace(reference, branch_revision=revision)) == (None, 'pose_branch_reference_invalid') - - -@pytest.mark.parametrize('operation', ['reset', 'newer_image']) -def test_late_fixed_projection_cannot_commit_across_reset_or_a_newer_image(monkeypatch, operation): - reference = recorded_reference() - tracker = seeded_tracker(reference, 1_000_000_000) - entered, release = Event(), Event() - project = tracking.fixed_pose_reprojection_error - calls = 0 - - def blocked(*args, **kwargs): - nonlocal calls - calls += 1 - if calls == 1: - entered.set() - assert release.wait(3) - return project(*args, **kwargs) - - monkeypatch.setattr(tracking, 'fixed_pose_reprojection_error', blocked) - with ThreadPoolExecutor(1) as pool: - pending = pool.submit(observe, tracker, 1_040_000_000, REAL['reference_corners_xy'], reference) - try: - assert entered.wait(3) - if operation == 'reset': - tracker.reset() - else: - assert observe(tracker, 1_080_000_000, REAL['reference_corners_xy'], reference)[0] - finally: - release.set() - pose, reason = pending.result(timeout=3) - assert pose is None - if operation == 'reset': - assert reason == 'pose_observation_superseded' - assert not tracker.last_candidate_diagnostics_by_role - else: - assert reason == 'pose_observation_not_new' - assert tracker.observation_snapshot(ROLE, 1_080_000_000)[1]['branch_status'] == 'tracking' - - -def test_live_fixed_evidence_keeps_corners_and_frozen_geometry_through_zero_and_sweep(): - profile, lock, capture = make_capture() - capture.profile = profile = replace(profile, measurement=replace(profile.measurement, - candidate_selection_tasks=frozenset({'thumb_pitch_dip_front'}))) - lock.begin_session() - lock.start_locking() - for stamp in range(1, 15): - capture.consume(frame(profile, stamp)) - original = capture.locked_reference('front') - zero = profile.motion.joint_zero_references['rh_thumb_cmc_pitch'] - for stamp, phase in ((20, 'joint_zero'), (21, 'sweep')): - motion = MotionCommand(phase, zero.command, 40, task_key=zero.task_key, - command_index=0, cycle=0, zero_joints=('rh_thumb_cmc_pitch',)) - rows, movement = capture.consume(frame(profile, stamp), motion) - assert movement is None - row = next(r for r in rows if r.get('joint') == 'rh_thumb_cmc_pitch') - parent, child = (row['pnp_observation_evidence'][side] for side in ('parent', 'child')) - assert parent['pose_source'] == 'verified_fixed_reference' - assert parent['corners_xy'] and parent['reprojection_valid_candidates'] - assert child['pose_source'] == 'current_image' # Moving role is never constrained. - assert parent['selected_pose']['quaternion_xyzw'] == list(original[0].quaternion_xyzw) - assert reference_branch_revisions([row], profile=profile)[('front', 'front_base')] == original[1] - candidate = next(r for r in rows if r['kind'] == 'pnp_candidate_frame') - assert candidate['roles']['front_base']['locked_reference'] is True - assert candidate['roles']['front_base']['corners_xy'] == parent['corners_xy'] - assert capture.locked_reference('front') == original diff --git a/src/linkerhand_calibration/test/test_frozen_candidate_selection.py b/src/linkerhand_calibration/test/test_frozen_candidate_selection.py deleted file mode 100644 index 3c80a6f..0000000 --- a/src/linkerhand_calibration/test/test_frozen_candidate_selection.py +++ /dev/null @@ -1,228 +0,0 @@ -"""Articulated scoring cannot exchange an online-frozen planar solution.""" - -from copy import deepcopy -import json - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.candidate_selection import resolve_chain_observations -from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION -from test_o12_observation_resolution import build_data, module - - -TASK = "thumb_mcp_dip_front" -PAIRS = {"thumb_mcp": module.THUMB_ROLES[:2], "thumb_dip": module.THUMB_ROLES[1:]} - - -def frozen_capture(monkeypatch): - """Use the existing independent articulated trajectory, recording its good branch.""" - rows = build_data(monkeypatch) - frames = {row["image_stamp_ns"]: row for row in rows if "roles" in row} - for frame in frames.values(): - frame["view"] = "front" - for role in module.THUMB_ROLES: - item = frame["roles"][role] - pose = module.solve_square_tag_ippe(item["corners_xy"])[-1] - item["selected"] = {"quaternion_xyzw": list(pose.quaternion_xyzw), - "translation_xyz_m": list(pose.translation_xyz_m), - "reprojection_error_px": pose.reprojection_error_px} - for row in rows: - if "joint" not in row: - continue - row["view"] = "front" - frame = frames[row["image_stamp_ns"]] - parent, child = (deepcopy(frame["roles"][role]["selected"]) for role in PAIRS[row["joint"]]) - quaternion, translation = module._relative(parent, child) - row.update(parent_pose_common=parent, child_pose_common=child, - relative_quaternion_xyzw=quaternion.tolist(), relative_translation_xyz_m=translation.tolist()) - row["pnp_observation_evidence"] = { - field: {"tag_role": role, "tag_id": index, "pose_source": "current_image", - "selected_pose": deepcopy(frame["roles"][role]["selected"]), - "candidate_diagnostics": {"observation_stamp_ns": row["image_stamp_ns"], - "branch_status": "tracking", "branch_revision": 1, "branch_frozen": True}} - for index, (field, role) in enumerate(zip(("parent", "child"), PAIRS[row["joint"]]))} - return [{"kind": "session_start", "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION}, *rows] - - -def resolve(rows, **kwargs): - return resolve_chain_observations(rows, task_key=TASK, role_pairs=PAIRS, - solve_pose=module.solve_square_tag_ippe, **kwargs) - - -def test_same_frozen_branch_preserves_training_and_holdout(monkeypatch): - rows = frozen_capture(monkeypatch) - before = deepcopy(rows) - result, report = resolve(rows) - assert rows == before - assert report["status"] == "resolved" - assert report["training_frames"] == 72 and report["holdout_frames"] == 24 - for original, resolved in zip(rows, result): - if "joint" in original: - assert np.allclose(original["relative_quaternion_xyzw"], resolved["relative_quaternion_xyzw"], atol=1e-12) - - -@pytest.mark.parametrize("cycle", [0, 3]) -@pytest.mark.parametrize("field", ["parent", "child"]) -def test_any_frozen_role_reselection_is_rejected_before_rewriting(monkeypatch, cycle, field): - rows = frozen_capture(monkeypatch) - row = next(row for row in rows if row.get("joint") == "thumb_dip" and row["cycle"] == cycle) - role = row["pnp_observation_evidence"][field]["tag_role"] - frame = next(item for item in rows if "roles" in item and item["image_stamp_ns"] == row["image_stamp_ns"]) - pose = row["pnp_observation_evidence"][field]["selected_pose"] - pose["quaternion_xyzw"] = (Rotation.from_euler("x", 10, degrees=True) - * Rotation.from_quat(pose["quaternion_xyzw"])).as_quat().tolist() - frame["roles"][role]["selected"] = deepcopy(pose) - # The same physical role can be the adjacent joint's child or parent. - for other in rows: - if other.get("image_stamp_ns") != row["image_stamp_ns"] or "joint" not in other: - continue - for item in other["pnp_observation_evidence"].values(): - if item["tag_role"] == role: - item["selected_pose"] = deepcopy(pose) - before = deepcopy(rows) - with pytest.raises(ValueError, match=rf"frozen_branch_reselection_forbidden:.*cycle={cycle}.*role={role}"): - resolve(rows) - assert rows == before - - -@pytest.mark.parametrize("mutation", ["missing", "not_frozen", "no_revision", "wrong_stamp", "role_changed", "candidate_pose_changed"]) -def test_incomplete_frozen_evidence_never_authorizes_reselection(monkeypatch, mutation): - rows = frozen_capture(monkeypatch) - row = next(row for row in rows if row.get("joint") == "thumb_dip") - item = row["pnp_observation_evidence"]["child"] - if mutation == "missing": - row.pop("pnp_observation_evidence") - elif mutation == "not_frozen": - item["candidate_diagnostics"]["branch_frozen"] = False - elif mutation == "no_revision": - item["candidate_diagnostics"].pop("branch_revision") - elif mutation == "wrong_stamp": - item["candidate_diagnostics"]["observation_stamp_ns"] += 1 - elif mutation == "role_changed": - item["tag_role"] = "another_tag" - else: - frame = next(frame for frame in rows if "roles" in frame and frame["image_stamp_ns"] == row["image_stamp_ns"]) - frame["roles"][item["tag_role"]]["selected"]["translation_xyz_m"][0] += .01 - before = deepcopy(rows) - with pytest.raises(ValueError, match="frozen_branch_"): - resolve(rows) - assert rows == before - - -def test_numerical_reconstruction_noise_is_not_a_branch_switch(monkeypatch): - rows = frozen_capture(monkeypatch) - for row in rows: - for item in row.get("pnp_observation_evidence", {}).values(): - pose = item["selected_pose"] - pose["quaternion_xyzw"] = (Rotation.from_rotvec([1e-9, 0, 0]) - * Rotation.from_quat(pose["quaternion_xyzw"])).as_quat().tolist() - pose["translation_xyz_m"][0] += 1e-10 - assert resolve(rows)[1]["status"] == "resolved" - - -def test_cached_fixed_pose_uses_reference_revision_without_fabricated_image_diagnostics(monkeypatch): - rows = frozen_capture(monkeypatch) - base_role = module.THUMB_ROLES[0] - for row in rows: - if "roles" in row: - row["roles"][base_role].update(locked_reference=True, corners_xy=None) - for item in row.get("pnp_observation_evidence", {}).values(): - if item["tag_role"] == base_role: - item.update(pose_source="cached_fixed_reference", reference_branch_revision=1, - candidate_diagnostics={}) - assert resolve(rows, fixed_roles=(base_role,))[1]["frozen_branches_preserved"] is True - - -def verified_fixed_capture(monkeypatch): - rows = frozen_capture(monkeypatch) - role = module.THUMB_ROLES[0] - frames = {row['image_stamp_ns']: row for row in rows if 'roles' in row} - # Give the stationary role independent pinhole image evidence; the - # existing moving-role fixture still supplies its articulated candidates. - half = .016 / 2 - points = np.array([[-half, half, 0], [half, half, 0], [half, -half, 0], [-half, -half, 0]]) - for frame in frames.values(): - item = frame['roles'][role] - pose = item['selected'] - xyz = Rotation.from_quat(pose['quaternion_xyzw']).apply(points) + pose['translation_xyz_m'] - pixels = xyz @ np.asarray(frame['camera_matrix']).T - item.update(locked_reference=True, corners_xy=(pixels[:, :2] / pixels[:, 2:]).tolist()) - pose['reprojection_error_px'] = 0. - for row in rows: - for item in row.get('pnp_observation_evidence', {}).values(): - if item['tag_role'] != role: - continue - data = frames[row['image_stamp_ns']]['roles'][role] - item.update(pose_source='verified_fixed_reference', reference_branch_revision=1, - corners_xy=deepcopy(data['corners_xy']), selected_pose=deepcopy(data['selected'])) - item['candidate_diagnostics']['fixed_reference_verified'] = True - return rows, role - - -def test_verified_fixed_pose_preserves_current_corners_and_frozen_training_and_holdout(monkeypatch): - rows, role = verified_fixed_capture(monkeypatch) - before = deepcopy(rows) - result, report = resolve(rows, fixed_roles=(role,)) - assert rows == before and report['frozen_branches_preserved'] - assert report['training_frames'] == 72 and report['holdout_frames'] == 24 - for first, second in zip(rows, result): - if 'joint' in first: - np.testing.assert_allclose(first['relative_quaternion_xyzw'], second['relative_quaternion_xyzw'], atol=1e-12) - - -@pytest.mark.parametrize('cycle', [0, 3]) -@pytest.mark.parametrize('mutation', ['unverified', 'revision', 'corners_changed', 'bad_projection', 'not_fixed']) -def test_fixed_pose_marker_cannot_bypass_live_evidence_checks(monkeypatch, cycle, mutation): - rows, role = verified_fixed_capture(monkeypatch) - row = next(row for row in rows if row.get('joint') == 'thumb_mcp' and row['cycle'] == cycle) - item = row['pnp_observation_evidence']['parent'] - frame = next(row2 for row2 in rows if 'roles' in row2 and row2['image_stamp_ns'] == row['image_stamp_ns']) - if mutation == 'unverified': - item['candidate_diagnostics']['fixed_reference_verified'] = False - elif mutation == 'revision': - item['reference_branch_revision'] += 1 - elif mutation in {'corners_changed', 'bad_projection'}: - item['corners_xy'][0][0] += 10 - if mutation == 'bad_projection': - frame['roles'][role]['corners_xy'] = deepcopy(item['corners_xy']) - with pytest.raises(ValueError, match='frozen_branch_|cached fixed reference'): - resolve(rows, fixed_roles=() if mutation == 'not_fixed' else (role,)) - - -def test_previous_frozen_policy_still_preserves_recorded_branches_when_read_offline(monkeypatch): - rows = frozen_capture(monkeypatch) - rows[0]['pose_tracking_policy_version'] = 'confirmed_branch_v1' - assert resolve(rows)[1]['frozen_branches_preserved'] - - -def test_old_direct_diagnostic_selection_remains_available(monkeypatch): - rows = build_data(monkeypatch) - result, report = resolve(rows) - assert report["changed_joint_rows"] > 0 - assert result != rows - - -@pytest.mark.parametrize("policy", [None, "old_branch_policy", POSE_TRACKING_POLICY_VERSION]) -def test_formal_finalizer_cannot_remove_header_to_bypass_frozen_branch_gate(monkeypatch, tmp_path, policy): - from pathlib import Path - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.artifacts import finalization - rows = build_data(monkeypatch) - if policy is not None: - rows.insert(0, {"kind": "session_start", "pose_tracking_policy_version": policy}) - original_resolver = finalization.resolve_chain_observations - monkeypatch.setattr(finalization, "resolve_chain_observations", - lambda records, **kwargs: original_resolver(records, solve_pose=module.solve_square_tag_ippe, **kwargs)) - monkeypatch.setattr(finalization, "fit_profile_calibration", lambda *args, **kwargs: pytest.fail("fit after branch rejection")) - profile = load_bundled_hand_profile("o12_right_16") - source = Path(__file__).resolve().parents[1]/"urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" - directory = tmp_path / "session" - with pytest.raises(ValueError, match="frozen_branch_"): - finalization.finalize_profile_session(profile=profile, records=rows, - session_dir=directory, serial_number="TEST", source_urdf=source, protected_inputs={}) - report = json.loads((directory / "pose_selection_diagnostics.json").read_text()) - assert report[TASK]["publication_allowed"] is False - assert not list(directory.glob("*.urdf")) - assert not (tmp_path / "latest_passed").exists() diff --git a/src/linkerhand_calibration/test/test_frozen_chain_validation.py b/src/linkerhand_calibration/test/test_frozen_chain_validation.py deleted file mode 100644 index 0df06f9..0000000 --- a/src/linkerhand_calibration/test/test_frozen_chain_validation.py +++ /dev/null @@ -1,135 +0,0 @@ -"""Independent forward prediction cannot absorb wrong commands or zero gauges.""" -from copy import deepcopy -from dataclasses import replace - -import numpy as np -import pytest - -from test_kinematic_chain_diagnostic import linked_fit -from linkerhand_calibration.core.domain.result import JointMapping -from linkerhand_calibration.core.geometry.tag_pose.frozen_chain_validation import FrozenChainCommands - - -@pytest.fixture(scope='module') -def frozen_fixture(linked_fit): - original, fit, _ = linked_fit - bundle = deepcopy(original) - tasks = ('mcp', 'pip', 'dip') - for row, phase in zip(bundle.rows, bundle.phases): - row.update(view='side', task_name=tasks[phase], cycle=0, - command_vector=[0, 0, 0], sample_phase='sweep') - increasing = (0., .5, 1.) - decreasing = (0., .55, 1.) - mappings = tuple(JointMapping(task, phase, 'command_u8', (0., 127., 255.), - increasing, increasing, decreasing) for phase, task in enumerate(tasks)) - shared = fit.x[:bundle.static_width].copy() - references = (.015, -.02, .007) - rows = [] - for phase, task in enumerate(tasks): - for direction in ('increasing', 'decreasing'): - for target in (0., 127., 255.): - row = deepcopy(bundle.rows[0]) - row.update(image_stamp_ns=10000+len(rows), task_name=task, cycle=3, - sample_phase='steady', direction=direction, steady_target=target, - command_vector=[0., 0., 0.]) - row['command_vector'][phase] = target - angle = mappings[phase].evaluate(target, direction)+references[phase] - pixels = bundle.project(shared, np.array([angle]), np.array([phase]))[0] - for role, corners in zip(bundle.roles, pixels.reshape(2, 4, 2)): - row['tags'][role]['corners_xy'] = corners.tolist() - rows.append(row) - def freeze(offsets=references, dependencies=(), first_knots=None): - curves = mappings if first_knots is None else ( - replace(mappings[0], knots=first_knots), *mappings[1:]) - return FrozenChainCommands(bundle, shared, curves, offsets, tasks, - dependency_images=dependencies) - return freeze, rows, bundle, shared - - -def test_frozen_directional_mapping_predicts_without_angle_fit(frozen_fixture, monkeypatch): - freeze, rows, bundle, shared = frozen_fixture - frozen = freeze() - previous = deepcopy(rows) - # The validator must not use either inverse-angle or geometric fitting. - def forbidden(*args, **kwargs): - raise AssertionError('validation attempted to refit') - monkeypatch.setattr(type(bundle), 'validate', forbidden) - monkeypatch.setattr(type(bundle), 'fit', forbidden) - result = frozen.evaluate(rows) - assert max(np.asarray(result.tag_rms_px).ravel()) < 1e-10 - assert rows == previous - assert result.predicted_angles_rad[1] != result.predicted_angles_rad[4] - - -def test_wrong_mapping_or_zero_remains_visible_in_pixels(frozen_fixture): - freeze, rows, _, _ = frozen_fixture - wrong = deepcopy(rows) - # The target changes, but actual pixels do not; inverse-angle fitting - # would hide this disagreement. Forward prediction must retain it. - wrong[1]['command_vector'][0] = wrong[1]['steady_target'] = 180. - assert max(np.asarray(freeze().evaluate(wrong).tag_rms_px).ravel()) > 3. - assert max(np.asarray(freeze(offsets=(.15, -.02, .007)).evaluate(rows).tag_rms_px).ravel()) > 3. - - -@pytest.mark.parametrize('change,reason', [ - (lambda r: r[0].update(cycle=0), 'independent_steady_cycle'), - (lambda r: r[0].update(sample_phase='sweep'), 'independent_steady_cycle'), - (lambda r: r[0].update(view='front'), 'view_changed'), - (lambda r: r[0].update(steady_target=1), 'steady_target_changed'), - (lambda r: r[0]['command_vector'].__setitem__(1, 10), 'held_commands_changed'), - (lambda r: r[0]['camera_matrix'][0].__setitem__(0, 1700.), 'camera_changed'), - (lambda r: r[0]['tags']['parent'].update(tag_size_m=.0165), 'tag_geometry_changed'), - (lambda r: r[0]['command_vector'].__setitem__(0, float('nan')), 'wire_command_invalid'), - (lambda r: r[0]['command_vector'].__setitem__(0, .5), 'wire_command_invalid'), - (lambda r: r[0]['tags']['parent'].update(corners_xy=[[0., 0.]]), 'pixels_invalid'), -]) -def test_frozen_validation_contracts(frozen_fixture, change, reason): - freeze, rows, _, _ = frozen_fixture - changed = deepcopy(rows) - change(changed) - with pytest.raises(ValueError, match=reason): - freeze().evaluate(changed) - - -def test_rejects_selection_leakage_duplicates_and_missing_direction(frozen_fixture): - freeze, rows, _, _ = frozen_fixture - with pytest.raises(ValueError, match='not_independent'): - freeze(dependencies=[rows[0]]).evaluate(rows) - with pytest.raises(ValueError, match='not_independent'): - freeze().evaluate([*rows, rows[0]]) - with pytest.raises(ValueError, match='directions_missing'): - freeze().evaluate([row for row in rows if row['direction'] == 'increasing']) - - -def test_snapshot_isolated_from_later_training_changes(frozen_fixture): - freeze, rows, bundle, shared = frozen_fixture - frozen = freeze() - previous = shared.copy() - shared[3] += .02 - try: - assert max(np.asarray(frozen.evaluate(rows).tag_rms_px).ravel()) < 1e-10 - finally: - shared[:] = previous - - -def test_stage_image_shift_is_not_registered_away(frozen_fixture): - freeze, rows, _, _ = frozen_fixture - changed = deepcopy(rows) - for row in changed: - if row['task_name'] == 'pip': - for tag in row['tags'].values(): - tag['corners_xy'] = (np.asarray(tag['corners_xy'])+[0., 5.]).tolist() - error = np.asarray(freeze().evaluate(changed).tag_rms_px) - assert np.allclose(error[6:12], 5.) - assert np.max(np.abs(error[:6])) < 1e-10 - - -def test_training_holdout_and_missing_mapping_support_rejected(frozen_fixture): - freeze, rows, bundle, _ = frozen_fixture - changed = deepcopy(rows) - changed[0]['image_stamp_ns'] = bundle.rows[0]['image_stamp_ns'] - with pytest.raises(ValueError, match='not_independent'): - freeze().evaluate(changed) - # SDK values outside actual fitted knots cannot be silently clipped. - with pytest.raises(ValueError, match='outside fitted support'): - freeze(first_knots=(10., 127., 250.)).evaluate(rows) diff --git a/src/linkerhand_calibration/test/test_frozen_evidence_storage.py b/src/linkerhand_calibration/test/test_frozen_evidence_storage.py deleted file mode 100644 index 0cd294d..0000000 --- a/src/linkerhand_calibration/test/test_frozen_evidence_storage.py +++ /dev/null @@ -1,84 +0,0 @@ -"""Frozen model sharing must preserve bytes, integrity and image isolation.""" -from copy import deepcopy -from dataclasses import dataclass -import hashlib -import json -import pickle - -import pytest - -from linkerhand_calibration.core.geometry.frozen_evidence import EvidenceDecoder, image_model_evidence - - -@dataclass(frozen=True) -class Model: - camera: tuple = ((1., 0., 0.), (0., 1., 0.), (0., 0., 1.)) - stamps: tuple = tuple(range(100)) - - -def test_live_snapshots_share_only_immutable_model(): - payload = image_model_evidence(Model()) - a = dict(frozen_image_model=payload, image_stamp_ns=1, errors=[.1]) - b = deepcopy(a) - assert b['frozen_image_model'] is payload - b['errors'].append(.2) - assert a['errors'] == [.1] - with pytest.raises(TypeError): payload['stamps'] = () - with pytest.raises(TypeError): payload['camera'][0][0] = 9 - assert pickle.loads(pickle.dumps(payload)) == payload - - -def test_journal_decoder_shares_validated_models_without_changing_json(): - payload = json.loads(json.dumps(image_model_evidence(Model()))) - digest = hashlib.sha256(json.dumps(payload,sort_keys=True,separators=(',', ':')).encode()).hexdigest() - row = dict(frozen_image_model=payload, image_model_sha256=digest, stamp=1) - decoder = EvidenceDecoder() - original = json.dumps(row,sort_keys=True) - first, second = decoder.loads(original), decoder.loads(original) - assert first is not second - assert first['frozen_image_model'] is second['frozen_image_model'] - assert json.dumps(first,sort_keys=True) == original - assert len(decoder.models) == 1 - row['frozen_image_model']['stamps'][0] = 999 - with pytest.raises(ValueError,match='model_hash_invalid'): - decoder.loads(json.dumps(row)) - assert first['frozen_image_model']['stamps'][0] == 0 - - -def test_legacy_plain_records_remain_plain_and_independent(): - decoder = EvidenceDecoder() - one = decoder.loads('{"kind":"joint_sample","state":[1,2]}') - two = decoder.loads('{"kind":"joint_sample","state":[1,2]}') - one['state'][0] = 4 - assert two['state'] == [1,2] - - -def test_passed_resume_defers_hash_checks_without_replacing_conflicting_content(): - decoder = EvidenceDecoder(verify_hashes=False) - row = {'frozen_image_model': {'values': [1, 2]}, 'image_model_sha256': 'same-claim'} - first = decoder.loads(json.dumps(row)) - assert decoder.loads(json.dumps(row))['frozen_image_model'] is first['frozen_image_model'] - row['frozen_image_model']['values'][0] = 9 - changed = decoder.loads(json.dumps(row)) - assert changed['frozen_image_model']['values'] == (9, 2) - assert first['frozen_image_model']['values'] == (1, 2) - with pytest.raises(ValueError, match='model_hash_invalid'): - EvidenceDecoder().loads(json.dumps(changed)) - - -def test_cached_model_is_content_bound_even_after_a_mutable_input_changes(): - from linkerhand_calibration.core.geometry.frozen_evidence import freeze_evidence - from linkerhand_calibration.core.geometry.image_motion_replay import frozen_image_model, image_model_sha256 - from test_image_motion_provenance import recorded_image - - frame, _ = recorded_image() - item = frame['roles']['moving'] - model = frozen_image_model(item) - frozen = freeze_evidence(item['frozen_image_model']) - assert frozen_image_model({**item, 'frozen_image_model': frozen}) is model - assert image_model_sha256(frozen) == item['image_model_sha256'] - item['frozen_image_model']['geometry'][0]['pivot_parent_xyz_m'] = (.2, .2, .2) - with pytest.raises(ValueError, match='model_hash_invalid'): - frozen_image_model(item) - item['image_model_sha256'] = image_model_sha256(item['frozen_image_model']) - assert frozen_image_model(item) != model diff --git a/src/linkerhand_calibration/test/test_frozen_rotation_reference.py b/src/linkerhand_calibration/test/test_frozen_rotation_reference.py deleted file mode 100644 index 601d6fa..0000000 --- a/src/linkerhand_calibration/test/test_frozen_rotation_reference.py +++ /dev/null @@ -1,47 +0,0 @@ -import math - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting.spatial import ( - fit_rotation_joint_curve, rotation_curve_holdout_errors, -) - - -def rows(cycles, *, slip=0.0, transverse=False): - mounting = Rotation.from_euler("xyz", (0.2, -0.3, 0.1)) - result = [] - for cycle in cycles: - for direction in ("decreasing", "increasing"): - for command in range(256): - angle = (255 - command)/255 + (0.02 if direction == "increasing" else 0) - rotation = mounting * Rotation.from_euler("z", angle) - if slip: - rotation = rotation * Rotation.from_euler("x" if transverse else "z", slip) - result.append({"cycle": cycle, "direction": direction, "command_u8": command, - "relative_quaternion_xyzw": rotation.as_quat().tolist()}) - return result - - -@pytest.mark.parametrize("transverse", [False, True]) -def test_live_shared_fitter_does_not_rezero_a_slipped_holdout_tag(transverse): - fit = fit_rotation_joint_curve(rows((0, 1, 2)), zero_command_u8=255, - canonical_zero_direction="decreasing") - clean = rotation_curve_holdout_errors(fit, rows((3,)), zero_command_u8=255) - assert np.max(np.abs(clean)) < 1e-7 - slipped = rotation_curve_holdout_errors(fit, - rows((3,), slip=math.radians(5), transverse=transverse), zero_command_u8=255) - assert np.min(np.abs(slipped)) > math.radians(4.9) - - -def test_live_shared_fitter_rejects_training_as_holdout(): - fit = fit_rotation_joint_curve(rows((0, 1, 2)), zero_command_u8=255) - with pytest.raises(ValueError, match="training cycles"): - rotation_curve_holdout_errors(fit, rows((2,)), zero_command_u8=255) - - -def test_two_directions_keep_the_same_training_reference(): - fit = fit_rotation_joint_curve(rows((0, 1, 2)), zero_command_u8=255) - assert fit.increasing_rad[-1] - fit.decreasing_rad[-1] == pytest.approx(0.02, abs=1e-7) - assert max(abs(v) for v in rotation_curve_holdout_errors(fit, rows((3,)), zero_command_u8=255)) < 1e-7 diff --git a/src/linkerhand_calibration/test/test_frozen_tag_replay.py b/src/linkerhand_calibration/test/test_frozen_tag_replay.py deleted file mode 100644 index 0b7bfc8..0000000 --- a/src/linkerhand_calibration/test/test_frozen_tag_replay.py +++ /dev/null @@ -1,138 +0,0 @@ -"""Independent rigid-body generator: never uses the fitted FK to make truth.""" - -from dataclasses import replace -import hashlib -import json - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting.tag_installation import ( - TagTrainingPose, fit_tag_installations, matrix_tuple, -) -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan -from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout, validate_serialized_tag_holdout -from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher, FrozenTagArtifactValidator - - -def transform(rpy=(0, 0, 0), xyz=(0, 0, 0)): - value = np.eye(4) - value[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() - value[:3, 3] = xyz - return value - - -def fixture(tmp_path, unit="rad"): - source = tmp_path / "raw.urdf" - source.write_text(''' - - - - - - - - ''') - zeros = {"drive": 0.07, "passive": -0.03} - rights = {"drive": ("origin.rpy", "limit.lower", "limit.upper"), - "passive": ("origin.rpy", "limit.lower", "limit.upper", "mimic.offset")} - digest = hashlib.sha256(source.read_bytes()).hexdigest() - plan = build_standard_correction_plan(source_urdf=source, source_sha256=digest, - zero_offsets_rad=zeros, authorized_fields=rights) - base = transform((0.13, -0.22, 0.19), (0.1, 0.2, 0.3)) - mounts = {"a": transform((0.3, 0.2, -0.4), (0.003, 0.012, 0.018)), - "b": transform((-0.4, 0.3, 0.2), (-0.013, 0.009, 0.016))} - training, holdout = [], [] - for cycle in range(4): - for direction in ("increasing", "decreasing"): - for i, phase in enumerate(np.linspace(0, 1, 80)): - sdk = phase if unit == "rad" else round(phase*255) - x = sdk if unit == "rad" else sdk/255 - # Known independent coordinates including direction-dependent - # backlash. Neither source-model FK nor the fitted curve makes truth. - cad = 0.2 + 0.8*x + (0.006 if direction == "increasing" else -0.006) - child = 0.6*cad + 0.04 - proximal = transform((0.2, -0.1, 0.3), (0.02, -0.01, 0.03)) @ transform((0, cad, 0)) - distal = proximal @ transform((-0.1, 0.2, 0.1), (0.01, 0, 0.04)) @ transform((child, 0, 0)) - for role, pose in (("a", proximal), ("b", distal)): - image = f"camera:{cycle}:{direction}:{i}" - observed = matrix_tuple(base @ pose @ mounts[role]) - if cycle < 3: - training.append(TagTrainingPose(image, role, cycle, observed, {"drive": cad, "passive": child})) - else: - holdout.append(TagHoldout(image, role, cycle, (sdk,), {"drive": direction}, observed)) - frozen = fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, - link_by_role={"a": "proximal", "b": "distal"}, observations=training) - knots = np.linspace(0, 1 if unit == "rad" else 255, 65 if unit == "rad" else 256) - angles = 0.2 + 0.8*knots/(1 if unit == "rad" else 255) - zeros["drive"] - row = {"urdf_joint": "drive", "motor_index": 0, "angle_rad": angles.tolist(), - "increasing_rad": (angles+0.006).tolist(), "decreasing_rad": (angles-0.006).tolist()} - if unit == "rad": - row["curve_input_knots_rad"] = knots.tolist() - payload = {"profile_id": "VIRTUAL/right/new/v1", "schema_version": 7 if unit == "rad" else 6, - "curve_input_domain": "feedback_" + unit, "joints": {"drive": row}} - return source, plan, base, training, frozen, holdout, payload - - -@pytest.mark.parametrize("unit", ["rad", "u8"]) -def test_written_standard_urdf_replays_independent_poses_and_publishes(tmp_path, unit): - source, plan, base, _, mounts, holdout, payload = fixture(tmp_path, unit) - session = tmp_path / "session" - session.mkdir() - urdf = plan.write(source, session / "corrected.urdf") - calibration = session / "calibration.json" - calibration.write_text(json.dumps(payload)) - loaded = [] - validator = FrozenTagArtifactValidator(payload["profile_id"], source, plan.source_sha256, - plan.authorized_fields, plan.zero_offsets_rad, matrix_tuple(base), mounts, - tuple(holdout), ("a", "b"), lambda path: loaded.append(path)) - release = ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, - calibration_json=calibration, corrected_urdf=urdf, validate=validator) - assert release.pointer.resolve() == session - evidence = json.loads(release.manifest.read_text())["validation"] - assert evidence["final_file_spatial_replay"] == "passed" - assert evidence["tag_holdout"]["b"]["maximum_deg"] < 1e-6 - assert loaded == [urdf] - - -@pytest.mark.parametrize("corruption", ["mimic", "origin", "json_zero", "tag_slip"]) -def test_final_file_or_tag_errors_cannot_be_absorbed_into_new_mounts(tmp_path, corruption): - source, plan, base, _, mounts, holdout, payload = fixture(tmp_path) - corrected = plan.write(source, tmp_path / "corrected.urdf") - if corruption == "mimic": - corrected.write_text(corrected.read_text().replace('multiplier="0.6"', 'multiplier="0.8"')) - elif corruption == "origin": - import xml.etree.ElementTree as ET - tree = ET.parse(corrected) - tree.find("joint/origin").set("rpy", "0.4 0.1 0.3") - tree.write(corrected) - elif corruption == "json_zero": - for name in ("angle_rad", "increasing_rad", "decreasing_rad"): - payload["joints"]["drive"][name] = [v + 0.1 for v in payload["joints"]["drive"][name]] - else: - holdout = [replace(row, common_from_tag=matrix_tuple( - np.asarray(row.common_from_tag) @ transform((0.1, 0, 0)))) for row in holdout] - with pytest.raises(ValueError, match="holdout_failed"): - validate_serialized_tag_holdout(corrected_urdf=corrected, payload=payload, - common_from_base=base, installations=mounts, observations=holdout, required_roles=("a", "b")) - - -def test_installation_is_unique_across_cycles_and_holdout_never_fits(tmp_path): - source, _, base, training, mounts, holdout, _ = fixture(tmp_path) - with pytest.raises(ValueError, match="training"): - fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, - link_by_role={"a": "proximal", "b": "distal"}, observations=[replace(training[0], cycle=3)]) - slipped = [replace(row, common_from_tag=matrix_tuple(np.asarray(row.common_from_tag) @ - transform((0.12, 0, 0)))) if row.cycle == 2 else row for row in training] - from linkerhand_calibration.core.fitting.tag_installation import TagInstallationFailure - with pytest.raises(TagInstallationFailure, match="tag_installation_not_rigid") as failure: - fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, - link_by_role={"a": "proximal", "b": "distal"}, observations=slipped) - assert set(failure.value.diagnostics) == {"a", "b"} - assert all(not row["passed"] for row in failure.value.diagnostics.values()) - # Same physical Tag observed through two tasks is deduplicated, not fitted twice. - duplicate = fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, - link_by_role={"a": "proximal", "b": "distal"}, observations=training+training) - assert duplicate == mounts diff --git a/src/linkerhand_calibration/test/test_full_hand.py b/src/linkerhand_calibration/test/test_full_hand.py deleted file mode 100644 index c6b5b6a..0000000 --- a/src/linkerhand_calibration/test/test_full_hand.py +++ /dev/null @@ -1,1092 +0,0 @@ -import math -from dataclasses import replace -from pathlib import Path - -import numpy as np -import pytest - -from linkerhand_calibration.full_hand import ( - ACTIVE_JOINTS, - IMAGE_TRAJECTORY_JOINTS, - JOINT_SPECS, - JointCurveFit, - LEFT_HAND_PROFILE, - MEASURED_JOINTS, - PASSIVE_JOINTS, - RIGHT_HAND_PROFILE, - RIGHT_19_HAND_PROFILE, - SPLAY_JOINTS, - SWEEP_SPECS, - THREE_CAMERA_BASELINE_COMMAND, - VIEW_TAGS, - build_calibration_motion_command, - build_calibration_preparation_waypoints, - build_calibration_return_waypoints, - build_calibration_speed_profile, - build_compact_payload, - build_full_hand_command, - clamp_runtime_fits_to_urdf_limits, - center_splay_curve, - compare_cross_view_roll_curves, - cross_view_roll_diagnostic_metrics, - derive_mimic_passive_fits, - fit_joint_center_curve, - fit_joint_image_curve, - fit_measured_joint_curve, - fit_projected_zero, - get_hand_calibration_profile, - measure_joint_observation, - validate_compact_payload, -) - - -def test_runtime_fit_saturates_at_source_urdf_limit() -> None: - values = tuple(np.linspace(0.0, 2.0, 256)) - fit = JointCurveFit( - angle_rad=values, - decreasing_rad=values, - increasing_rad=values, - circle={}, - maximum_monotonic_correction_rad=0.0, - maximum_hysteresis_rad=0.0, - quality={}, - ) - - bounded = clamp_runtime_fits_to_urdf_limits( - Path(__file__).resolve().parents[1] - / "urdf/g20_right/linkerhand_g20_right.urdf", - {"thumb_cmc_yaw": fit}, - )["thumb_cmc_yaw"] - - assert max(bounded.angle_rad) == pytest.approx(1.57) - assert max(bounded.decreasing_rad) == pytest.approx(1.57) - assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0 - - -def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None: - profile = get_hand_calibration_profile("right", "g20_right_19") - assert profile is RIGHT_19_HAND_PROFILE - assert len(profile.sweep_specs) == 16 - assert len(profile.measured_joints) == 21 - assert len(profile.record_joints) == 25 - assert list(profile.view_tags["front"].values()) == [ - 0, 1, 2, 3, 10, 11, 12, 13 - ] - assert list(profile.view_tags["side"].values()) == [ - 4, 5, 6, 7, 14, 15, 16, 17, 18 - ] - assert list(profile.view_tags["top"].values()) == [8, 9] - assert profile.preflight_view_roles == { - "front": ("front_base",), - "side": ("side_base",), - "top": ("top_base",), - } - assert [spec.task_name for spec in profile.sweep_specs[:4]] == [ - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - "thumb_mcp_ip_front", - "thumb_cmc_yaw_top", - ] - assert [ - spec.task_name for spec in profile.sweep_specs[4:7] - ] == [ - "pinky_roll_multiview", - "pinky_pitch_side", - "pinky_pip_side", - ] - assert profile.sweep_specs[4].joints == ( - "pinky_mcp_roll", - "pinky_mcp_roll_side", - ) - assert { - ( - observer.source_name, - observer.task_name, - observer.view, - observer.model_joint, - observer.motor_index, - ) - for observer in profile.palm_axis_observers - } == { - ( - "thumb_cmc_pitch_top_axis", - "thumb_cmc_pitch_front", - "top", - "thumb_cmc_pitch", - 0, - ), - ( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll_front", - "top", - "thumb_cmc_roll", - 5, - ), - } - assert profile.minimum_palm_orientation_sources == 2 - assert profile.palm_orientation_minimum_arc_rad == pytest.approx( - math.radians(10.0) - ) - assert profile.palm_orientation_maximum_command_distance_u8 == 64 - assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip") - assert { - name: spec.source_joint - for name, spec in profile.joint_specs.items() - if spec.source_joint is not None - } == {} - - -def test_right_19_keeps_four_visual_dip_curves_independent() -> None: - profile = RIGHT_19_HAND_PROFILE - values = tuple(0.002 * (255 - command) for command in range(256)) - fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {}) - measured = {name: fit for name in profile.measured_joints} - dip_values = tuple(0.0013 * (255 - command) for command in range(256)) - for finger in ("index", "middle", "ring", "pinky"): - measured[f"{finger}_dip"] = JointCurveFit( - dip_values, - dip_values, - dip_values, - {"visual_measurement": True}, - 0.0, - 0.007, - {"visual_measurement": 1.0}, - ) - source_urdf = ( - Path(__file__).resolve().parents[1] - / "urdf/g20_right/linkerhand_g20_right.urdf" - ) - - runtime = derive_mimic_passive_fits( - source_urdf, measured, profile=profile - ) - - assert set(runtime) == set(profile.joint_specs) - for finger in ("index", "middle", "ring", "pinky"): - dip = runtime[f"{finger}_dip"] - assert dip.angle_rad == pytest.approx(dip_values) - assert dip.maximum_hysteresis_rad == pytest.approx(0.007) - assert dip.circle["visual_measurement"] is True - - -def _changed_motors( - previous: tuple[int, ...] | list[int], waypoint: tuple[int, ...] -) -> list[int]: - return [ - index - for index, (left, right) in enumerate(zip(previous, waypoint)) - if left != right - ] - - -def _phase_changes( - previous: tuple[int, ...] | list[int], - waypoints: tuple[tuple[int, ...], ...], -) -> list[list[int]]: - phases: list[list[int]] = [] - for waypoint in waypoints: - phases.append(_changed_motors(previous, waypoint)) - previous = waypoint - return phases - - -def test_right_19_side_clearance_waypoints_group_joint_classes() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs - if item.task_name == "index_roll_multiview" - ) - baseline = [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, - 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] - waypoints = build_calibration_preparation_waypoints( - spec, - 255, - current_command=baseline, - profile=profile, - ) - # Side clearance keeps all neighbouring roll axes neutral, flexes the - # neighbour PIPs together, then their pitches, and moves the index target - # last through the cleared swept volume. - assert _phase_changes(baseline, waypoints) == [ - [17, 18, 19], [2, 3, 4], [6], - ] - assert waypoints[-1][1] == 255 and waypoints[-1][16] == 255 - - returns = build_calibration_return_waypoints( - baseline, - current_command=waypoints[-1], - profile=profile, - ) - assert _phase_changes(waypoints[-1], returns) == [ - [6], [2, 3, 4], [17, 18, 19], - ] - assert list(returns[-1]) == baseline - - -def test_right_19_side_clearance_waypoints_move_one_motor_at_a_time() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs - if item.task_name == "index_roll_multiview" - ) - waypoints = build_calibration_preparation_waypoints( - spec, - 255, - current_command=[255, 255, 255, 255, 255, 255, 127, 127, 127, 127, - 255, 255, 255, 255, 255, 255, 255, 255, 255, 255], - profile=profile, - parallel=False, - ) - previous = tuple([255] * 6 + [127] * 4 + [255] * 10) - changed_motors = [] - for waypoint in waypoints: - changed = _changed_motors(previous, waypoint) - assert len(changed) == 1 - changed_motors.append(changed[0]) - previous = waypoint - # Side clearance keeps all neighbouring roll axes neutral, flexes them out - # of the swept volume, and moves the index target last. - assert changed_motors[:7] == [19, 4, 18, 3, 17, 2, 6] - assert previous[1] == 255 and previous[16] == 255 - - returns = build_calibration_return_waypoints( - [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, - 255, 255, 255, 255, 255, 255, 255, 255, 255, 255], - current_command=previous, - profile=profile, - parallel=False, - ) - for waypoint in returns: - changed = sum(left != right for left, right in zip(previous, waypoint)) - assert changed == 1 - previous = waypoint - - -def test_right_15_ring_multiview_keeps_pinky_roll_neutral_and_flexes_it() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs - if item.task_name == "ring_roll_multiview" - ) - current = tuple(THREE_CAMERA_BASELINE_COMMAND) - - waypoints = build_calibration_preparation_waypoints( - spec, - 255, - current_command=current, - profile=profile, - ) - - assert _phase_changes(current, waypoints) == [[6, 7], [19], [4], [8]] - assert all(waypoint[9] == 127 for waypoint in waypoints) - assert waypoints[-1][8] == 255 - assert waypoints[-1][19] == 0 - assert waypoints[-1][4] == 0 - - -def test_middle_multiview_recovers_neighbour_from_observed_58_not_zero() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs - if item.task_name == "middle_roll_multiview" - ) - # Session 20260819_160238 stopped here: the legacy front-only avoidance - # asked neighbouring ring roll (motor 8) for 0 and it saturated at 58. - current = [254] * 20 - current[6:10] = [254, 127, 58, 127] - current[10] = 252 - current[11:15] = [0, 0, 0, 0] - - waypoints = build_calibration_preparation_waypoints( - spec, - 255, - current_command=current, - profile=profile, - ) - - # Both differing roll axes (parked motor 6 and the stuck motor 8) move in - # the single roll phase, still never commanded toward 0. - phases = _phase_changes(current, waypoints) - assert phases[0] == [6, 8] - assert waypoints[0][8] == 127 - assert all( - waypoint[motor] != 0 - for waypoint in waypoints - for motor in range(6, 10) - ) - assert waypoints[-1][6:10] == (255, 255, 127, 127) - - -def test_right_19_ring_pitch_keeps_target_neutral_during_clearance() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs - if item.task_name == "ring_pitch_side" - ) - current = tuple(THREE_CAMERA_BASELINE_COMMAND) - - waypoints = build_calibration_preparation_waypoints( - spec, - 255, - current_command=current, - profile=profile, - ) - - assert _phase_changes(current, waypoints) == [[6, 7], [19], [4]] - assert all(waypoint[8] == 127 for waypoint in waypoints) - assert all(waypoint[9] == 127 for waypoint in waypoints) - assert waypoints[-1][9] == 127 - assert waypoints[-1][19] == 0 - assert waypoints[-1][4] == 0 - - -def test_right_19_waypoints_never_wait_for_reserved_feedback_channels() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = profile.sweep_specs[0] - # This is the real G20 feedback shape: reserved channels 11..14 report zero - # while active endpoint channels commonly settle one or two counts low. - current = [ - 255, 254, 254, 254, 254, 254, 127, 127, 127, 127, - 253, 0, 0, 0, 0, 255, 253, 253, 253, 253, - ] - waypoints = build_calibration_preparation_waypoints( - spec, - 255, - current_command=current, - profile=profile, - ) - assert waypoints - assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in waypoints) - controlled = {joint.motor_index for joint in profile.joint_specs.values()} - for previous, waypoint in zip((tuple(current), *waypoints[:-1]), waypoints): - assert set(_changed_motors(previous, waypoint)) <= controlled - - returns = build_calibration_return_waypoints( - list(THREE_CAMERA_BASELINE_COMMAND), - current_command=waypoints[-1], - profile=profile, - ) - assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in returns) - - -def test_right_19_parallel_return_moves_rolls_together_then_fingers() -> None: - # Full four-finger avoidance pose: rolls parked at 255 and every clearance - # finger flexed to 0. - current = list(THREE_CAMERA_BASELINE_COMMAND) - current[6:10] = [255, 255, 255, 255] - for motor in (1, 2, 3, 4, 16, 17, 18, 19): - current[motor] = 0 - returns = build_calibration_return_waypoints( - list(THREE_CAMERA_BASELINE_COMMAND), - current_command=current, - profile=RIGHT_19_HAND_PROFILE, - anchor_roll_motor=9, - ) - assert _phase_changes(current, returns) == [ - [6, 7, 8, 9], [1, 2, 3, 4], [16, 17, 18, 19], - ] - assert list(returns[-1]) == list(THREE_CAMERA_BASELINE_COMMAND) - - -def test_right_19_pinky_exit_returns_target_roll_before_other_fingers() -> None: - current = list(THREE_CAMERA_BASELINE_COMMAND) - current[6:10] = [255, 255, 255, 255] - returns = build_calibration_return_waypoints( - list(THREE_CAMERA_BASELINE_COMMAND), - current_command=current, - profile=RIGHT_19_HAND_PROFILE, - anchor_roll_motor=9, - parallel=False, - ) - phases = _phase_changes(current, returns) - assert all(len(phase) == 1 for phase in phases) - assert [motor for phase in phases for motor in phase][:4] == [9, 8, 7, 6] - - -def test_right_19_ring_exit_returns_ring_before_neighbours() -> None: - current = list(THREE_CAMERA_BASELINE_COMMAND) - current[6:10] = [255, 255, 255, 0] - returns = build_calibration_return_waypoints( - list(THREE_CAMERA_BASELINE_COMMAND), - current_command=current, - profile=RIGHT_19_HAND_PROFILE, - anchor_roll_motor=8, - parallel=False, - ) - - first = returns[0] - assert first[8] == 127 - assert first[6:8] == (255, 255) - assert first[9] == 0 - - -def test_cross_view_roll_curve_must_agree_before_fusion() -> None: - values = tuple(0.5 * (127 - command) / 127 for command in range(256)) - fit = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) - assert compare_cross_view_roll_curves(fit, fit)[ - "angle_rad_rms_difference_rad" - ] == 0.0 - bad_values = tuple(value + 0.03 * (command != 127) for command, value in enumerate(values)) - bad = replace(fit, angle_rad=bad_values, decreasing_rad=bad_values, - increasing_rad=bad_values) - with pytest.raises(ValueError, match="cross_view_roll_curve"): - compare_cross_view_roll_curves(fit, bad) - - reverse = replace( - fit, - increasing_rad=tuple(value + math.radians(0.5) for value in values), - ) - with pytest.raises(ValueError, match="branch_gap_difference"): - compare_cross_view_roll_curves(fit, reverse) - - -def test_cross_view_roll_curve_allows_bounded_projection_scale() -> None: - values = tuple(0.5 * (127 - command) / 127 for command in range(256)) - primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) - projected = tuple(0.8 * value for value in values) - secondary = replace( - primary, - angle_rad=projected, - decreasing_rad=projected, - increasing_rad=projected, - ) - - with pytest.raises(ValueError, match="cross_view_roll_curve"): - compare_cross_view_roll_curves(primary, secondary) - - metrics = compare_cross_view_roll_curves( - primary, - secondary, - allow_projection_scale=True, - ) - - assert metrics["projection_scale"] == pytest.approx(1.25) - assert metrics["angle_rad_rms_difference_rad"] == pytest.approx(0.0) - assert metrics["raw_angle_rad_rms_difference_rad"] > 0.0 - - -def test_projected_cross_view_keeps_branch_gap_as_diagnostic() -> None: - values = tuple(0.5 * (127 - command) / 127 for command in range(256)) - primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) - shifted_increasing = tuple( - value + math.radians(0.5) for value in values - ) - secondary = replace(primary, increasing_rad=shifted_increasing) - - metrics = compare_cross_view_roll_curves( - primary, - secondary, - allow_projection_scale=True, - ) - - assert metrics["baseline_branch_gap_difference_rad"] > math.radians(0.3) - assert metrics["baseline_branch_gap_gate_applied"] == 0.0 - - -def test_cross_view_roll_curve_rejects_implausible_projection_scale() -> None: - values = tuple(0.5 * (127 - command) / 127 for command in range(256)) - primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) - collapsed = tuple(0.2 * value for value in values) - secondary = replace( - primary, - angle_rad=collapsed, - decreasing_rad=collapsed, - increasing_rad=collapsed, - ) - - with pytest.raises(ValueError, match="projection_scale_ratio"): - compare_cross_view_roll_curves( - primary, - secondary, - allow_projection_scale=True, - ) - - -def test_validation_only_cross_view_metrics_never_create_a_retry_gate() -> None: - values = tuple(0.5 * (127 - command) / 127 for command in range(256)) - primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) - biased = tuple( - value - + math.radians(2.5) * math.sin(math.pi * command / 255.0) - for command, value in enumerate(values) - ) - secondary = replace( - primary, - angle_rad=biased, - decreasing_rad=biased, - increasing_rad=biased, - ) - - metrics = cross_view_roll_diagnostic_metrics(primary, secondary) - - assert metrics["angle_rad_rms_difference_rad"] > math.radians(1.0) - - reversed_secondary = replace( - secondary, - angle_rad=tuple(-value for value in secondary.angle_rad), - decreasing_rad=tuple(-value for value in secondary.decreasing_rad), - increasing_rad=tuple(-value for value in secondary.increasing_rad), - ) - assert cross_view_roll_diagnostic_metrics( - primary, reversed_secondary - ) == {"direction_disagrees": 1.0} - - -def _records() -> list[dict[str, object]]: - commands = list(range(0, 256, 16)) - if commands[-1] != 255: - commands.append(255) - records: list[dict[str, object]] = [] - centre = np.asarray([0.006, -0.004, 0.012]) - radius = 0.025 - image_centre = np.asarray([30.0, -12.0]) - image_radius = 100.0 - for cycle in range(3): - for direction, sequence in ( - ("decreasing", reversed(commands)), - ("increasing", commands), - ): - for command in sequence: - angle = 0.70 * (255.0 - command) / 255.0 - point = centre + np.asarray( - [radius * math.cos(angle), radius * math.sin(angle), 0.0] - ) - # At command 255 the inward vector points along image +x, so - # table_projected_zero_rad is exactly zero. - image_point = image_centre + np.asarray( - [ - -image_radius * math.cos(angle), - image_radius * math.sin(angle), - ] - ) - records.append( - { - "cycle": cycle, - "direction": direction, - "command_u8": command, - "relative_translation_xyz_m": point.tolist(), - "image_relative_xy_px": image_point.tolist(), - } - ) - return records - - -def test_joint_layout_covers_16_active_and_5_passive_joints() -> None: - assert len(JOINT_SPECS) == 21 - assert len(ACTIVE_JOINTS) == 16 - assert len(PASSIVE_JOINTS) == 5 - - -def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None: - profile = get_hand_calibration_profile("right") - assert profile is RIGHT_HAND_PROFILE - assert profile.reference_finger == "pinky" - assert [spec.motor_index for spec in profile.sweep_specs] == [ - 0, 5, 15, 9, 4, 19, 10 - ] - assert profile.view_tags["front"]["pinky_roll"] == 10 - assert profile.view_tags["side"] == { - "side_base": 4, - "pinky_mcp": 5, - "pinky_pip": 6, - "pinky_dip": 7, - } - assert profile.preflight_view_roles["side"] == ( - "side_base", - "pinky_mcp", - "pinky_pip", - "pinky_dip", - ) - thumb_pitch = profile.joint_specs["thumb_cmc_pitch"] - assert thumb_pitch.view == "front" - assert thumb_pitch.parent_role == "front_base" - assert thumb_pitch.child_role == "thumb_cmc" - assert profile.joint_specs["index_mcp_roll"].source_joint == ( - "pinky_mcp_roll" - ) - assert profile.joint_specs["middle_mcp_pitch"].source_joint == ( - "pinky_mcp_pitch" - ) - assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip" - assert profile.joint_specs["index_dip"].source_joint == "pinky_dip" - - roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9) - command = build_calibration_motion_command( - roll, 127, profile=profile - ) - assert command[9] == 127 - assert command[6:9] == [255, 255, 255] - speeds = build_calibration_speed_profile( - roll, - normal_speed=15, - index_roll_speed=5, - index_flex_speed=10, - profile=profile, - ) - assert speeds == [15, 15, 15, 15, 5] - assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set( - range(11) - ) - assert VIEW_TAGS["front"]["index_roll"] == 10 - assert VIEW_TAGS["side"] == { - "side_base": 4, - "index_mcp": 5, - "index_pip": 6, - "index_dip": 7, - } - assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9} - assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10] - - -def test_joint_trajectory_spaces_match_observation_geometry() -> None: - assert IMAGE_TRAJECTORY_JOINTS == { - "thumb_cmc_pitch", - "thumb_cmc_roll", - "thumb_mcp", - "thumb_ip", - "index_mcp_roll", - "index_mcp_pitch", - "index_pip", - } - records = _records() - for name in IMAGE_TRAJECTORY_JOINTS: - fit = fit_measured_joint_curve(name, records) - assert fit.circle["space"] == "image_2d" - for name in ("index_dip", "thumb_cmc_yaw"): - assert "space" not in fit_measured_joint_curve(name, records).circle - - -def test_full_hand_command_changes_exactly_one_controlled_motor() -> None: - result = build_full_hand_command(6, 27) - assert result[6] == 27 - assert result[:6] == [255] * 6 - with pytest.raises(ValueError, match="controlled"): - build_full_hand_command(11, 27) - - -def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None: - baseline = [255] * 20 - baseline[6:10] = [127, 127, 127, 127] - index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6) - - result = build_calibration_motion_command(index_roll, 27, baseline) - - assert result[6:10] == [27, 0, 0, 0] - assert result[:6] == baseline[:6] - assert result[10:] == baseline[10:] - - -def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None: - profile = RIGHT_HAND_PROFILE - pinky_roll = next( - spec for spec in profile.sweep_specs if spec.motor_index == 9 - ) - - result = build_calibration_motion_command( - pinky_roll, 27, profile=profile - ) - - assert result[6:10] == [255, 255, 255, 27] - assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255} - - -def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None: - baseline = [255] * 20 - baseline[6:10] = [127, 127, 127, 127] - thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0) - - result = build_calibration_motion_command(thumb_pitch, 17, baseline) - - assert result[0] == 17 - assert result[6:10] == [127, 127, 127, 127] - - -def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None: - profile = RIGHT_HAND_PROFILE - thumb_pitch = next( - spec for spec in profile.sweep_specs if spec.motor_index == 0 - ) - - result = build_calibration_motion_command( - thumb_pitch, 17, profile=profile - ) - - assert result[0] == 17 - assert result[10] == 255 - assert result[5] == 255 - assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255} - - -def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None: - thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0) - - result = build_calibration_motion_command(thumb_pitch, 17) - - assert result[0] == 17 - assert result[5] == 255 - assert result[10] == 255 - assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {} - - -def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None: - baseline = [255] * 20 - baseline[6:10] = [127, 127, 127, 127] - thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10) - - result = build_calibration_motion_command(thumb_yaw, 27, baseline) - - assert result[5] == 145 - assert result[10] == 27 - assert result[:5] == baseline[:5] - assert result[6:10] == baseline[6:10] - assert result[11:] == baseline[11:] - - -def test_only_index_roll_uses_the_slow_index_finger_speed() -> None: - index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6) - index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1) - - assert build_calibration_speed_profile( - index_roll, - normal_speed=15, - index_roll_speed=5, - index_flex_speed=10, - ) == [15, 5, 15, 15, 15] - assert build_calibration_speed_profile( - index_pitch, - normal_speed=15, - index_roll_speed=5, - index_flex_speed=10, - ) == [15, 10, 15, 15, 15] - - index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16) - assert build_calibration_speed_profile( - index_pip, - normal_speed=15, - index_roll_speed=5, - index_flex_speed=10, - ) == [15, 10, 15, 15, 15] - - thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0) - assert build_calibration_speed_profile( - thumb_pitch, - normal_speed=15, - index_roll_speed=5, - index_flex_speed=10, - ) == [15, 15, 15, 15, 15] - - -def test_calibration_speed_profile_rejects_out_of_range_values() -> None: - index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6) - with pytest.raises(ValueError, match="speeds"): - build_calibration_speed_profile( - index_roll, - normal_speed=15, - index_roll_speed=256, - index_flex_speed=10, - ) - - -def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None: - records = _records() - fit = fit_joint_center_curve(records) - assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8) - assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4) - assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8) - assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6) - - -def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None: - records = _records() - for record in records: - command = int(record["command_u8"]) - record["relative_translation_xyz_m"] = [ - 0.001 * command, - 0.0, - 0.0, - ] - - fit = fit_measured_joint_curve("thumb_mcp", records) - - assert fit.circle["space"] == "image_2d" - assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02) - assert fit.angle_rad[255] == pytest.approx(0.0) - assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01 - assert math.degrees(fit.maximum_hysteresis_rad) < 0.01 - - command_zero = next( - record for record in records if int(record["command_u8"]) == 0 - ) - observed = measure_joint_observation( - fit, - vector_xyz_m=command_zero["relative_translation_xyz_m"], - image_vector_xy_px=command_zero["image_relative_xy_px"], - ) - assert observed == pytest.approx(0.70, abs=0.02) - - -def test_image_curve_rejects_insufficient_projected_arc() -> None: - records = _records() - for record in records: - command = float(record["command_u8"]) - angle = math.radians(2.0) * (255.0 - command) / 255.0 - record["image_relative_xy_px"] = [ - 100.0 * math.cos(angle), - 100.0 * math.sin(angle), - ] - - with pytest.raises( - ValueError, match="joint_image_trajectory_quality_failed:arc" - ): - fit_joint_image_curve(records) - - -def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None: - fit = fit_joint_center_curve(_records()) - centred, zero_command, midpoint = center_splay_curve(fit) - assert midpoint == pytest.approx(0.35, abs=1.0e-4) - assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4) - assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4) - assert zero_command in {127, 128} - assert abs(centred.angle_rad[zero_command]) <= 0.002 - - -def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None: - base = fit_joint_center_curve(_records()) - splay, zero_command, _ = center_splay_curve(base) - splay = replace( - splay, - angle_rad=tuple( - value - splay.angle_rad[zero_command] - for value in splay.angle_rad - ), - ) - measured = { - name: splay if name == "index_mcp_roll" else base - for name in MEASURED_JOINTS - } - offsets = {name: 0.01 for name in ACTIVE_JOINTS} - baseline = [255] * 20 - baseline[6:10] = [zero_command] * 4 - payload = build_compact_payload( - serial_number="G20_LEFT_001", - measured_fits=measured, - urdf_zero_offsets_rad=offsets, - validation_errors_rad=[0.01, -0.02], - passed=True, - baseline=baseline, - ) - validate_compact_payload(payload) - assert set(payload) == { - "schema_version", - "model", - "side", - "serial_number", - "angle_unit", - "command_range", - "baseline_command_u8", - "joints", - "quality", - } - assert len(payload["joints"]) == 21 - assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values()) - assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16 - assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5 - assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll" - assert ( - payload["joints"]["middle_mcp_roll"]["angle_rad"] - == payload["joints"]["index_mcp_roll"]["angle_rad"] - ) - assert payload["joints"]["index_mcp_roll"]["zero_angles"] == { - "urdf_zero_offset_rad": pytest.approx(0.01) - } - for name in SPLAY_JOINTS: - assert payload["joints"][name]["zero_command_u8"] == zero_command - - -def test_compact_payload_allows_skipped_random_validation() -> None: - base = fit_joint_center_curve(_records()) - splay, zero_command, _ = center_splay_curve(base) - splay = replace( - splay, - angle_rad=tuple( - value - splay.angle_rad[zero_command] - for value in splay.angle_rad - ), - ) - measured = { - name: splay if name == "index_mcp_roll" else base - for name in MEASURED_JOINTS - } - offsets = {name: 0.0 for name in ACTIVE_JOINTS} - baseline = [255] * 20 - baseline[6:10] = [zero_command] * 4 - - payload = build_compact_payload( - serial_number="G20_LEFT_001", - measured_fits=measured, - urdf_zero_offsets_rad=offsets, - validation_errors_rad=[], - passed=True, - baseline=baseline, - ) - - validate_compact_payload(payload) - assert payload["quality"] == { - "passed": True, - "validation_mae_rad": None, - "validation_p95_rad": None, - } - - -def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None: - profile = RIGHT_HAND_PROFILE - base = fit_joint_center_curve(_records()) - splay, zero_command, _ = center_splay_curve(base) - splay = replace( - splay, - angle_rad=tuple( - value - splay.angle_rad[zero_command] - for value in splay.angle_rad - ), - ) - measured = { - name: splay if name == "pinky_mcp_roll" else base - for name in profile.measured_joints - } - offsets = { - name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0) - for name in profile.active_joints - } - baseline = [255] * 20 - baseline[6:10] = [zero_command] * 4 - payload = build_compact_payload( - serial_number="G20_RIGHT_001", - measured_fits=measured, - urdf_zero_offsets_rad=offsets, - validation_errors_rad=[0.01], - passed=True, - baseline=baseline, - side="right", - ) - - validate_compact_payload(payload) - assert payload["schema_version"] == 4 - assert payload["side"] == "right" - assert set(payload) == { - "schema_version", - "model", - "side", - "serial_number", - "angle_unit", - "command_range", - "baseline_command_u8", - "joints", - "quality", - } - for finger in ("index", "middle", "ring"): - assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == ( - "pinky_mcp_roll" - ) - assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == ( - "pinky_mcp_pitch" - ) - assert payload["joints"][f"{finger}_pip"]["source_joint"] == ( - "pinky_pip" - ) - for suffix in ("mcp_roll", "mcp_pitch", "pip"): - joint = payload["joints"][f"{finger}_{suffix}"] - source = payload["joints"][joint["source_joint"]] - assert joint["angle_rad"] == source["angle_rad"] - assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0} - assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01} - - -def test_right_19_schema_v4_records_visual_dips_and_16_static_zeros() -> None: - from linkerhand_calibration.urdf_zero import ( - get_zero_calibration_profile, - ) - - profile = RIGHT_19_HAND_PROFILE - zero = get_zero_calibration_profile("right", "g20_right_19") - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - fits = {} - diagnostics = {} - for name, spec in profile.joint_specs.items(): - zero_command = baseline[spec.motor_index] - values = tuple( - 0.002 * (zero_command - command) for command in range(256) - ) - fits[name] = JointCurveFit( - values, - values, - values, - {}, - 0.0, - 0.0, - {"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5}, - ) - diagnostics[name] = { - "cycle_travel_rad": [0.5, 0.5, 0.5, 0.5], - "cycle_travel_range_rad": 0.0, - "baseline_hysteresis_by_cycle_rad": [0.0, 0.0, 0.0, 0.0], - "holdout_cycle": 3, - "holdout_cycle_mae_rad": 0.0, - "holdout_cycle_max_rad": 0.0, - } - offsets = {name: 0.0 for name in profile.active_joints} - payload = build_compact_payload( - serial_number="G20_RIGHT_019", - measured_fits=fits, - urdf_zero_offsets_rad=offsets, - validation_errors_rad=[0.0], - passed=True, - baseline=baseline, - side="right", - layout_id="g20_right_19", - zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints}, - zero_cycle_offsets_rad={ - name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints - }, - zero_observers=zero.offset_observer_joint, - artifact_hashes={ - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "corrected_urdf_sha256": "2" * 64, - }, - cross_view_roll_metrics={ - f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0} - for finger in ("index", "middle", "ring", "pinky") - }, - joint_dynamic_diagnostics=diagnostics, - zero_geometry_diagnostics={ - "training_cycles": (0, 1, 2), - "validation_cycle": 3, - "axis_line_rms_m": 0.0, - "validation_line_error_by_joint_m": {}, - }, - ) - - validate_compact_payload(payload) - assert payload["schema_version"] == 4 - assert set(payload) == { - "schema_version", "model", "side", "serial_number", "angle_unit", - "command_range", "baseline_command_u8", "joints", "quality", - } - assert len(zero.direct_zero_joints) == 16 - assert all( - f"{finger}_pip" in zero.direct_zero_joints - and f"{finger}_dip" not in zero.direct_zero_joints - for finger in ("index", "middle", "ring", "pinky") - ) - assert payload["joints"]["thumb_mcp"]["zero_angles"] == { - "urdf_zero_offset_rad": 0.0 - } - assert payload["joints"]["thumb_ip"]["passive"] is True - assert set(payload["joints"]["thumb_ip"]) == { - "motor_index", "angle_rad", "passive" - } - assert all( - "source_joint" not in joint for joint in payload["joints"].values() - ) diff --git a/src/linkerhand_calibration/test/test_g20_right_product.py b/src/linkerhand_calibration/test/test_g20_right_product.py deleted file mode 100644 index 1fe6fc0..0000000 --- a/src/linkerhand_calibration/test/test_g20_right_product.py +++ /dev/null @@ -1,985 +0,0 @@ -from dataclasses import replace -import json -import math -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -import pytest -import yaml - -from linkerhand_calibration.full_hand import ( - G20_COMBINATION_REQUIRED_TARGET_KEYS, - G20_RIGHT_19_LAYOUT, - JointCurveFit, - build_compact_payload, - get_hand_calibration_profile, -) -from linkerhand_calibration.operator_report import ( - ProgressEstimator, - build_failure_report, - classify_error, - render_progress_zh, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.runner import ( - _automatic_resume_candidate, - _calibration_node_exited_before_status, - _launch_command, - _resolve_partial_base_session, - _status_timeout_seconds, -) -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.publication import ( - ACTIVE_ZERO_JOINTS, - PASSIVE_JOINTS, - atomic_session_pointer, -) -from linkerhand_calibration.storage import atomic_write_json -from linkerhand_calibration.urdf_zero import ( - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, - UrdfKinematicModel, - write_zero_corrected_urdf, -) -from linkerhand_calibration.runtime import ACQUISITION_POLICY_VERSION - - -REPO = Path(__file__).resolve().parents[3] -PRODUCT = REPO / "src/linkerhand_calibration/config/g20_right_product.yaml" - - -def _config(tmp_path: Path, *, passes: int = 2): - value = load_product_config(PRODUCT, workspace=REPO, check_can=False) - return replace( - value, - output_root=tmp_path / "calibration_output", - required_independent_passes=passes, - ) - - -def _payload( - serial: str = "G20_RIGHT_001", - *, - zero_offsets: dict[str, float] | None = None, -) -> dict: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - offsets = zero_offsets or { - name: 0.01 for name in profile.active_joints - } - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - fits = {} - for name, spec in profile.joint_specs.items(): - zero = baseline[spec.motor_index] - decreasing = tuple(0.0011 * (zero - value) for value in range(256)) - increasing = tuple(0.0009 * (zero - value) for value in range(256)) - fits[name] = JointCurveFit( - angle_rad=decreasing, - decreasing_rad=decreasing, - increasing_rad=increasing, - circle={}, - maximum_monotonic_correction_rad=0.0, - maximum_hysteresis_rad=math.radians(1.0), - quality={}, - ) - return build_compact_payload( - serial_number=serial, - measured_fits=fits, - urdf_zero_offsets_rad=offsets, - validation_errors_rad=[0.0, math.radians(0.5)], - passed=True, - baseline=baseline, - side="right", - layout_id=G20_RIGHT_19_LAYOUT, - ) - - -def _make_passed_session( - config, - stamp: str, - *, - zero_offsets: dict[str, float] | None = None, -) -> Path: - session = config.session_root / stamp - session.mkdir(parents=True) - offsets = zero_offsets or { - name: 0.01 for name in ACTIVE_ZERO_JOINTS - } - payload = _payload(config.serial_number, zero_offsets=offsets) - atomic_write_json( - session / f"g20_right_{config.serial_number}_calibration.json", payload - ) - write_zero_corrected_urdf( - source_urdf=config.source_urdf, - output_directory=session, - serial_number=config.serial_number, - offsets_rad=offsets, - endpoint_anchored_offsets_rad={ - name: offsets[name] - for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - }, - timestamp=stamp, - ) - (session / "raw_samples.jsonl").write_text('{"kind":"session_start"}\n') - (session / "calibration.log").write_text("complete\n") - return session - - -def _passed_node_status() -> dict: - observation_counts = { - key: 2 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS - } - validation_counts = { - key: 1 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS - } - return { - "state": "COMPLETE", - "combination_validation": { - "enabled": True, - "completed": True, - "completed_poses": 8, - "total_poses": 8, - "position_p95_m": 0.001, - "orientation_p95_rad": math.radians(1.0), - "required_targets": list(G20_COMBINATION_REQUIRED_TARGET_KEYS), - "observation_counts": observation_counts, - "validation_counts": validation_counts, - }, - } - - -def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None: - config = load_product_config(PRODUCT, workspace=REPO, check_can=False) - assert (config.model, config.side, config.tag_layout) == ( - "G20", - "right", - G20_RIGHT_19_LAYOUT, - ) - assert config.calibration_contract.profile.command_count == 20 - assert config.calibration_contract.typed_profile.artifacts.publish_corrected_urdf - assert ( - config.calibration_contract.typed_profile.measurement - .stable_cross_view_cone_bias - ) - hand_profile = config.calibration_contract.profile - typed_measurements = ( - config.calibration_contract.typed_profile.measurement.measurements - ) - assert hand_profile.record_specs["thumb_ip"].pose_axis_line_required is False - assert typed_measurements["thumb_ip"].pose_axis_line_required is False - assert all( - spec.pose_axis_line_required - for name, spec in hand_profile.record_specs.items() - if name != "thumb_ip" - ) - assert config.serial_number == "G20_RIGHT_001" - assert config.namespace == "/g20_calibration" - assert config.required_independent_passes == 1 - assert set(config.cameras) == {"front", "side", "top"} - assert len({camera["serial_number"] for camera in config.cameras.values()}) == 3 - assert config.source_urdf.is_file() - assert config.source_urdf == ( - REPO - / "src/linkerhand_calibration/urdf/g20_right/linkerhand_g20_right.urdf" - ) - assert config.camera_extrinsics.is_file() - assert config.calibration_config == ( - REPO - / "src/linkerhand_calibration/config/three_camera_calibration.yaml" - ) - assert config.tag_config == ( - REPO - / "src/linkerhand_calibration/config/three_camera_tags_g20_right_19.yaml" - ) - product_text = PRODUCT.read_text(encoding="utf-8") - assert "src/g20_thumb_apriltag_calibration/config/" in product_text - - -def test_schema_v2_loads_the_same_registered_product(tmp_path) -> None: - payload = yaml.safe_load(PRODUCT.read_text(encoding="utf-8")) - payload["schema_version"] = 2 - payload["profile_id"] = "G20/right/g20_right_19/v1" - payload.pop("model") - payload.pop("side") - payload.pop("tag_layout") - config_path = tmp_path / "product_v2.yaml" - config_path.write_text( - yaml.safe_dump(payload, allow_unicode=True, sort_keys=False), - encoding="utf-8", - ) - - config = load_product_config(config_path, workspace=REPO, check_can=False) - - assert config.schema_version == 2 - assert config.profile_key.profile_id == "G20/right/g20_right_19/v1" - assert config.calibration_contract.typed_profile.key == config.profile_key - - -_FIXED_BASE_CORNERS_BY_VIEW = { - "front": ( - (100.0, 100.0), - (120.0, 100.0), - (120.0, 120.0), - (100.0, 120.0), - ), - "side": ( - (200.0, 100.0), - (220.0, 100.0), - (220.0, 120.0), - (200.0, 120.0), - ), - "top": ( - (300.0, 100.0), - (320.0, 100.0), - (320.0, 120.0), - (300.0, 120.0), - ), -} - - -def _fixed_base_reference_rows( - corners_by_view: dict[str, tuple[tuple[float, float], ...]] | None = None, -) -> list[dict]: - selected = corners_by_view or _FIXED_BASE_CORNERS_BY_VIEW - return [ - { - "kind": "fixed_base_reference_locked", - "view": view, - "corner_reference_xy": [list(point) for point in corners], - } - for view, corners in selected.items() - ] - - -def _resume_reference_views( - corners_by_view: dict[str, tuple[tuple[float, float], ...]] | None = None, -) -> dict[str, SimpleNamespace]: - selected = corners_by_view or _FIXED_BASE_CORNERS_BY_VIEW - return { - view: SimpleNamespace(locked_base_corners_xy=corners) - for view, corners in selected.items() - } - - -def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None: - config = _config(tmp_path) - session = config.session_root / "20260819_150000" - session.mkdir(parents=True) - (session / "raw_samples.jsonl").write_text( - json.dumps( - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "source_urdf_sha256": config.source_urdf_sha256, - } - ) - + "\n" - ) - atomic_write_json( - session / "calibration_summary_zh.json", - { - "result": "FAIL", - "hashes": { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - }, - }, - ) - atomic_session_pointer(config.session_root, "latest_attempt", session) - - assert _automatic_resume_candidate(config) == session - - summary = json.loads( - (session / "calibration_summary_zh.json").read_text() - ) - summary["hashes"]["camera_extrinsics_sha256"] = "0" * 64 - atomic_write_json(session / "calibration_summary_zh.json", summary) - assert _automatic_resume_candidate(config) is None - - -def test_thumb_scope_resolves_passed_base_and_launches_partial_mode( - tmp_path: Path, -) -> None: - config = _config(tmp_path) - session = config.session_root / "20260830_120000" - session.mkdir(parents=True) - (session / "raw_samples.jsonl").write_text("{}\n", encoding="utf-8") - (session / f"g20_right_{config.serial_number}_calibration.json").write_text( - "{}\n", encoding="utf-8" - ) - atomic_write_json( - session / "calibration_summary_zh.json", - { - "result": "PASS", - "quality": {"passed": True}, - "hashes": { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - }, - }, - ) - # Construct an archived fixture, not a new certified publication. - (config.session_root / "latest_passed").symlink_to(session, target_is_directory=True) - - resolved = _resolve_partial_base_session( - config, config.session_root / "latest_passed" - ) - command = _launch_command( - config, - config.session_root / "20260830_130000", - resume_from=resolved, - recalibration_scope="thumb", - ) - - assert resolved == session.resolve() - assert "recalibration_scope:=thumb" in command - assert f"resume_raw_samples_path:={session / 'raw_samples.jsonl'}" in command - - -def test_standalone_thumb_launch_has_no_resume_dependency(tmp_path: Path) -> None: - config = _config(tmp_path) - command = _launch_command( - config, - config.session_root / "20260831_140000", - recalibration_scope="thumb", - ) - - assert "recalibration_scope:=thumb" in command - assert all("resume_raw_samples_path" not in value for value in command) - - -def test_thumb_scope_rejects_base_with_different_geometry(tmp_path: Path) -> None: - config = _config(tmp_path) - session = config.session_root / "20260830_120000" - session.mkdir(parents=True) - (session / "raw_samples.jsonl").touch() - (session / f"g20_right_{config.serial_number}_calibration.json").write_text( - "{}\n", encoding="utf-8" - ) - atomic_write_json( - session / "calibration_summary_zh.json", - { - "result": "PASS", - "quality": {"passed": True}, - "hashes": { - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - }, - }, - ) - - with pytest.raises(ValueError, match="differs from the current product"): - _resolve_partial_base_session(config, session) - - -def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary( - tmp_path: Path, -) -> None: - config = _config(tmp_path) - session = config.session_root / "20260819_153000" - session.mkdir(parents=True) - (session / "raw_samples.jsonl").write_text( - json.dumps( - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_file": str(config.camera_extrinsics), - } - ) - + "\n", - encoding="utf-8", - ) - atomic_session_pointer(config.session_root, "latest_attempt", session) - - assert not (session / "calibration_summary_zh.json").exists() - assert _automatic_resume_candidate(config) == session - - -def test_automatic_resume_skips_newer_attempt_without_start_checkpoint( - tmp_path: Path, -) -> None: - config = _config(tmp_path) - usable = config.session_root / "20260819_150000" - usable.mkdir(parents=True) - (usable / "raw_samples.jsonl").write_text( - json.dumps( - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "source_urdf_sha256": config.source_urdf_sha256, - } - ) - + "\n", - encoding="utf-8", - ) - summary = { - "result": "FAIL", - "hashes": { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - }, - } - atomic_write_json(usable / "calibration_summary_zh.json", summary) - - interrupted = config.session_root / "20260819_160000" - interrupted.mkdir() - (interrupted / "raw_samples.jsonl").write_text("", encoding="utf-8") - atomic_write_json(interrupted / "calibration_summary_zh.json", summary) - atomic_session_pointer(config.session_root, "latest_attempt", interrupted) - - assert _automatic_resume_candidate(config) == usable - - -def test_runtime_json_is_minimal_v4_with_21_independent_midpoint_curves() -> None: - payload = _payload() - assert payload["schema_version"] == 4 - assert len(payload["joints"]) == 21 - assert all("source_joint" not in joint for joint in payload["joints"].values()) - assert sum("zero_angles" in joint for joint in payload["joints"].values()) == 16 - assert { - name for name, joint in payload["joints"].items() if joint.get("passive") - } == PASSIVE_JOINTS - # midpoint of 0.0011 and 0.0009 rad/u8 - assert payload["joints"]["thumb_cmc_pitch"]["angle_rad"][100] == pytest.approx( - 0.155 - ) - - - - - - - - - - - - - - - - - - - - - - -def test_latest_attempt_pointer_is_atomic_session_binding(tmp_path: Path) -> None: - root = tmp_path / "G20_RIGHT_001" - first = root / "20260819_120000" - second = root / "20260819_130000" - first.mkdir(parents=True) - second.mkdir() - atomic_session_pointer(root, "latest_attempt", first) - assert (root / "latest_attempt").resolve() == first - atomic_session_pointer(root, "latest_attempt", second) - assert (root / "latest_attempt").resolve() == second - - -def test_failure_report_is_copyable_and_persisted(tmp_path: Path) -> None: - config = _config(tmp_path, passes=1) - session = config.session_root / "20260819_140000" - session.mkdir(parents=True) - status = { - "state": "PAUSED", - "reason": "synchronised_tag_state_timeout", - "feedback_hz": 29.8, - "active": {"joints": ["index_pip"], "valid_frames": 41}, - "views": { - "front": {"ready": True, "missing_tag_ids": []}, - "side": {"ready": False, "missing_tag_ids": [17]}, - "top": {"ready": True, "missing_tag_ids": []}, - }, - } - payload, block = build_failure_report( - config, session, status, reason=status["reason"] - ) - assert payload["error_code"] == "OBS-TAG-103" - assert "请复制以下内容给开发者" in block - assert "会话编号：G20_RIGHT_001_20260819_140000" in block - saved = json.loads((session / "calibration_summary_zh.json").read_text()) - assert saved["quality"]["passed"] is False - - -def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Path) -> None: - config = _config(tmp_path, passes=1) - session = config.session_root / "20260820_172334" - session.mkdir(parents=True) - status = { - "state": "PAUSED", - "reason": "synchronised_tag_state_timeout:front", - "feedback_hz": 58.8, - "active": {"valid_frames": 381}, - "views": { - "front": { - "ready": True, - "missing_tag_ids": [], - "group_pnp_reason": "group_missing_pose_candidates", - "group_missing_candidate_roles": ["thumb_ip"], - "pnp_rejection_counts": { - "thumb_ip:no_pose_within_reprojection_or_tilt_limit": 12, - }, - "pnp_candidate_diagnostics": { - "thumb_ip": { - "tag_id": 3, - "solved_candidate_count": 2, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - } - }, - }, - "side": {"ready": True, "missing_tag_ids": []}, - "top": {"ready": True, "missing_tag_ids": []}, - }, - } - - payload, _block = build_failure_report( - config, session, status, reason=status["reason"] - ) - - assert payload["error_code"] == "CAM-GEOMETRY-201" - assert "轨迹在运动中失效" in payload["problem_zh"] - assert "整组PnP几何检查拒绝" in payload["explanation_zh"] - assert "缺候选=thumb_ip" in payload["explanation_zh"] - assert payload["pnp_diagnostics"]["front"][ - "group_missing_candidate_roles" - ] == ["thumb_ip"] - assert "不要调整" in payload["automatic_action_zh"] - - -def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Path) -> None: - config = _config(tmp_path, passes=1) - session = config.session_root / "20260821_154105" - session.mkdir(parents=True) - status = { - "state": "PAUSED", - "reason": "sweep_start_tag_timeout", - "feedback_hz": 58.8, - "active": {"valid_frames": 0}, - "views": { - "front": {"ready": True, "missing_tag_ids": []}, - "side": { - "ready": True, - "missing_tag_ids": [], - "group_pnp_reason": "group_initializing:7/8", - "pnp_initialization_progress": { - "accepted": 7, - "required": 8, - }, - "group_pnp_rejection_counts": { - "group_normal_alignment": 5, - }, - }, - "top": {"ready": True, "missing_tag_ids": []}, - }, - } - - payload, _block = build_failure_report( - config, session, status, reason=status["reason"] - ) - - assert payload["error_code"] == "CAM-GEOMETRY-201" - assert "PnP" in payload["problem_zh"] - assert "初始化7/8" in payload["explanation_zh"] - assert "group_normal_alignment×5" in payload["explanation_zh"] - - -def test_multiview_sample_failure_has_stable_observation_code() -> None: - code, problem, _suggestion = classify_error( - "sweep_bins_too_few:pinky_mcp_roll_side", {} - ) - - assert code == "OBS-SAMPLE-104" - assert "轨迹不完整" in problem - - -def test_palm_orientation_coverage_has_validation_error_code() -> None: - code, problem, suggestion = classify_error( - "palm_orientation_quality_failed", {} - ) - - assert code == "VAL-QUALITY-501" - assert "方向校正" in problem - assert "至少三枚" in suggestion - - -@pytest.mark.parametrize( - "reason", - ( - "name 'endpoint_zero_offsets' is not defined", - "validated_endpoint_zero_state_incomplete:missing=index_pip;extra=-", - ), -) -def test_endpoint_zero_lifecycle_failure_is_a_publication_error( - reason: str, -) -> None: - code, problem, suggestion = classify_error(reason, {}) - - assert code == "PUB-ARTIFACT-601" - assert "发布阶段" in problem - assert "不要移动相机或Tag" in suggestion - - -def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None: - assert _status_timeout_seconds({"state": "FITTING"}) == 600.0 - assert _status_timeout_seconds({"state": "SWEEP"}) == 90.0 - assert _status_timeout_seconds({"state": "RETURN_BASELINE"}) == 90.0 - - -def test_stopped_status_heartbeat_has_stable_communication_code() -> None: - code, problem, suggestion = classify_error( - "MOTION-COMM-303:calibration status stopped", {} - ) - - assert code == "MOTION-COMM-303" - assert "状态心跳" in problem - assert "断点" in suggestion - - -def test_fixed_base_reference_movement_has_stable_camera_code() -> None: - code, problem, suggestion = classify_error( - "fixed_base_reference_moved", {} - ) - - assert code == "CAM-REFERENCE-204" - assert "Tag" in problem - assert "新会话预检前重新摆放" in suggestion - - -def test_missing_startup_status_has_specific_camera_status_code() -> None: - code, problem, suggestion = classify_error( - "CAM-STATUS-202:no calibration status received", {} - ) - - assert code == "CAM-STATUS-202" - assert "未发布状态" in problem - assert "尚未开始运动" in suggestion - - -def test_sample_domain_contract_has_specific_non_publication_code() -> None: - code, problem, suggestion = classify_error( - "DATA-CONTRACT-701:sample is missing explicit feedback_u8", {} - ) - - assert code == "DATA-CONTRACT-701" - assert "数据契约" in problem - assert "无需重新采集" in suggestion - - -def test_startup_watchdog_detects_only_calibration_child_exit( - tmp_path: Path, -) -> None: - log = tmp_path / "calibration.log" - log.write_text( - "[three_camera_calibration_node-6]: process has died " - "[pid 123, exit code 1]\n", - encoding="utf-8", - ) - assert _calibration_node_exited_before_status(log) is True - - log.write_text( - "[component_container_mt-4]: process has died [exit code -11]\n", - encoding="utf-8", - ) - assert _calibration_node_exited_before_status(log) is False - - -def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: - status = { - "state": "SWEEP", - "progress": 0.43, - "feedback_hz": 98.0, - "resume": { - "used": True, - "completed_task_count": 3, - "total_task_count": 16, - }, - "active": { - "joints": ["index_pip"], - "cycle": 2, - "repetitions": 4, - "direction": "increasing", - "current_motion_target_u8": 117, - "actual_u8": 116.5, - "valid_frames": 80, - "automatic_retry_count": 1, - "fit_attempt": 3, - "fit_attempt_limit": 3, - "fit_retry_cycles": [2], - }, - "views": { - view: { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [0], - "detected_tag_ids": [0], - } - for view in ("front", "side", "top") - }, - } - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - assert "43.0%" in text - assert "食指PIP" in text - assert "相机：3/3" in text - assert "反馈：98.0 Hz" in text - assert "当前任务ID：正面[0✓] 侧面[0✓] 顶部[0✓]" in text - assert "断点：已恢复 3/16 个完整任务" in text - assert "首次拟合定位第 2 轮需复核，仅补采对应轮次双向" in text - - -def test_progress_reports_new_start_position_without_claiming_resume() -> None: - status = { - "state": "SWEEP", - "progress": 0.01, - "feedback_hz": 58.8, - "active": {}, - "views": {}, - "resume": { - "used": False, - "checkpoint_requested": True, - "checkpoint_pending": False, - "start_position_policy": ( - "discard_all_tasks_for_new_start_pose" - ), - }, - } - - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - - assert "标定前机械手位置已变化" in text - assert "未混用旧断点，本次整手从头采集" in text - assert "已恢复" not in text - - -def test_progress_describes_nonlocalized_full_fit_retry() -> None: - status = { - "state": "SWEEP", - "progress": 0.9, - "feedback_hz": 58.0, - "active": { - "joints": ["thumb_cmc_pitch"], - "cycle": 1, - "repetitions": 4, - "direction": "decreasing", - "valid_frames": 20, - "automatic_retry_count": 1, - "fit_attempt": 2, - "fit_attempt_limit": 3, - "fit_retry_cycles": [1, 2, 3, 4], - }, - "views": {}, - } - - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - - assert "未能定位单一异常轮，联合补采第 1/2/3/4 轮双向" in text - - -def test_preflight_progress_marks_non_base_tag_occlusion_as_allowed() -> None: - status = { - "state": "PREFLIGHT", - "progress": 0.0, - "feedback_hz": 40.5, - "active": {}, - "views": { - "front": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "configured_tag_ids": list(range(8)), - "visible_configured_tag_ids": list(range(6)), - "required_tag_ids": [0], - "detected_tag_ids": [0], - }, - "side": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "configured_tag_ids": list(range(9)), - "visible_configured_tag_ids": list(range(5)), - "required_tag_ids": [4], - "detected_tag_ids": [4], - }, - "top": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "configured_tag_ids": [8, 9], - "visible_configured_tag_ids": [8, 9], - "required_tag_ids": [8], - "detected_tag_ids": [8], - }, - }, - } - - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - - assert "当前 13/19（允许遮挡）" in text - assert "本阶段必需 3/3" in text - - -def test_sweep_progress_lists_visible_and_missing_task_tag_ids() -> None: - status = { - "state": "SWEEP", - "progress": 0.5, - "feedback_hz": 58.8, - "active": { - "joints": ["ring_pip", "ring_dip"], - "cycle": 1, - "repetitions": 4, - "direction": "decreasing", - }, - "views": { - "front": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [0], - "detected_tag_ids": [0], - }, - "side": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [4, 5, 14], - "detected_tag_ids": [4, 14], - }, - "top": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [8], - "detected_tag_ids": [8], - }, - }, - } - - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - - assert "Tag：4/5 可见" in text - assert "正面[0✓]" in text - assert "侧面[4✓,5✗,14✓]" in text - assert "顶部[8✓]" in text - assert "✓实时可见/锁=基准锁定/✗不可用" in text - - -def test_multiview_progress_marks_occluded_front_base_as_locked() -> None: - status = { - "state": "PREPARE_SWEEP", - "reason": "waiting_for_task_tags_at_sweep_start", - "progress": 0.562, - "feedback_hz": 58.5, - "active": { - "task_name": "middle_roll_multiview", - "joints": ["middle_mcp_roll", "middle_mcp_roll_side"], - "cycle": 0, - "repetitions": 4, - "direction": "decreasing", - "current_motion_target_u8": 255, - "actual_u8": 254.0, - }, - "views": { - "front": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [0, 12], - "locked_reference_tag_ids": [0], - "detected_tag_ids": [12], - }, - "side": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [4, 15], - "locked_reference_tag_ids": [], - "detected_tag_ids": [4, 15], - "pnp_pose_valid": False, - "pnp_initialization_progress": { - "accepted": 5, - "required": 8, - }, - "pnp_rejection_counts": { - "middle_pip:no_pose_within_reprojection_or_tilt_limit": 7, - }, - }, - "top": { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "required_tag_ids": [8], - "locked_reference_tag_ids": [], - "detected_tag_ids": [8], - }, - }, - } - - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - - assert "已到扫描起点，等待任务Tag" in text - assert "命令/反馈：255/254.0" in text - assert "Tag：5/5 可见（含锁定 1）" in text - assert "正面[0锁,12✓]" in text - assert "预计剩余 等待Tag" in text - assert "PnP：侧面 初始化5/8" in text - assert "Tag可见不等于三维位姿有效" in text - - -def test_device_preflight_progress_says_tags_are_checked_after_recovery() -> None: - status = { - "state": "PREFLIGHT", - "preflight_mode": "device_only_before_baseline", - "progress": 0.0, - "feedback_hz": 58.9, - "active": {}, - "views": { - view: { - "camera_info_valid": True, - "camera_extrinsics_valid": True, - "configured_tag_ids": list(range(6)), - "visible_configured_tag_ids": list(range(3)), - "required_tag_ids": [0], - "detected_tag_ids": [], - } - for view in ("front", "side", "top") - }, - } - - text = render_progress_zh( - "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) - ) - - assert "阶段：设备连接预检" in text - assert "Tag：当前 9/18（基准恢复后检查）" in text - assert "本阶段必需" not in text - - -def test_combination_kinematics_can_use_independent_passive_curve() -> None: - config = load_product_config(PRODUCT, workspace=REPO, check_can=False) - model = UrdfKinematicModel(config.source_urdf) - angles = {"thumb_mcp": 0.4, "thumb_ip": 0.2} - independent = model.link_transform( - "thumb_ip", - zero_offsets={}, - joint_angles=angles, - independent_mimic_angles=True, - ) - mimic = model.link_transform( - "thumb_ip", - zero_offsets={}, - joint_angles=angles, - independent_mimic_angles=False, - ) - assert not np.allclose(independent, mimic) diff --git a/src/linkerhand_calibration/test/test_golden_regression.py b/src/linkerhand_calibration/test/test_golden_regression.py deleted file mode 100644 index 7e1a367..0000000 --- a/src/linkerhand_calibration/test/test_golden_regression.py +++ /dev/null @@ -1,72 +0,0 @@ -import hashlib -import json -from pathlib import Path - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.golden_regression import ( - validate_golden_sessions, -) - - -def _write(path: Path, value: str) -> str: - path.write_text(value, encoding="utf-8") - return hashlib.sha256(value.encode("utf-8")).hexdigest() - - -def test_static_golden_validation_covers_pass_fail_and_partial_scopes( - tmp_path: Path, -) -> None: - passed = { - "20260830_181154": ("full", {}), - "20260831_141123": ( - "thumb", - {"preserved_certified_zero_joints": [f"joint_{i}" for i in range(12)]}, - ), - "20260831_163843": ("thumb", {"standalone": True}), - } - for session_id, (scope, extras) in passed.items(): - session = tmp_path / session_id - session.mkdir() - payload = ( - {"artifact_type": "g20_right_standalone_thumb_calibration"} - if extras.pop("standalone", False) - else {"artifact_type": "full"} - ) - json_text = json.dumps(payload) + "\n" - json_hash = _write(session / "calibration.json", json_text) - urdf_hash = _write(session / "corrected.urdf", f"<{session_id}/>\n") - summary = { - "result": "PASS", - "calibration_scope": scope, - "artifacts": {"json": "calibration.json", "urdf": "corrected.urdf"}, - "hashes": { - "calibration_json_sha256": json_hash, - "corrected_urdf_sha256": urdf_hash, - }, - **extras, - } - (session / "calibration_summary_zh.json").write_text( - json.dumps(summary), encoding="utf-8" - ) - - failures = { - "20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"), - "20260831_142322": ( - "VAL-QUALITY-501", - "zero_model_validation_failed", - ), - } - for session_id, (code, reason) in failures.items(): - session = tmp_path / session_id - session.mkdir() - (session / "calibration_summary_zh.json").write_text( - json.dumps( - {"result": "FAIL", "error_code": code, "reason": reason} - ), - encoding="utf-8", - ) - - (tmp_path / "latest_passed").symlink_to("20260830_181154") - result = validate_golden_sessions(tmp_path, replay_full_session=False) - - assert result["20260830_181154"]["result"] == "PASS" - assert result["20260831_111837"]["error_code"] == "FIT-MODEL-401" diff --git a/src/linkerhand_calibration/test/test_held_joint_replay.py b/src/linkerhand_calibration/test/test_held_joint_replay.py deleted file mode 100644 index d75a405..0000000 --- a/src/linkerhand_calibration/test/test_held_joint_replay.py +++ /dev/null @@ -1,86 +0,0 @@ -"""A held ancestor's hysteresis is part of the physical FK condition.""" - -from types import SimpleNamespace as NS - -import numpy as np -import pytest - -from linkerhand_calibration.core.domain.result import JointMapping -from linkerhand_calibration.core.fitting.tag_installation import TagInstallationFailure, matrix_tuple -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.core.urdf.tag_acceptance import validate_serialized_tag_holdout -from linkerhand_calibration.runtime.artifacts.evidence import prepare_tag_replay -from test_frozen_tag_replay import transform - - -def virtual_capture(tmp_path): - source = tmp_path / "source.urdf" - source.write_text(''' - - - - - - ''') - tasks = tuple(NS(key=name, joints=(name,), command_index=i, start_value=0., end_value=1.) - for i, name in enumerate(("root", "tip"))) - profile = NS(retained_joints=frozenset(), command=NS(unit="rad", urdf_joint_by_joint={}), - measurement=NS(measurements={name:NS(view="front", child_role=name, kind="relative_rotation") - for name in ("root", "tip")}), motion=NS(tasks=tasks), - vision=NS(views=(NS(name="front", tags=( - NS(role="root", link="proximal"), NS(role="tip", link="distal"))),))) - mappings = {name: JointMapping(name,i,"feedback_rad",(0.,1.),(0.,1.),(delta,1+delta),(-delta,1-delta)) - for i,(name,delta) in enumerate((("root",.08),("tip",.025)))} - base=transform((.1,-.2,.3),(.1,.2,.7)) - mounts={"root":transform((.2,.3,-.1),(.01,.02,.03)), - "tip":transform((-.3,.1,.2),(.015,.01,.025))} - rows=[] - for cycle in range(4): - for task in tasks: - for direction in ("increasing","decreasing"): - held="increasing" if (cycle+(direction=="increasing"))%2 else "decreasing" - for i,x in enumerate(np.linspace(.05,.95,50)): - sdk=[.45,.55];sdk[task.command_index]=float(x) - directions=[held,held];directions[task.command_index]=direction - # Elementary ground truth, independent of URDF FK and fit. - root=sdk[0]+(.08 if directions[0]=="increasing" else -.08) - tip=sdk[1]+(.025 if directions[1]=="increasing" else -.025) - pose=transform((0,0,root)) - if task.key=="tip":pose=pose@transform(xyz=(.04,0,.05))@transform((0,tip,0)) - rows.append({"joint":task.key,"task_name":task.key,"view":"front","cycle":cycle, - "sample_id":f"front:{task.key}:{cycle}:{direction}:{i}","direction":direction, - "state_rad":sdk,"command_vector_rad":sdk,"command_direction_by_index":directions, - "child_pose_common":matrix_tuple(base@pose@mounts[task.key])}) - return profile,source,mappings,base,rows - - -def test_held_direction_is_preserved_through_training_and_final_urdf_replay(tmp_path): - profile,source,mappings,base,rows=virtual_capture(tmp_path) - args=dict(profile=profile,source_model=UrdfKinematicModel(source), - plan=NS(mimic_output={},zero_offsets_rad={}),output_mappings=mappings,common_from_base=base) - # Dropping known arrival branches falsely makes a perfectly rigid Tag - # appear to move by >4 degrees between captures of its child joint. - without_history=[{k:v for k,v in row.items() if k!="command_direction_by_index"} for row in rows] - with pytest.raises(TagInstallationFailure): - prepare_tag_replay(**args,records=without_history) - replay=prepare_tag_replay(**args,records=rows) - assert max(m.training_rotation_p95_deg for m in replay.installations.values())<1e-6 - payload={"schema_version":7,"profile_id":"synthetic","curve_input_domain":"feedback_rad", - "joints":{name:{"urdf_joint":name,"motor_index":m.motor_index, - "curve_input_knots_rad":list(m.knots),"angle_rad":list(m.angle_rad), - "increasing_rad":list(m.increasing_rad),"decreasing_rad":list(m.decreasing_rad)} - for name,m in mappings.items()}} - metrics=validate_serialized_tag_holdout(corrected_urdf=source,payload=payload, - common_from_base=replay.common_from_base,installations=replay.installations, - observations=replay.holdout,required_roles=replay.required_roles) - assert max(r["maximum_deg"] for r in metrics.values())<1e-6 - - -@pytest.mark.parametrize("history", [["increasing"], ["increasing","invalid"], "increasing"]) -def test_partial_or_invalid_direction_history_cannot_silently_use_mean(tmp_path, history): - profile,source,mappings,base,rows=virtual_capture(tmp_path) - rows[0]["command_direction_by_index"]=history - with pytest.raises(ValueError,match="invalid SDK direction history"): - prepare_tag_replay(profile=profile,source_model=UrdfKinematicModel(source), - plan=NS(mimic_output={},zero_offsets_rad={}),output_mappings=mappings, - common_from_base=base,records=rows) diff --git a/src/linkerhand_calibration/test/test_hikrobot_camera.py b/src/linkerhand_calibration/test/test_hikrobot_camera.py deleted file mode 100644 index fc75b61..0000000 --- a/src/linkerhand_calibration/test/test_hikrobot_camera.py +++ /dev/null @@ -1,136 +0,0 @@ -from pathlib import Path -from ctypes import c_uint8 -from threading import Event -from types import SimpleNamespace -from unittest.mock import Mock - -import pytest -import yaml - -from linkerhand_calibration.hikrobot_camera import ( - DeviceDescriptor, - HikrobotCameraNode, - decode_c_string, - load_camera_calibration, - resolve_camera_info_path, - select_device, -) - - -def test_decode_c_string_stops_at_first_null() -> None: - assert decode_c_string(b"DB2163742\0ignored") == "DB2163742" - - -def test_select_device_accepts_serial_or_guid() -> None: - devices = [ - DeviceDescriptor( - index=0, - model="MV-CS020-10UM", - serial="DB2163742", - guid="2BDFB2163742", - ), - DeviceDescriptor( - index=1, - model="MV-CS020-10UM", - serial="DB2163739", - guid="2BDFB2163739", - ), - ] - - assert select_device(devices, "DB2163742", "MV-CS020-10UM").index == 0 - assert select_device(devices, "2BDFB2163739", "MV-CS020-10UM").index == 1 - - -def test_select_device_never_guesses_when_multiple_cameras_exist() -> None: - devices = [ - DeviceDescriptor(0, "MV-CS020-10UM", "one", "guid-one"), - DeviceDescriptor(1, "MV-CS020-10UM", "two", "guid-two"), - ] - - with pytest.raises(RuntimeError, match="selector is required"): - select_device(devices, "", "MV-CS020-10UM") - - -def test_select_device_rejects_wrong_model() -> None: - devices = [DeviceDescriptor(0, "other", "DB2163742", "guid")] - - with pytest.raises(RuntimeError, match="expected a model"): - select_device(devices, "DB2163742", "MV-CS020-10UM") - - -def test_load_standard_camera_calibration(tmp_path: Path) -> None: - path = tmp_path / "camera.yaml" - path.write_text( - yaml.safe_dump( - { - "image_width": 1624, - "image_height": 1240, - "camera_name": "hikrobot_front_DB2163742", - "camera_matrix": { - "rows": 3, - "cols": 3, - "data": [1000.0, 0.0, 812.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 1.0], - }, - "distortion_model": "plumb_bob", - "distortion_coefficients": { - "rows": 1, - "cols": 5, - "data": [0.1, -0.2, 0.0, 0.0, 0.1], - }, - "rectification_matrix": { - "rows": 3, - "cols": 3, - "data": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], - }, - "projection_matrix": { - "rows": 3, - "cols": 4, - "data": [1000.0, 0.0, 812.0, 0.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 0.0, 1.0, 0.0], - }, - }, - sort_keys=False, - ), - encoding="utf-8", - ) - - calibration = load_camera_calibration(path) - - assert calibration.width == 1624 - assert calibration.height == 1240 - assert calibration.k[0] == 1000.0 - assert calibration.p[5] == 1001.0 - - -def test_camera_info_url_only_accepts_local_files(tmp_path: Path) -> None: - path = resolve_camera_info_path(str(tmp_path / "front.yaml")) - assert path == (tmp_path / "front.yaml").resolve() - assert resolve_camera_info_path("") is None - with pytest.raises(ValueError, match="filesystem path"): - resolve_camera_info_path("package://example/front.yaml") - - -def test_grab_loop_retries_clock_failures_without_publishing_or_exiting(monkeypatch): - stop = Event() - waits = [] - monkeypatch.setattr(stop, 'wait', lambda seconds: waits.append(seconds)) - timing = Mock() - timing.refresh_if_due.side_effect = [ValueError('clock mismatch'), ValueError('latch failed'), None] - camera, logger = Mock(), Mock() - camera.MV_CC_GetImageBuffer.return_value = 0 - camera.MV_CC_FreeImageBuffer.return_value = 0 - published = [] - - def publish(frame): - assert timing.refresh_if_due.call_count == 3 - assert timing.invalidate.call_count == 2 - published.append(frame) - stop.set() - - node = SimpleNamespace(_camera=camera, _mvs=SimpleNamespace(MV_FRAME_OUT=c_uint8), - _camera_timing=timing, _stop_event=stop, - get_parameter=lambda name: SimpleNamespace(value=1000), - get_logger=lambda: logger, _publish_frame=publish) - HikrobotCameraNode._grab_loop(node) - assert len(published) == 1 and waits == [.2, .2] - assert camera.MV_CC_GetImageBuffer.call_count == camera.MV_CC_FreeImageBuffer.call_count == 1 - assert logger.error.call_count == logger.info.call_count == 1 diff --git a/src/linkerhand_calibration/test/test_image_bundle_optimizer.py b/src/linkerhand_calibration/test/test_image_bundle_optimizer.py deleted file mode 100644 index 2e5f2d5..0000000 --- a/src/linkerhand_calibration/test/test_image_bundle_optimizer.py +++ /dev/null @@ -1,179 +0,0 @@ -"""Finite numerical recovery, real acquisition replay, and unchanged refusal gates.""" -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -import pytest -from scipy.sparse import eye - -from linkerhand_calibration.core.geometry.tag_pose import image_bundle_optimizer as optimizer -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ImageMotionParameters - - -def result(status, cost=2., nfev=150, x=(.5,)): - return SimpleNamespace(status=status, success=status > 0, cost=cost, nfev=nfev, - x=np.array(x), fun=np.array([1.]), optimality=.1) - - -@pytest.mark.parametrize('terminal', ['exhausted', 'exception', 'worse', 'nonfinite']) -def test_unresolved_recovery_never_authorizes_or_repeats(monkeypatch, terminal): - first = result(0) - calls = [] - def solve(fun, initial, **kwargs): - calls.append((initial.copy(), kwargs)) - if len(calls) == 1: - return first - if terminal == 'exception': - raise ValueError('numerical failure') - return result(0 if terminal == 'exhausted' else 2, - cost=3. if terminal == 'worse' else 1., nfev=50, - x=(np.nan,) if terminal == 'nonfinite' else (.4,)) - monkeypatch.setattr(optimizer, 'least_squares', solve) - fit = optimizer.solve_image_bundle(lambda x: x, np.array([.9]), - sparsity=eye(1), scale=np.ones(1), limits=ImageMotionParameters(), bounds=([0.], [1.])) - assert not fit.success and fit.status == 0 - assert len(calls) == len(fit.image_solver_attempts) == 2 - assert calls[1][0] == first.x - assert calls[1][1]['tr_solver'] == 'exact' - assert calls[1][1]['max_nfev'] == 50 - assert calls[0][1]['bounds'] == calls[1][1]['bounds'] - for tolerance in ('ftol', 'xtol', 'gtol'): - assert calls[0][1][tolerance] == calls[1][1][tolerance] - - -@pytest.mark.parametrize('case', ['converged', 'large_graph', 'disabled', 'nonfinite']) -def test_ineligible_retry_does_not_spend_more_budget(monkeypatch, case): - first = result(2 if case == 'converged' else 0, - x=tuple(np.zeros(optimizer.MAXIMUM_DENSE_PARAMETERS+1)) if case == 'large_graph' else (np.nan if case == 'nonfinite' else .5,)) - calls = [] - monkeypatch.setattr(optimizer, 'least_squares', lambda *a, **k: calls.append(k) or first) - fit = optimizer.solve_image_bundle(lambda x: x, np.zeros(len(first.x)), - sparsity=eye(len(first.x)), scale=np.ones(len(first.x)), - limits=ImageMotionParameters(maximum_solver_recovery_evaluations=0 if case == 'disabled' else 50)) - assert fit is first and len(calls) == 1 and len(fit.image_solver_attempts) == 1 - - -@pytest.fixture(scope='module') -def exhausted_ip(): - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from linkerhand_calibration.runtime.passed_resume import restore_parent_models - from o30_capture_fixture import SOURCE - data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_thumb_ip_solver_budget.json.gz').read_bytes())) - rows, failure = data['records'], data['original_failure'] - profile = load_bundled_hand_profile('o30_right_18') - owner = BranchInitialization(profile, source_urdf=SOURCE) - restore_parent_models(owner.parent_references, - [r for r in rows if r['kind'] == 'motion_branch_initialization'], failure['session_epoch']) - owner.parent_references.verified[(failure['session_epoch'], failure['task_name'])] = next( - r for r in rows if r['kind'] == 'parent_reference_verified') - motion = MotionCommand('zero_approach', profile.motion.joint_zero_references['thumb_ip'].command, - 200., task_key=failure['task_name'], reference_joints=('thumb_ip',)) - owner.begin(motion, session_epoch=failure['session_epoch'], motion_version=failure['motion_version']) - for row in rows: - if row['kind'] == 'motion_branch_observation' and row['task_name'] == failure['task_name']: - owner.observe(row) - request = owner.request('front', replace(motion, phase='joint_zero', zero_joints=('thumb_ip',)), - session_epoch=failure['session_epoch'], motion_version=failure['motion_version']+1) - assert not request.invalid_reason - assert tuple(f.evidence.stamp_ns for f in request.image_frames) == tuple(failure['source_image_stamps']) - return data, request, solve_initialization(request) - - -@pytest.mark.replay -def test_real_exhausted_candidate_is_solved_then_rejected_by_original_pixel_gate(exhausted_ip): - from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion - from linkerhand_calibration.runtime.branch_initialization import initialization_artifacts - from linkerhand_calibration.runtime.motion_provenance import _check_measured_transfer_report - from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_motion_model_from_dict, image_model_payload - data, request, resolved = exhausted_ip - assert data['original_failure']['hypotheses'][1]['training_solver_status'] == 0 - assert resolved.resolved, resolved.reason - winner, alternative = resolved.hypotheses - assert winner.training_accepted and len(winner.training_solver_attempts) == 1 - assert alternative.converged and not alternative.training_accepted - assert alternative.reason == 'image_motion_reprojection_failed' - assert [a.backend for a in alternative.training_solver_attempts] == ['sparse_lsmr', 'dense_exact'] - assert alternative.training_solver_attempts[0].status == 0 - assert alternative.training_solver_attempts[1].status > 0 - assert alternative.training_solver_evaluations <= 200 - assert winner.validation_quality[0].maximum_frame_rms_px < .34 - assert alternative.validation_quality[0].maximum_frame_rms_px > 1.5 - assert resolved.model.maximum_reprojection_error_px == 1.5 - assert set(resolved.model.training_stamps_ns).isdisjoint(resolved.model.validation_stamps_ns) - _, report = initialization_artifacts(request, resolved) - _check_measured_transfer_report(report) - assert image_motion_model_from_dict(report['frozen_image_model']) == resolved.model - for payload in report['evidence_payloads'].values(): - assert payload['image_solver_policy'] == optimizer.IMAGE_BUNDLE_SOLVER_POLICY - assert payload['image_solver_attempts'] == report['image_solver_attempts'] - # Audit replays the same raw pixels with the same deterministic two-stage solver. - replayed = resolve_image_motion(request.image_frames, request.relations, - geometry_constraints=request.geometry_constraints, source_hinges=request.source_hinges, - source_urdf_sha256=request.source_urdf_sha256) - assert replayed.resolved and image_model_sha256(image_model_payload(replayed.model)) == image_model_sha256(image_model_payload(resolved.model)) - - -@pytest.mark.replay -@pytest.mark.parametrize('eligible_winner', [True, False]) -def test_budget_exhaustion_still_vetoes_without_motion_retry(exhausted_ip, eligible_winner): - from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver - from linkerhand_calibration.runtime.zero_recovery import failure_category, FailureCategory, permits_recovery - from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh - _, request, resolved = exhausted_ip - winner, alternative = resolved.hypotheses - winner = replace(winner, training_accepted=eligible_winner) - alternative = replace(alternative, converged=False, reason='image_motion_optimizer_unresolved', - training_solver_status=0, training_accepted=False) - validation = [i for i, f in enumerate(request.image_frames) - if f.evidence.stamp_ns in resolved.model.validation_stamps_ns] - outcome = solver.select_image_hypotheses(request.image_frames, validation, - [winner, alternative], [resolved.model, resolved.model], - [np.zeros(len(validation))]*2, ImageMotionParameters()) - assert not outcome.resolved and outcome.model is None - assert outcome.reason == 'image_motion_solver_budget_exhausted' - assert failure_category(outcome.reason) == FailureCategory.NUMERICAL_SOLVER - assert not permits_recovery([outcome.reason], 0) - assert reason_zh({'reason': 'joint_zero_motion_unresolved:'+outcome.reason}, model_name='O30')[0] == 'OBS-SOLVER-118' - - -@pytest.mark.replay -def test_recovered_real_preparation_passes_formal_source_and_parent_contracts(exhausted_ip): - from linkerhand_calibration.runtime.branch_initialization import initialization_artifacts - from linkerhand_calibration.runtime.motion_provenance import _initializations, validate_source_geometry - from linkerhand_calibration.profiles import load_bundled_hand_profile - from o30_capture_fixture import SOURCE - data, request, resolved = exhausted_ip - _, report = initialization_artifacts(request, resolved) - records = json.loads(json.dumps([*data['records'], report])) - profile = load_bundled_hand_profile('o30_right_18') - assert _initializations(records) - validate_source_geometry(profile, SOURCE, records) - - -@pytest.mark.replay -def test_solver_audit_tampering_rejected_before_source_replay(exhausted_ip): - from linkerhand_calibration.runtime.branch_initialization import initialization_artifacts - from linkerhand_calibration.runtime.motion_provenance import _initializations - _, request, resolved = exhausted_ip - _, report = initialization_artifacts(request, resolved) - for field in ('image_solver_attempts', 'image_solver_policy'): - changed = deepcopy(report) - changed[field] = [] if field == 'image_solver_attempts' else 'ignore_unfinished' - with pytest.raises(ValueError, match='solver_report_changed'): - _initializations([changed]) - changed = json.loads(json.dumps(report)) - changed['hypotheses'][1]['training_solver_attempts'][1]['status'] = 0 - with pytest.raises(ValueError, match='solver_report_changed'): - _initializations([changed]) - changed = deepcopy(report) - changed.pop('image_solver_policy') - changed.pop('image_solver_attempts') - with pytest.raises(ValueError, match='solver_report_changed'): - _initializations([changed]) diff --git a/src/linkerhand_calibration/test/test_image_error_budget.py b/src/linkerhand_calibration/test/test_image_error_budget.py deleted file mode 100644 index 1445a6e..0000000 --- a/src/linkerhand_calibration/test/test_image_error_budget.py +++ /dev/null @@ -1,124 +0,0 @@ -"""Independent truth separates measurement noise from motion approximation.""" - -from dataclasses import replace -import math - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting.tag_installation import FrozenTagInstallation -from linkerhand_calibration.core.urdf.image_acceptance import ( - ImageHoldoutFailure, TagImageObservation, validate_serialized_image_holdout, -) -from linkerhand_calibration.core.urdf.image_consistency import _rigid_projection -from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout, validate_serialized_tag_holdout - - -def known_scene(tmp_path, *, focal, error_deg=.5, translation_m=0.): - source = tmp_path / "known.urdf" - source.write_text(''' - - - ''') - payload = dict(schema_version=7, profile_id="KNOWN/right/one/v1", curve_input_domain="feedback_rad", - joints={"drive": dict(urdf_joint="drive", motor_index=0, - curve_input_knots_rad=[0., 1.], angle_rad=[0., 1.])}) - base, mount = np.eye(4), np.eye(4) - base[2, 3], mount[0, 3] = .8, .1 - frozen = FrozenTagInstallation("tip", "finger", tuple(map(tuple, mount)), ("train:0",), 0., 0.) - objects = np.array([[-.008, .008, 0], [.008, .008, 0], [.008, -.008, 0], [-.008, -.008, 0]]) - camera = np.array([[focal, 0, 812.], [0, focal, 620.], [0, 0, 1.]]) - poses, images = [], [] - for index, sdk in enumerate(np.linspace(.1, .9, 40)): - angle = float(sdk) + math.radians(error_deg) - rotation = np.array([[math.cos(angle), -math.sin(angle), 0], - [math.sin(angle), math.cos(angle), 0], [0, 0, 1.]]) - pose = np.eye(4) - pose[:3, :3] = rotation - pose[:3, 3] = [.1*math.cos(angle)+translation_m, .1*math.sin(angle), .8] - row = TagHoldout(f"camera:{index}", "tip", 3, (float(sdk),), {}, tuple(map(tuple, pose))) - points = (objects @ rotation.T + pose[:3, 3]) @ camera.T - corners = points[:, :2] / points[:, 2:] - poses.append(row) - images.append(TagImageObservation(row.sample_id, row.role, 3, row.sdk_values, (), - "camera", .016, tuple(map(tuple, corners)), tuple(map(tuple, camera)), - tuple(map(tuple, np.eye(4))), task_name="drive")) - return dict(corrected_urdf=source, payload=payload, common_from_base=base, - installations={"tip": frozen}, required_roles=("tip",), observations=images, - independent_observations=poses, input_kind="feedback", holdout_cycle=3, minimum_samples=40) - - -@pytest.mark.parametrize("focal", [1000., 3500.]) -def test_same_allowed_physical_error_does_not_depend_on_focal_length(tmp_path, focal): - scene = known_scene(tmp_path, focal=focal) - physical = {key: value for key, value in scene.items() if key not in {"observations", "independent_observations"}} - physical["observations"] = scene["independent_observations"] - validate_serialized_tag_holdout(**physical) - metrics = validate_serialized_image_holdout(**scene)["tip"] - assert metrics["independent_motion"]["mae_deg"] == pytest.approx(.5) - assert metrics["maximum_px"] < 1e-8 - assert (metrics["nominal_projection"]["maximum_px"] > 1.5) == (focal == 3500.) - - -@pytest.mark.parametrize("error", [dict(error_deg=4.), dict(error_deg=0., translation_m=.004)]) -def test_perfect_measurement_pixels_cannot_bypass_physical_accuracy(tmp_path, error): - scene = known_scene(tmp_path, focal=3500., **error) - with pytest.raises(ImageHoldoutFailure) as caught: - validate_serialized_image_holdout(**scene) - report = caught.value.report["tip"] - assert report["maximum_px"] < 1e-8 - assert not report["independent_motion"]["passed"] - - -def test_extra_view_witness_is_separate_from_independent_accuracy(tmp_path): - scene = known_scene(tmp_path, focal=3500.) - extra = replace(scene["observations"][0], sample_id="camera:extra", task_name="another_motion") - scene["observations"].append(extra) - report = validate_serialized_image_holdout(**scene)["tip"] - assert report["count"] == 41 - assert report["independent_motion"]["count"] == 40 - witness = report["auxiliary_consistency_by_task"]["another_motion"] - assert witness["passed"] and witness["maximum_px"] <= 1.5 - assert witness["interpretation"] == "feasible_pose_displacement_only" - - -def test_stationary_images_cannot_dilute_inconsistent_motion(tmp_path): - scene = known_scene(tmp_path, focal=3500., error_deg=0.) - original = scene["observations"][0] - scene["observations"].extend(replace(original, sample_id=f"camera:static:{i}", - task_name="stationary") for i in range(100)) - scene["observations"].append(replace(original, sample_id="camera:bad", task_name="moving", - corners_xy=tuple((x+80, y) for x, y in original.corners_xy))) - with pytest.raises(ImageHoldoutFailure) as caught: - validate_serialized_image_holdout(**scene) - report = caught.value.report["tip"] - assert report["independent_motion"]["passed"] - assert report["auxiliary_consistency_by_task"]["stationary"]["passed"] - assert not report["auxiliary_consistency_by_task"]["moving"]["passed"] - - -@pytest.mark.parametrize("mutation", ["missing_pose", "sdk", "direction", "duplicate_pose"]) -def test_independent_pose_identity_and_inputs_cannot_be_replaced_by_witnesses(tmp_path, mutation): - scene = known_scene(tmp_path, focal=3500.) - rows = scene["independent_observations"] - if mutation == "missing_pose": rows.pop() - elif mutation == "sdk": rows[0] = replace(rows[0], sdk_values=(.2,)) - elif mutation == "direction": rows[0] = replace(rows[0], sdk_directions_by_joint={"drive": "increasing"}) - else: rows.append(rows[0]) - with pytest.raises(ValueError, match="final_image_holdout"): - validate_serialized_image_holdout(**scene) - - -@pytest.mark.parametrize("x", [np.zeros(6), np.array([.6, -.3, .2, -.8, .2, .5])]) -def test_rigid_projection_jacobian_matches_independent_finite_differences(x): - pose = np.eye(4) - pose[:3, :3] = Rotation.from_euler("xyz", [.3, -.6, .2]).as_matrix() - pose[:3, 3] = [.08, -.03, .5] - camera = np.array([[3500., 4., 820.], [0, 3600., 650.], [0, 0, 1.]]) - objects = np.array([[-.008, .008, 0], [.008, .008, 0], [.008, -.008, 0], [-.008, -.008, 0]]) - args = (pose, camera, objects, np.zeros((4, 2)), np.deg2rad(3.), .003) - _, analytic = _rigid_projection(x, *args) - numeric = np.stack([(_rigid_projection(x+step, *args)[0]-_rigid_projection(x-step, *args)[0]) / 2e-5 - for step in np.eye(6)*1e-5], axis=-1) - np.testing.assert_allclose(analytic, numeric, rtol=2e-6, atol=2e-8) diff --git a/src/linkerhand_calibration/test/test_image_model_selection.py b/src/linkerhand_calibration/test/test_image_model_selection.py deleted file mode 100644 index 1dfb526..0000000 --- a/src/linkerhand_calibration/test/test_image_model_selection.py +++ /dev/null @@ -1,126 +0,0 @@ -"""Selection stays independent of holdout quality, order and duplicate starts.""" -from dataclasses import replace -import gzip -from itertools import permutations -import json -from pathlib import Path - -import numpy as np -import pytest - -from linkerhand_calibration.core.geometry.tag_pose.image_model_selection import ( - compare_image_losses, equivalent_frame_selections, holm_adjusted_probabilities, - training_candidate_index, -) -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionHypothesis -from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageRoleQuality - - -def hypothesis(training, validation, reason=""): - return ImageMotionHypothesis((("root", 0), ("moving", 0)), True, reason, - (ImageRoleQuality("moving", 30, training, training, training),), - (ImageRoleQuality("moving", 30, validation, validation, validation),), training_accepted=True) - - -def test_holdout_cannot_select_or_replace_training_candidate(): - candidates = (hypothesis(.2, 1.6, "image_motion_reprojection_failed"), hypothesis(.3, .1)) - assert training_candidate_index(candidates) == 0 - changed = tuple(replace(h, validation_quality=(), reason="holdout_unavailable") for h in candidates) - assert training_candidate_index(changed) == 0 - assert training_candidate_index(tuple(replace(h, training_accepted=False) for h in candidates)) is None - - -def test_failed_selected_holdout_vetoes_resolver_without_trying_runner_up(monkeypatch): - from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver - from test_motion_image_diagnostics import frames, RELATIONS - - outcomes = iter(((hypothesis(.2, 1.6, "image_motion_reprojection_failed"), object(), np.array([1.6])), - (hypothesis(.3, .1), object(), np.array([.1])))) - monkeypatch.setattr(solver, "_fit_hypothesis", lambda *args, **kwargs: next(outcomes)) - result = solver.resolve_image_motion(frames(alternatives=True), RELATIONS) - assert not result.resolved and result.model is None - assert result.reason == "image_motion_no_observable_image_model" - - -def test_validation_exception_preserves_training_identity(monkeypatch): - from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver - from linkerhand_calibration.core.geometry.tag_pose.motion_image_solver import ImageHingeBundle - from test_motion_image_diagnostics import frames, RELATIONS - - def fail(*args, **kwargs): - raise ValueError("validation_failed") - monkeypatch.setattr(ImageHingeBundle, "validate", fail) - result = solver.resolve_image_motion(frames(), RELATIONS) - assert not result.resolved - assert result.hypotheses[0].training_accepted - assert result.hypotheses[0].reason == "validation_failed" - - -@pytest.mark.parametrize("values,expected", [ - ([(0, .008), (1, 0.), (2, 0.)], {0: .008, 1: 0., 2: 0.}), - ([(0, .008), (1, .009), (2, 0.)], {0: .016, 1: .016, 2: 0.}), - ([(0, .001), (1, .005), (2, .008)], {0: .003, 1: .01, 2: .01}), -]) -def test_holm_preserves_family_level_and_is_order_independent(values, expected): - for ordering in permutations(values): - assert dict(holm_adjusted_probabilities(ordering)) == pytest.approx(expected) - - -def test_holm_rejects_invalid_probabilities_and_duplicate_ids(): - assert dict(holm_adjusted_probabilities([(0, float("nan")), (1, -.1), (2, 1.1)])) == {0: 1., 1: 1., 2: 1.} - assert holm_adjusted_probabilities([]) == () - with pytest.raises(ValueError, match="duplicate"): - holm_adjusted_probabilities([(0, .01), (0, .02)]) - - -def test_comparison_keeps_pixel_margin_and_rejects_isolated_outlier(): - angles = np.linspace(0., 1.2, 72)[:, None] - baseline = np.full(72, .3) - assert compare_image_losses(baseline, baseline + .02, angles, .03) == 1. - assert compare_image_losses(baseline, baseline + .2, angles, .03) < .001 - outlier = baseline.copy() - outlier[-2:] += 20 - assert compare_image_losses(baseline, outlier, angles, .03) > .01 - assert compare_image_losses(baseline, baseline + .2, np.zeros_like(angles), .03) == 1. - assert compare_image_losses(baseline, baseline + np.nan, angles, .03) == 1. - # Exact image repetitions must not count as new motion evidence. - repeated = compare_image_losses(np.repeat(baseline, 4), np.repeat(baseline + .2, 4), - np.repeat(angles, 4, axis=0), .03) - assert repeated == compare_image_losses(baseline, baseline + .2, angles, .03) - - -def test_equivalence_matches_roles_and_rejects_missing_or_duplicate_roles(): - from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionFrameSelection - from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRoleSelection - from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - - pose = SquareTagPose((0., 0., 0., 1.), (0., 0., 1.), .1) - first = ImageMotionFrameSelection(True, "", (MotionRoleSelection("root", pose), MotionRoleSelection("tip", pose))) - assert equivalent_frame_selections(first, replace(first, selections=first.selections[::-1])) - assert not equivalent_frame_selections(first, replace(first, selections=first.selections[:1])) - assert not equivalent_frame_selections(first, replace(first, selections=(first.selections[0],)*2)) - assert not equivalent_frame_selections(first, replace(first, resolved=False)) - - -@pytest.mark.parametrize("attempt,frames_count", [(0, 96), (1, 95)]) -def test_recorded_side_attempts_pass_with_original_geometry_and_quality_limits(attempt, frames_count): - from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry - from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion - from test_production_image_motion import recorded_image_frames - - fixture = Path(__file__).parent / "fixtures/o6_side_preparation_193254.json.gz" - case = json.loads(gzip.decompress(fixture.read_bytes()))["cases"][attempt] - frames = recorded_image_frames(case["frames"]) - assert len(frames) == frames_count - result = resolve_image_motion(frames, tuple(MotionRelation(**r) for r in case["relations"]), - geometry_constraints=tuple(ParallelAxisGeometry(**c) for c in case["constraints"]), - source_urdf_sha256=case["source_urdf_sha256"]) - assert result.resolved, result.reason - 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) - winner = training_candidate_index(result.hypotheses) - assert result.model.branches == result.hypotheses[winner].branches - adjusted = result.hypotheses[winner].comparison_adjusted_p_values - assert len(adjusted) == 3 and max(p for _, p in adjusted) <= .01 diff --git a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py deleted file mode 100644 index 59a7752..0000000 --- a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py +++ /dev/null @@ -1,223 +0,0 @@ -"""Capture persists the actual frozen image solver boundary for read-back.""" - -from copy import deepcopy -from dataclasses import replace -from types import SimpleNamespace -import json - -import numpy as np -import pytest - -from linkerhand_calibration.core.geometry.image_motion_replay import replay_image_models, validate_image_sample -from linkerhand_calibration.core.geometry.frozen_evidence import EvidenceDecoder -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import image_motion_model_from_dict -from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose -from linkerhand_calibration.runtime import capture as capture_module -from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from test_image_motion_provenance import image_motion_capture - - -def capture_rig(monkeypatch): - profile, records = image_motion_capture() - profile = replace(profile, measurement=replace(profile.measurement, candidate_selection_tasks=frozenset())) - original = next(row for row in records if row["kind"] == "pnp_candidate_frame") - task = profile.motion.tasks[0] - moving = next(role for role, item in original["roles"].items() if "frozen_image_model" in item) - root = next(role for role in original["roles"] if role != moving) - model = image_motion_model_from_dict(original["roles"][moving]["frozen_image_model"]) - branch = SimpleNamespace(task_name=task.key, view=task.view, constraints=(), - image_model=model, evidence_ids=((moving, "a" * 64),)) - - class Tracker: - maximum_reprojection_error_px = 1.5 - - def estimate(self, role, corners, *, stamp_ns, **kwargs): - self.stamp = stamp_ns - return SquareTagPose(**original["roles"][role]["reprojection_valid_candidates"][0]), "" - - def observation_snapshot(self, role, stamp_ns): - item = original["roles"][role] - diagnostics = dict(item["candidate_diagnostics"], observation_stamp_ns=stamp_ns) - return tuple(SquareTagPose(**p) for p in item["reprojection_valid_candidates"]), diagnostics, 0 - - def freeze_roles(self, *args, **kwargs): - return True - - tracker = Tracker() - lock = SimpleNamespace(fixed_tag_by_view={task.view: 6}, missing_views=set(), locking_enabled=False, - observe=lambda *args: None) - capture = ObservationCapture(profile, reference_lock=lock, trackers={task.view: tracker}, - extrinsics=SimpleNamespace(transform=lambda _: np.eye(4))) - capture.locked_poses[task.view] = SquareTagPose(**original["roles"][root]["selected"]) - capture.locked_revisions[task.view] = 7 - capture.install_motion_model(branch) - - class SelectedEvidence: - def observation_snapshot(self, role, stamp_ns): - candidates, diagnostics, corrections = tracker.observation_snapshot(role, stamp_ns) - if role == moving: - diagnostics.update(frozen_image_model=original["roles"][role]["frozen_image_model"], - image_model_sha256=original["roles"][role]["image_model_sha256"], - motion_evidence_verified=True, motion_evidence_id="a" * 64) - return candidates, diagnostics, corrections - - def select(frame, model, tracker, selected, *, base, cached): - return {moving: SquareTagPose(**original["roles"][moving]["selected"])}, {}, SelectedEvidence() - - monkeypatch.setattr(capture_module, "select_motion_poses", select) - values = tuple(profile.command.baseline_values) - frame = CaptureFrame(task.view, 1000, np.asarray(original["camera_matrix"]), - {role: np.asarray(item["corners_xy"]) for role, item in original["roles"].items()}, values, values) - return profile, capture, frame, branch - - -@pytest.mark.parametrize("phase", ["joint_zero", "steady", "sweep"]) -def test_actual_capture_writes_replayable_image_model_when_candidate_strategy_is_empty(monkeypatch, phase): - profile, capture, frame, branch = capture_rig(monkeypatch) - task = profile.motion.tasks[0] - motion = MotionCommand(phase, profile.command.baseline_values, 40., task_key=task.key, - command_index=task.command_index, cycle=0, direction="increasing", zero_joints=task.joints) - rows, movement = capture.consume(frame, motion) - assert movement is None - candidates = next(row for row in rows if row["kind"] == "pnp_candidate_frame") - sample = next(row for row in rows if row["kind"] in {"joint_sample", "joint_zero_sample"}) - assert candidates["sample_phase"] == phase - replayed = replay_image_models(candidates) - validate_image_sample(sample, candidates, replayed) - moving = branch.image_model.geometry[0].relation.child_role - evidence = candidates["roles"][moving] - assert evidence["pose_source"] == "image_constrained_current_image" - assert evidence["selected"] != evidence["reprojection_valid_candidates"][0] - decoder = EvidenceDecoder() - persisted_frame = decoder.loads(json.dumps(candidates)) - persisted_sample = decoder.loads(json.dumps(sample)) - validate_image_sample(persisted_sample, persisted_frame, replay_image_models(persisted_frame)) - - -def test_unconfirmed_diagnostic_does_not_emit_formal_image_candidate_frame(monkeypatch): - profile, capture, frame, _ = capture_rig(monkeypatch) - task = profile.motion.tasks[0] - motion = MotionCommand("sweep", profile.command.baseline_values, 40., task_key=task.key, - command_index=task.command_index, cycle=0, direction="increasing") - rows, _ = capture.consume(frame, motion, diagnostic=True, accepted_joints=frozenset()) - assert [row["kind"] for row in rows] == ["diagnostic_observation_frame"] - - -@pytest.mark.parametrize('reason', [ - 'pose_image_motion_model_changed', - 'pose_image_motion_decision_invalid', - 'pose_observation_superseded', -]) -def test_failed_commit_keeps_original_reason_without_authorizing_missing_snapshot(monkeypatch, reason): - profile, capture, frame, branch = capture_rig(monkeypatch) - task = profile.motion.tasks[0] - role = branch.image_model.geometry[0].relation.child_role - evidence = SimpleNamespace(observation_snapshot=lambda *args: None) - monkeypatch.setattr(capture_module, 'select_motion_poses', - lambda *args, **kwargs: ({}, {role: reason}, evidence)) - motion = MotionCommand('joint_zero', profile.command.baseline_values, 40., - task_key=task.key, zero_joints=task.joints) - rows, _ = capture.consume(frame, motion) - event = next(row for row in rows if row['kind'] == 'pnp_branch_event' and row['tag_role'] == role) - assert event['reason'] == reason - assert event['pnp_observation_evidence']['selected_pose'] is None - assert not event['pnp_observation_evidence']['candidate_diagnostics'] - assert not any(row['kind'] in {'joint_zero_sample', 'pnp_candidate_frame'} for row in rows) - - -@pytest.mark.parametrize("feedback_available", [True, False]) -def test_diagnostic_hold_keeps_transient_raw_images_before_formal_steady_window(monkeypatch, feedback_available): - profile, capture, frame, _ = capture_rig(monkeypatch) - task = profile.motion.tasks[0] - command, feedback, target = [list(profile.command.baseline_values) for _ in range(3)] - command[task.command_index], feedback[task.command_index], target[task.command_index] = 224., 230., 192. - frame = replace(frame, command=tuple(command), feedback=tuple(feedback) if feedback_available else None) - motion = MotionCommand("steady", tuple(target), 40., task_key=task.key, - command_index=task.command_index, cycle=0, direction="decreasing", steady_index=0, - minimum_hold_seconds=10.) - rows, _ = capture.consume(frame, motion, diagnostic=True, allow_recording=False, - accepted_joints=frozenset(task.joints)) - assert [row["kind"] for row in rows] == ["diagnostic_observation_frame"] - raw = rows[0] - assert raw["sample_phase"] == "steady" and raw["steady_target"] == 192. - assert raw["steady_index"] == 0 and raw["minimum_hold_seconds"] == 10. - assert raw["target_command_vector"] == target and raw["command_vector"] == command - assert raw["feedback_vector"] == (feedback if feedback_available else None) - assert not raw["accepted_sample_window_open"] and not raw["is_accepted_joint_sample"] - assert raw["tags"] and raw["camera_matrix"] == frame.camera_matrix.tolist() - - -def test_formal_capture_does_not_record_unstable_steady_images(monkeypatch): - profile, capture, frame, _ = capture_rig(monkeypatch) - task = profile.motion.tasks[0] - motion = MotionCommand("steady", profile.command.baseline_values, 40., task_key=task.key, - command_index=task.command_index, cycle=0, direction="decreasing", steady_index=0) - rows, _ = capture.consume(frame, motion, diagnostic=False, allow_recording=False) - assert not rows - - -@pytest.mark.parametrize("phase", ["joint_zero", "sweep"]) -def test_missing_image_model_root_cannot_be_replaced_by_stationary_cache(monkeypatch, phase): - profile, capture, frame, branch = capture_rig(monkeypatch) - task = profile.motion.tasks[0] - root = branch.image_model.geometry[0].relation.parent_role - frame = replace(frame, corners={role: points for role, points in frame.corners.items() if role != root}) - motion = MotionCommand(phase, profile.command.baseline_values, 40., task_key=task.key, - command_index=task.command_index, cycle=0, direction="increasing", zero_joints=task.joints) - rows, movement = capture.consume(frame, motion) - assert movement is None - assert not any(row["kind"] in {"joint_sample", "joint_zero_sample", "pnp_candidate_frame"} for row in rows) - - -def test_installed_image_model_is_managed_even_without_current_task(monkeypatch): - _, capture, frame, branch = capture_rig(monkeypatch) - tracker = capture.trackers[frame.view] - estimate = tracker.estimate - children = {item.relation.child_role for item in branch.image_model.geometry} - observed = set() - - def check_managed(role, *args, **kwargs): - if role in children: - assert kwargs.get("motion_managed") is True - observed.add(role) - return estimate(role, *args, **kwargs) - - monkeypatch.setattr(tracker, "estimate", check_managed) - rows, movement = capture.consume(frame, None) - assert observed == children and movement is None - assert not rows - - -def test_current_task_resolves_installed_parent_model_first_and_keeps_ancestral_roles(monkeypatch): - profile, capture, _, branch = capture_rig(monkeypatch) - parent = deepcopy(branch) - parent.task_name = "previous_joint_task" - first = parent.image_model.geometry[0] - child = deepcopy(branch) - child.image_model = replace(branch.image_model, - geometry=(replace(first, relation=replace(first.relation, - parent_role=first.relation.child_role, child_role="terminal")),), - tag_sizes=((first.relation.child_role, .016), ("terminal", .016)), - branches=((first.relation.child_role, 0), ("terminal", 0))) - child.evidence_ids = (("terminal", "b" * 64),) - capture._motion_models.clear() - capture.install_motion_model(parent) - capture.install_motion_model(child) - motion = SimpleNamespace(task_key=child.task_name) - assert capture._motion_model_schedule(child.view, motion) == (parent, child) - evidence = {"terminal": {"frozen_image_model": {"tag_sizes": child.image_model.tag_sizes}}, - first.relation.child_role: {"frozen_image_model": {"tag_sizes": parent.image_model.tag_sizes}}} - assert capture._image_model_roles({"terminal"}, evidence) == { - "terminal", first.relation.parent_role, first.relation.child_role} - - -def test_installation_cannot_change_an_existing_physical_tag_model(monkeypatch): - _, capture, _, branch = capture_rig(monkeypatch) - replacement = deepcopy(branch) - replacement.task_name = "another_task" - geometry = replacement.image_model.geometry[0] - replacement.image_model = replace(replacement.image_model, - geometry=(replace(geometry, pivot_parent_xyz_m=(.1, .1, .1)),)) - with pytest.raises(ValueError, match="image_motion_model_replacement_forbidden"): - capture.install_motion_model(replacement) diff --git a/src/linkerhand_calibration/test/test_image_motion_live_flow.py b/src/linkerhand_calibration/test/test_image_motion_live_flow.py deleted file mode 100644 index aeac63a..0000000 --- a/src/linkerhand_calibration/test/test_image_motion_live_flow.py +++ /dev/null @@ -1,79 +0,0 @@ -"""One coordinator transport run using the complete recorded thumb image arc. - -SDK commands select the nearest recorded image; timestamps are virtual. This -exercises the production image solver, new tracker commit and evidence writer, -not hand accuracy or an independent camera truth dataset. -""" -import gzip -from dataclasses import replace -import json -from pathlib import Path - -import numpy as np -import pytest - -from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection -from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance, validate_source_geometry -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from test_diagnostic_coordinator import run_until, THUMB_JOINTS -from runtime_host_fixture import coordinator_fixture, ready - - -@pytest.mark.parametrize("reference_mode", ["tag_pose", "stationary_image"]) -def test_recorded_image_geometry_completes_thumb_coordinator_without_publication(tmp_path, monkeypatch, reference_mode): - import test_diagnostic_coordinator as transport - payload = json.loads(gzip.decompress((Path(__file__).parent / "fixtures" / - "o6_image_live_flow_200044.json.gz").read_bytes())) - assert payload["source_sha256"] == "52f9ed7510fc58a4271532a2dc13a678f2e4aa2f1dc3153bf749fcf377cb23c5" - host, clock = coordinator_fixture(tmp_path, diagnostic_capture="o6_thumb", - profile_transform=lambda p: replace(p, acquisition=replace(p.acquisition, - fixed_reference_mode=reference_mode))) - ready(host, clock) - assert host.branch_initialization.image_geometry - for view, data in payload["views"].items(): - host.cameras.matrices[view] = np.asarray(data["camera_matrix"]) - host.cameras.image_sizes[view] = (2048, 2048) - monkeypatch.setattr(host.finalization, "start", lambda *a, **kw: pytest.fail("diagnostic started finalizer")) - assert host.start().success - - def recorded_frame(host, view, feedback): - phase = host._motion.phase if host._motion is not None else "joint_zero" - target = feedback[0 if view == "front" else 1] - pool = payload["views"][view]["frames"] - # Preparation exclusively uses recorded preparation images. A later - # direction cannot silently enter the geometry-training callback. - selected_phase = "sweep" if phase == "sweep" else "zero_approach" - available = [row for row in pool if row["phase"] == selected_phase] - if phase == "sweep": - direction = host._motion.direction - matching = [row for row in available if row["direction"] == direction] - available = matching or available - row = min(available, key=lambda r: abs(r["input"] - target)) - return DetectionInput(view, host.ports.clock_ns(), tuple(TagDetection(tag["id"], - tuple(tuple(p) for p in tag["corners"]), 0, 100.) for tag in row["tags"])) - - monkeypatch.setattr(transport, "projected_frame", recorded_frame) - try: - run_until(host, clock, lambda: host.execution.session.phase == Phase.DIAGNOSTIC_COMPLETE) - assert set(host.execution.zero_references) == THUMB_JOINTS - assert not host.execution.diagnostic_zero_attempts - assert not host.finalization.started and not host.final_json and not host.final_urdf - records = host.raw_records - initializations = [r for r in records if r["kind"] == "motion_branch_initialization"] - assert len(initializations) == 2 and all(r["status"] == "resolved" for r in initializations) - assert all(r.get("frozen_image_model") and r.get("image_model_sha256") for r in initializations) - samples = [r for r in records if r["kind"] == "joint_sample"] - assert {r["joint"] for r in samples} == THUMB_JOINTS - assert {r["direction"] for r in samples} == {"increasing", "decreasing"} - assert len(host.execution.completed_units) == 4 - proof = validate_motion_provenance(host.profile, records, required=True) - assert proof["motion_evidence_verified"] and not proof["is_accuracy_certificate"] - validate_source_geometry(host.profile, host.parameters.source_urdf, records) - from linkerhand_calibration.core.artifacts.storage import load_jsonl - journal = load_jsonl(host.raw_path) - assert json.loads(json.dumps(records[0])) == journal[0] - assert validate_motion_provenance(host.profile, journal, required=True) == proof - with pytest.raises(ValueError, match="image_model_source_urdf_changed"): - validate_motion_provenance(host.profile, records[1:], required=True) - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_image_motion_provenance.py b/src/linkerhand_calibration/test/test_image_motion_provenance.py deleted file mode 100644 index a9d0b1b..0000000 --- a/src/linkerhand_calibration/test/test_image_motion_provenance.py +++ /dev/null @@ -1,270 +0,0 @@ -"""Constrained image poses keep raw candidates and survive immutable replay.""" - -from copy import deepcopy -from dataclasses import asdict, replace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.image_motion_replay import ( - IMAGE_POSE_SOURCE, image_model_sha256, replay_image_models, validate_image_sample, -) -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionModel -from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageHingeGeometry -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import select_image_motion_frame -from test_motion_image_diagnostics import MATRIX, frames - - -def recorded_image(*, stamp=None, cycle=0): - model = ImageMotionModel((ImageHingeGeometry(MotionRelation("hinge", "root", "moving"), - (0., 0., 1.), (0., 0., 0., 1.), (.01, .02, 0.), (.035, 0., 0.)),), MATRIX, - (("root", .016), ("moving", .016)), (1, 2), (1,), (2,), (("root", 0), ("moving", 0))) - original = frames(depth_error=.008)[24] - if stamp is not None: - original = replace(original, evidence=replace(original.evidence, stamp_ns=stamp)) - result = select_image_motion_frame(model, original) - assert result.resolved, result - selected = {item.role: asdict(item.pose) for item in result.selections} - observations = {item.role: item for item in original.observations} - stamp = original.evidence.stamp_ns - frame = {"kind": "pnp_candidate_frame", "task_name": "task", "view": "front", - "image_stamp_ns": stamp, "cycle": cycle, "direction": "increasing", "attempt": 1, - "tag_size_m": .016, "camera_matrix": MATRIX, - "camera_matrix_source": "CameraInfo.P[:3,:3]", "input_is_rectified": True, - "roles": {}} - evidence = {} - for candidates in original.evidence.roles: - role = candidates.role - data = {"corners_xy": observations[role].corners_xy, "tag_size_m": .016, - "selected": selected[role], "reprojection_valid_candidates": [asdict(p) for p in candidates.candidates], - "pose_source": IMAGE_POSE_SOURCE if role == "moving" else "verified_fixed_reference", - "locked_reference": role == "root"} - if role == "moving": - data.update(frozen_image_model=asdict(model), image_model_sha256=image_model_sha256(asdict(model))) - frame["roles"][role] = data - evidence["parent" if role == "root" else "child"] = {**data, "tag_role": role, - "selected_pose": selected[role], "reference_branch_revision": 1, - "candidate_diagnostics": {"branch_frozen": True, "branch_revision": 1, - "branch_status": "tracking", "observation_stamp_ns": stamp, - "fixed_reference_verified": role == "root"}} - data["candidate_diagnostics"] = evidence["parent" if role == "root" else "child"]["candidate_diagnostics"] - rp, rc = (Rotation.from_quat(selected[r]["quaternion_xyzw"]) for r in ("root", "moving")) - row = {key: frame[key] for key in ("task_name", "view", "image_stamp_ns", "cycle", "direction", "attempt")} - row.update(kind="joint_sample", joint="hinge", sample_phase="sweep", pnp_observation_evidence=evidence, - parent_pose_common=selected["root"], child_pose_common=selected["moving"], - relative_quaternion_xyzw=(rp.inv()*rc).as_quat().tolist(), - relative_translation_xyz_m=rp.inv().apply(np.subtract(selected["moving"]["translation_xyz_m"], - selected["root"]["translation_xyz_m"])).tolist()) - return frame, row - - -def test_image_pose_replay_is_distinct_from_raw_pnp_and_never_mutates_capture(): - frame, row = recorded_image() - before = deepcopy((frame, row)) - replay = replay_image_models(frame) - validate_image_sample(row, frame, replay) - assert frame == before[0] and row == before[1] - assert frame["roles"]["moving"]["selected"] != frame["roles"]["moving"]["reprojection_valid_candidates"][0] - - -@pytest.mark.parametrize("mutation", ["model", "hash", "projection", "size", "corners", "selected", - "source", "raw_candidates", "future_support"]) -def test_frozen_model_and_current_image_evidence_cannot_be_substituted(mutation): - frame, _ = recorded_image() - moving = frame["roles"]["moving"] - if mutation == "model": - moving["frozen_image_model"]["geometry"][0]["pivot_parent_xyz_m"] = (.2, .2, .2) - elif mutation == "hash": - moving["image_model_sha256"] = "a" * 64 - elif mutation == "projection": - frame["camera_matrix_source"] = "CameraInfo.K" - elif mutation == "size": - moving["tag_size_m"] = .02 - elif mutation == "corners": - moving["corners_xy"] = tuple((x + 15., y) for x, y in moving["corners_xy"]) - elif mutation == "selected": - moving["selected"] = {**moving["selected"], "translation_xyz_m": (.1, .1, .8)} - elif mutation == "source": - moving["pose_source"] = "current_image" - elif mutation == "raw_candidates": - moving.pop("reprojection_valid_candidates") - else: - frame["image_stamp_ns"] = 2 - with pytest.raises(ValueError, match="image_motion_provenance_"): - replay_image_models(frame) - - -@pytest.mark.parametrize("mutation", ["sample_model", "sample_corners", "sample_pose", "relative", "identity", "marker"]) -def test_sample_must_match_the_replayed_frozen_image(mutation): - frame, row = recorded_image() - row = deepcopy(row) - item = row["pnp_observation_evidence"]["child"] - if mutation == "sample_model": - item["image_model_sha256"] = "a" * 64 - elif mutation == "sample_corners": - item["corners_xy"] = ((0., 0.),) * 4 - elif mutation == "sample_pose": - item["selected_pose"]["translation_xyz_m"] = (.1, .1, .8) - elif mutation == "relative": - row["relative_translation_xyz_m"] = [0., 0., 0.] - elif mutation == "identity": - row["image_stamp_ns"] += 1 - else: - item["pose_source"] = "current_image" - item.pop("frozen_image_model") - item.pop("image_model_sha256") - with pytest.raises(ValueError, match="image_motion_provenance_"): - validate_image_sample(row, frame, replay_image_models(frame)) - - -def test_articulated_finalizer_replays_model_without_running_ippe_or_refitting(): - from linkerhand_calibration.core.geometry.candidate_selection import resolve_chain_observations - frame, row = recorded_image() - records = [] - # Distinct images with equal pose are sufficient for this read-back test; - # the separate motion initializer and final fitter enforce motion span. - for index in range(65): - current_frame, current_row = deepcopy(frame), deepcopy(row) - for record in (current_frame, current_row): - record.update(image_stamp_ns=100 + index, cycle=0 if index < 40 else 3) - for item in current_row["pnp_observation_evidence"].values(): - item["candidate_diagnostics"]["observation_stamp_ns"] = 100 + index - for item in current_frame["roles"].values(): - item["candidate_diagnostics"]["observation_stamp_ns"] = 100 + index - records.extend((current_frame, current_row)) - before = deepcopy(records) - result, report = resolve_chain_observations(records, task_key="task", role_pairs={"hinge": ("root", "moving")}, - fixed_roles={"root"}, require_frozen_branches=True, - solve_pose=lambda *args, **kwargs: pytest.fail("IPPE reselection would change frozen geometry")) - assert result == before == records - assert report["changed_joint_rows"] == 0 and report["geometry_refitted"] is False - assert report["holdout_frames"] == 25 - - -def image_motion_capture(): - """Construct a journal from an independently specified exact hinge model. - - Solver observability is tested in its own module. Here a known model and - images isolate read-back, authorization and zero/holdout identity handling. - """ - from linkerhand_calibration.core.domain.reference import build_joint_zero_reference - from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 - from test_motion_reference_provenance import motion_capture - - profile, old_reference, records = motion_capture(source_stamps=tuple(range(10, 34))) - # The independent projected hinge fixture uses 16 mm tags. Match the - # capture profile to that geometry instead of inheriting live tag sizes. - profile = replace(profile, vision=replace(profile.vision, views=tuple( - replace(view, tags=tuple(replace(tag, size_m=.016) for tag in view.tags)) - for view in profile.vision.views))) - name = old_reference.joint - spec = profile.measurement.measurements[name] - role_names = {"root": spec.parent_role, "moving": spec.child_role} - original_frame, original_sample = recorded_image() - raw_model = original_frame["roles"]["moving"]["frozen_image_model"] - raw_model["geometry"][0]["relation"] = dict(joint=name, parent_role=spec.parent_role, child_role=spec.child_role) - raw_model.update(tag_sizes=tuple((role_names[role], size) for role, size in raw_model["tag_sizes"]), - branches=tuple((role_names[role], branch) for role, branch in raw_model["branches"]), - source_stamps_ns=tuple(range(10, 34)), training_stamps_ns=tuple(range(10, 22)), - validation_stamps_ns=tuple(range(22, 34))) - digest = image_model_sha256(raw_model) - original_frame["roles"] = {role_names[role]: item for role, item in original_frame["roles"].items()} - moving = original_frame["roles"][spec.child_role] - moving.update(frozen_image_model=raw_model, image_model_sha256=digest) - for item in original_sample["pnp_observation_evidence"].values(): - item["tag_role"] = role_names[item["tag_role"]] - if item.get("frozen_image_model"): - item.update(frozen_image_model=raw_model, image_model_sha256=digest) - sources = [row for row in records if row["kind"] == "motion_branch_observation"] - for source in sources: - source.update(camera_matrix=MATRIX, input_is_rectified=True, sample_phase="zero_approach", - tags={role: {"tag_size_m": item["tag_size_m"], "corners_xy": item["corners_xy"], - "reprojection_valid_candidates": item["reprojection_valid_candidates"]} - for role, item in original_frame["roles"].items()}) - audit = next(row for row in records if row["kind"] == "motion_branch_initialization") - audit.update(constraints=[], frozen_image_model=raw_model, image_model_sha256=digest, - source_frame_hashes={str(row["image_stamp_ns"]): source_frame_sha256(row) for row in sources}) - payload = {key: audit[key] for key in ("task_name", "view", "zero_joints", "session_epoch", - "motion_version", "source_image_stamps", "source_frame_hashes", "constraints", - "frozen_image_model", "image_model_sha256")} - payload["tag_role"] = spec.child_role - evidence_id = image_model_sha256(payload) - audit.update(evidence_ids={spec.child_role: evidence_id}, evidence_payloads={spec.child_role: payload}) - output = [*sources, audit] - zero_rows = [] - for row in records: - if row["kind"] not in {"joint_zero_sample", "joint_sample"}: - continue - frame = deepcopy(original_frame) - for key in ("task_name", "view", "image_stamp_ns", "cycle", "direction", "attempt"): - frame[key] = row.get(key) - for item in frame["roles"].values(): - item["candidate_diagnostics"]["observation_stamp_ns"] = row["image_stamp_ns"] - row = deepcopy(row) - for field in ("pnp_observation_evidence", "parent_pose_common", "child_pose_common", - "relative_quaternion_xyzw", "relative_translation_xyz_m"): - row[field] = deepcopy(original_sample[field]) - for item in row["pnp_observation_evidence"].values(): - item["candidate_diagnostics"].update(observation_stamp_ns=row["image_stamp_ns"], - branch_frozen=row["kind"] != "joint_zero_sample", branch_revision=7) - if item["tag_role"] == spec.child_role: - item["candidate_diagnostics"].update(motion_evidence_verified=True, motion_evidence_id=evidence_id) - output.extend((frame, row)) - if row["kind"] == "joint_zero_sample": - zero_rows.append(row) - reference = build_joint_zero_reference(profile, name, zero_rows, session_epoch=1, motion_version=2) - output.append(reference.as_record()) - return profile, output - - -def test_new_model_reference_and_fourth_round_remain_bound_to_pre_zero_images(): - from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance - profile, records = image_motion_capture() - result = validate_motion_provenance(profile, records, require_complete=True) - assert result["motion_evidence_verified"] and not result["is_accuracy_certificate"] - - -def test_resume_preserves_zero_image_frames_for_later_provenance_replay(): - from linkerhand_calibration.core.domain.reference import read_joint_zero_references - from linkerhand_calibration.runtime.joint_resume import JointResume - from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance - - profile, records = image_motion_capture() - reference = next(iter(read_joint_zero_references(profile, records).values())) - zero_images = {(s.view, s.image_stamp_ns) for s in reference.samples} - expected = [r for r in records if r["kind"] == "pnp_candidate_frame" - and (r["view"], r["image_stamp_ns"]) in zero_images] - assert expected - for _ in range(2): - resume = JointResume(profile) - resume.references, resume.rows = {reference.joint: reference}, tuple(records) - # This test isolates transport of the frozen zero's original image - # evidence; independent new-pose verification is tested separately. - assert resume.verify(reference) is reference - copied = resume.take_motion_provenance_records() - assert [r for r in copied if r["kind"] == "pnp_candidate_frame"] == expected - records = [*copied, reference.as_record()] - assert validate_motion_provenance(profile, records, required=True)["motion_evidence_verified"] - - -@pytest.mark.parametrize("mutation", ["missing_frame", "source_changed", "holdout_changed", "old_model_disguised"]) -def test_final_provenance_does_not_accept_unreplayable_image_geometry(mutation): - from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance - profile, records = image_motion_capture() - if mutation == "missing_frame": - records = [row for row in records if row["kind"] != "pnp_candidate_frame"] - elif mutation == "source_changed": - records[0]["camera_matrix"] = ((1700., 0., 640.), *MATRIX[1:]) - else: - row = next(row for row in records if row.get("kind") == "joint_sample" and row.get("cycle") == 3) - item = row["pnp_observation_evidence"]["child"] - if mutation == "holdout_changed": - item["image_model_sha256"] = "a" * 64 - else: - item["pose_source"] = "current_image" - item.pop("frozen_image_model") - item.pop("image_model_sha256") - with pytest.raises(ValueError, match="provenance_"): - validate_motion_provenance(profile, records, require_complete=True) diff --git a/src/linkerhand_calibration/test/test_image_motion_tracker_commit.py b/src/linkerhand_calibration/test/test_image_motion_tracker_commit.py deleted file mode 100644 index 65281be..0000000 --- a/src/linkerhand_calibration/test/test_image_motion_tracker_commit.py +++ /dev/null @@ -1,150 +0,0 @@ -"""The constrained-pose bridge verifies pixels once and commits metadata atomically.""" -from dataclasses import replace -from threading import Thread - -import pytest - -from linkerhand_calibration.core.geometry.tag_pose import tracking, image_motion_commit -from linkerhand_calibration.core.geometry.tag_pose.fixed_reference import FixedTagReference -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import RoleCandidates -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion -from linkerhand_calibration.core.geometry.tag_pose.tracking import MotionPoseSelection, SquareTagPoseTracker -from test_motion_image_diagnostics import frames, RELATIONS - - -@pytest.fixture(scope="module") -def model_source(): - source = frames(depth_error=.008) - result = resolve_image_motion(source, RELATIONS) - assert result.resolved - return result.model, source - - -@pytest.fixture -def observed(monkeypatch, model_source): - model, source = model_source - tracker = SquareTagPoseTracker() - root = source[0].evidence.roles[0].candidates[0] - root_observation = source[0].observations[0] - monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: [root]) - for index in range(5): - selected, _ = tracker.estimate("root", root_observation.corners_xy, tag_size_m=.016, - camera_matrix=source[0].camera_matrix, stamp_ns=100_000_000 + index * 50_000_000) - assert selected is not None - snapshot = tracker.motion_snapshot("root", 300_000_000) - assert tracker.freeze_roles({"root": snapshot.revision}) - reference = FixedTagReference(selected, snapshot.revision) - - def observe(frame): - for role, observation in zip(frame.evidence.roles, frame.observations): - monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, _p=role.candidates, **k: list(_p)) - tracker.estimate(role.role, observation.corners_xy, tag_size_m=observation.tag_size_m, - camera_matrix=frame.camera_matrix, stamp_ns=frame.evidence.stamp_ns, - fixed_reference=reference if role.role == "root" else None, - motion_managed=role.role != "root") - actual_root = tracker.motion_snapshot("root", frame.evidence.stamp_ns) - return replace(frame, evidence=replace(frame.evidence, roles=( - RoleCandidates("root", (actual_root.previous_pose,), True), frame.evidence.roles[1]))) - - frame = observe(source[12]) - return tracker, model, frame, observe, source - - -def verify(observed): - tracker, model, frame, _, _ = observed - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - return tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "evidence"}) - - -def test_constrained_pose_has_its_own_verified_commit_without_changing_pnp(observed, monkeypatch): - tracker, _, frame, _, _ = observed - original = tracker.last_candidates_by_role["moving"] - calls = [] - solve = image_motion_commit.select_image_motion_frame - def unlocked(model, frame): - completed = [] - thread = Thread(target=lambda: completed.append(tracker.motion_snapshot("moving", frame.evidence.stamp_ns))) - thread.start(); thread.join(timeout=1) - assert completed, "numerical image solve held the tracker lock" - calls.append(1) - return solve(model, frame) - monkeypatch.setattr(image_motion_commit, "select_image_motion_frame", unlocked) - decision = verify(observed) - assert decision.accepted, decision - assert decision.selections[0].pose not in original - legacy = tracker.commit_motion_frame(decision.selections, establish=True) - assert not legacy.accepted and legacy.reason == "pose_motion_selection_invalid" - result = tracker.commit_image_motion_frame(decision, establish=True) - assert result.accepted and len(calls) == 1 - assert tracker.last_candidates_by_role["moving"] == original - diagnostics = result.observation_snapshots[0].diagnostics - assert diagnostics["pose_source"] == "image_constrained_current_image" - assert diagnostics["frozen_image_model"]["policy"] == decision.model.policy - assert diagnostics["image_model_sha256"] == decision.model_sha256 - assert diagnostics["image_motion_quality"] - assert not tracker.commit_image_motion_frame(decision, establish=True).accepted - - -def test_copied_decision_cannot_authorize_an_arbitrary_pose(observed): - tracker, _, _, _, _ = observed - decision = verify(observed) - assert decision.accepted - pose = replace(decision.selections[0].pose, translation_xyz_m=(0., 0., .1)) - forged = replace(decision, selections=(replace(decision.selections[0], pose=pose),)) - assert tracker.commit_image_motion_frame(forged, establish=True).reason == "pose_image_motion_decision_invalid" - assert tracker.commit_image_motion_frame(decision, establish=True).accepted - - -def test_child_or_root_arriving_after_solve_invalidates_entire_decision(observed): - tracker, _, _, observe, source = observed - decision = verify(observed) - observe(source[13]) - result = tracker.commit_image_motion_frame(decision, establish=True) - assert not result.accepted and result.reason == "pose_observation_superseded" - assert tracker._branches.states["moving"].motion_evidence_id is None - - -def test_different_pixels_cannot_be_substituted_before_verification(observed): - tracker, model, frame, _, _ = observed - shifted = replace(frame.observations[1], corners_xy=tuple((x + .1, y) for x, y in frame.observations[1].corners_xy)) - frame = replace(frame, observations=(frame.observations[0], shifted)) - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - decision = tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "evidence"}) - assert not decision.accepted and decision.reason == "pose_image_motion_observation_mismatch" - - -def test_reset_and_freeze_cannot_adopt_a_delayed_model(observed): - tracker, _, _, _, _ = observed - decision = verify(observed) - tracker.reset() - assert tracker.commit_image_motion_frame(decision, establish=True).reason == "pose_observation_superseded" - - -def test_frozen_identity_cannot_change_model_or_return_to_legacy_solver(observed): - tracker, model, frame, observe, source = observed - decision = verify(observed) - assert tracker.commit_image_motion_frame(decision, establish=True).accepted - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - assert tracker.freeze_roles({"moving": snapshot.revision}, require_motion_evidence=True) - frame = observe(source[13]) - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - legacy = MotionPoseSelection(snapshot, snapshot.candidates[0], "evidence") - assert tracker.commit_motion_frame((legacy,)).reason == "pose_motion_solver_changed" - changed = replace(model, source_stamps_ns=(*model.source_stamps_ns, model.source_stamps_ns[-1] + 1)) - decision = tracker.verify_image_motion_frame((snapshot,), changed, frame, {"moving": "evidence"}) - assert decision.accepted - assert tracker.commit_image_motion_frame(decision).reason == "pose_image_motion_model_changed" - decision = tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "evidence"}) - assert tracker.commit_image_motion_frame(decision).accepted - - -def test_only_the_fixed_root_advancing_also_invalidates_decision(observed, monkeypatch): - tracker, _, frame, _, _ = observed - decision = verify(observed) - root = tracker.motion_snapshot("root", frame.evidence.stamp_ns) - monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: [root.previous_pose]) - tracker.estimate("root", frame.observations[0].corners_xy, tag_size_m=.016, - camera_matrix=frame.camera_matrix, stamp_ns=frame.evidence.stamp_ns + 1, - fixed_reference=FixedTagReference(root.previous_pose, root.revision)) - assert tracker.commit_image_motion_frame(decision, establish=True).reason == "pose_observation_superseded" - assert tracker._branches.states["moving"].motion_evidence_id is None diff --git a/src/linkerhand_calibration/test/test_image_projection_worker.py b/src/linkerhand_calibration/test/test_image_projection_worker.py deleted file mode 100644 index b8de98d..0000000 --- a/src/linkerhand_calibration/test/test_image_projection_worker.py +++ /dev/null @@ -1,53 +0,0 @@ -"""Independent execution preserves current pixels, geometry and rejection.""" - -from dataclasses import replace -from types import SimpleNamespace - -from linkerhand_calibration.core.geometry.tag_pose.image_projection_worker import ImageProjectionWorker -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_motion_model_from_dict -from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame -from test_image_motion_provenance import recorded_image -from test_motion_image_diagnostics import frames -from test_image_motion_tracker_commit import observed, model_source - - -def test_process_projection_matches_direct_solution_and_rejects_changed_camera(): - recorded, _ = recorded_image() - model = image_motion_model_from_dict(recorded["roles"]["moving"]["frozen_image_model"]) - frame = frames(depth_error=.008)[24] - worker = ImageProjectionWorker(workers=1) - try: - actual = worker.solve(model, frame) - assert actual.resolved - expected = select_image_motion_frame(model, frame) - assert actual == expected - changed = replace(frame, camera_matrix=((1700., 0., 640.), *frame.camera_matrix[1:])) - assert worker.solve(model, changed).reason == "image_motion_camera_changed" - finally: - worker.close() - assert not worker.solve(model, frame).resolved - - -def test_process_result_still_requires_current_tracker_identity(observed): - tracker, model, frame, observe, source = observed - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - worker = ImageProjectionWorker(workers=1) - try: - decision = tracker.verify_image_motion_frame((snapshot,), model, frame, - {"moving": "evidence"}, projection_worker=worker) - assert decision.accepted - observe(source[13]) - result = tracker.commit_image_motion_frame(decision, establish=True) - assert not result.accepted and result.reason == "pose_observation_superseded" - assert tracker._branches.states["moving"].motion_evidence_id is None - finally: - worker.close() - - -def test_caller_cannot_substitute_a_pose_proposal_as_a_worker(observed): - tracker, model, frame, _, _ = observed - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - forged = SimpleNamespace(solve=lambda *_: select_image_motion_frame(model, frame)) - decision = tracker.verify_image_motion_frame((snapshot,), model, frame, - {"moving": "evidence"}, projection_worker=forged) - assert not decision.accepted and decision.reason == "pose_image_motion_worker_invalid" diff --git a/src/linkerhand_calibration/test/test_independent_urdf_export.py b/src/linkerhand_calibration/test/test_independent_urdf_export.py deleted file mode 100644 index a238f78..0000000 --- a/src/linkerhand_calibration/test/test_independent_urdf_export.py +++ /dev/null @@ -1,206 +0,0 @@ -"""A near-zero limit must never flatten a nonlinear passive joint's travel.""" - -from copy import deepcopy -import hashlib -import json -import xml.etree.ElementTree as ET - -import numpy as np -import pytest - -from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.core.urdf.patch import UrdfJointPatch, UrdfPatchSet, write_urdf_patches -from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan -from linkerhand_calibration.core.urdf.validate import validate_structural_urdf_diff -from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( - rebuild_urdf_from_json, serialize_urdf_correction, validate_json_urdf_correspondence, -) - - -@pytest.fixture -def nonlinear_release(tmp_path): - source = tmp_path / "original.urdf" - source.write_text(''' - - - - - - - -''') - digest = hashlib.sha256(source.read_bytes()).hexdigest() - rights = {name: ("origin.rpy", "limit.lower", "limit.upper", "mimic.multiplier", "mimic.offset") - for name in ("pitch", "ip")} - plan = build_standard_correction_plan(source_urdf=source, source_sha256=digest, - zero_offsets_rad={}, authorized_fields=rights, independent_joint_channels={"pitch": 0, "ip": 0}) - parent = np.linspace(0, .6, 256) - child = 1.71 * parent + .39 * parent**2 - payload = {"format": "unified_calibration_v3", "schema_version": 3, - "profile_id": "VIRTUAL/right/nonlinear/v3", "input_unit": "u8", "baseline_command": [0], - "joints": {name: {"sdk_channel": 0, "angle_rad": angles.tolist()} - for name, angles in (("pitch", parent), ("ip", child))}, - "urdf_correction": serialize_urdf_correction(plan)} - path = tmp_path / "calibration.json" - path.write_text(json.dumps(payload)) - return source, rights, plan, payload, path - - -def rebuild(data, output): - source, rights, _, payload, path = data - path.write_text(json.dumps(payload)) - return rebuild_urdf_from_json(path, source, output, authorized_fields=rights, - expected_profile_id="VIRTUAL/right/nonlinear/v3") - - -def test_nonlinear_angles_survive_normal_urdf_fk_and_json_rebuild(nonlinear_release, tmp_path): - source, _, plan, payload, _ = nonlinear_release - original = source.read_bytes() - output = rebuild(nonlinear_release, tmp_path / "corrected.urdf") - assert output.read_bytes() == plan.write(source, tmp_path / "direct.urdf").read_bytes() - validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) - model = UrdfKinematicModel(output) - mapping = SerializedJointMapping(payload, input_kind="command") - ratios = [] - for command in (32, 128, 255): - angles = mapping.evaluate([command], "", active_joints=("pitch", "ip")) - resolved = model.resolve_angles(angles) # No custom mimic override. - assert resolved == angles - ratios.append(resolved["ip"] / resolved["pitch"]) - expected = np.eye(4) - for name in ("pitch", "ip"): - joint = model.joints[name] - expected = expected @ joint.origin @ model._motion(joint, angles[name]) - assert np.allclose(model.link_transform("ip", zero_offsets={}, joint_angles=angles), expected) - assert ratios[-1] - ratios[0] > .2 - assert np.degrees(resolved["ip"]) > 66 - assert source.read_bytes() == original - for name, joint in model.joints.items(): - before = UrdfKinematicModel(source).joints[name] - assert joint.lower == before.lower and joint.upper == before.upper - assert np.array_equal(joint.origin, before.origin) - assert ET.parse(output).findall("./joint/mimic") == [] - - -@pytest.mark.parametrize("corruption", ["wrong_channel", "missing_curve", "wrong_parent", "missing_removal", - "legacy_removal", "coefficient_and_removal", "extra_removal", "changed_axis"]) -def test_incomplete_or_changed_coupling_cannot_rebuild(nonlinear_release, tmp_path, corruption): - source, rights, plan, original, path = nonlinear_release - payload = deepcopy(original) - correction = payload["urdf_correction"] - if corruption == "wrong_channel": - payload["joints"]["ip"]["sdk_channel"] = 1 - payload["baseline_command"].append(0) - elif corruption == "missing_curve": - del payload["joints"]["ip"] - elif corruption == "wrong_parent": - correction["passive_joint_sources"]["ip"] = "missing" - elif corruption == "missing_removal": - correction["joint_patches"]["ip"] = {"limit_upper": "1.2"} - elif corruption == "legacy_removal": - correction["schema_version"] = 1 - del correction["joint_angle_source"], correction["passive_joint_sources"] - elif corruption == "coefficient_and_removal": - correction["joint_patches"]["ip"]["mimic_multiplier"] = "2" - elif corruption == "extra_removal": - correction["joint_patches"]["pitch"] = {"remove_mimic": True} - else: - correction["joint_patches"]["ip"]["axis_xyz"] = "1 0 0" - output = tmp_path / "invalid.urdf" - with pytest.raises(ValueError): - rebuild((source, rights, plan, payload, path), output) - assert not output.exists() - - -def test_removal_requires_its_own_right_and_preserves_linkage(nonlinear_release, tmp_path): - source, rights, plan, payload, _ = nonlinear_release - with pytest.raises(ValueError, match="authorization"): - write_urdf_patches(source_urdf=source, destination_urdf=tmp_path / "unauthorized.urdf", - patches=UrdfPatchSet({"ip": UrdfJointPatch(remove_mimic=True)}), authorized_fields=rights) - output = rebuild(nonlinear_release, tmp_path / "valid.urdf") - changed = output.read_text().replace('parent link="proximal"', 'parent link="base"') - output.write_text(changed) - with pytest.raises(ValueError, match="topology"): - validate_structural_urdf_diff(source, output, authorized_fields=plan.authorized_fields) - with pytest.raises(ValueError, match="overridden"): - validate_compact_urdf_tables(payload, UrdfKinematicModel(source)) - - -def test_robot_state_publisher_preserves_json_passive_angles(nonlinear_release, tmp_path): - assert_robot_state_publisher_angles(nonlinear_release, tmp_path) - - -def assert_robot_state_publisher_angles(nonlinear_release, tmp_path, *, use_mimic=False): - """Use private topics, real ROS TF and no SDK or motor-command publisher.""" - rclpy = pytest.importorskip("rclpy") - from ament_index_python.packages import get_package_prefix, PackageNotFoundError - from pathlib import Path - import os - import subprocess - import time - import uuid - import yaml - from scipy.spatial.transform import Rotation - from sensor_msgs.msg import JointState - from tf2_msgs.msg import TFMessage - try: - executable = Path(get_package_prefix("robot_state_publisher")) / "lib/robot_state_publisher/robot_state_publisher" - except PackageNotFoundError: - pytest.skip("robot_state_publisher is not installed") - output = rebuild(nonlinear_release, tmp_path / "ros.urdf") - model = UrdfKinematicModel(output) - parameters = tmp_path / "parameters.yaml" - parameters.write_text(yaml.safe_dump({"/**": {"ros__parameters": { - "robot_description": output.read_text(), "publish_frequency": 100.0, "ignore_timestamp": True}}})) - namespace = "/nonlinear_export_test_" + uuid.uuid4().hex - context = rclpy.context.Context() - rclpy.init(context=context) - node = rclpy.create_node("json_angle_test", namespace=namespace, context=context) - executor = rclpy.executors.SingleThreadedExecutor(context=context) - executor.add_node(node) - messages = [] - subscription = node.create_subscription(TFMessage, namespace + "/tf", messages.append, 10) - publisher = node.create_publisher(JointState, namespace + "/joint_states", 10) - process = subprocess.Popen([str(executable), "--ros-args", "--params-file", str(parameters), - "-r", f"__ns:={namespace}", "-r", f"/tf:={namespace}/tf", - "-r", f"/tf_static:={namespace}/tf_static"], stdout=subprocess.DEVNULL, - stderr=subprocess.DEVNULL, env=dict(os.environ, ROS_LOG_DIR=str(tmp_path))) - try: - for command in (32, 128, 255): - payload = nonlinear_release[3] - angles = [payload["joints"][name]["angle_rad"][command] for name in ("pitch", "ip")] - state = JointState() - state.header.stamp = node.get_clock().now().to_msg() - state.name, state.position = (["pitch"], angles[:1]) if use_mimic else (["pitch", "ip"], angles) - messages.clear() - deadline = time.monotonic() + 8 - observed = None - while time.monotonic() < deadline: - assert process.poll() is None - publisher.publish(state) - executor.spin_once(timeout_sec=.05) - for message in messages: - for transform in message.transforms: - if transform.child_frame_id == "distal" and transform.header.stamp == state.header.stamp: - observed = transform - if observed is not None: - break - assert observed is not None, "no passive-joint TF from robot_state_publisher" - assert observed.header.frame_id == "proximal" - q = observed.transform.rotation - rotation = Rotation.from_quat([q.x, q.y, q.z, q.w]) - expected_angle = model.resolve_angles({"pitch": angles[0]})["ip"] if use_mimic else angles[1] - expected = Rotation.from_rotvec([0, 0, expected_angle]) - assert (rotation.inv() * expected).magnitude() < 1e-10 - finally: - process.terminate() - try: - process.wait(timeout=2) - except subprocess.TimeoutExpired: - process.kill() - process.wait(timeout=2) - executor.shutdown() - node.destroy_subscription(subscription) - node.destroy_node() - context.shutdown() diff --git a/src/linkerhand_calibration/test/test_interleaved_motion_efficiency.py b/src/linkerhand_calibration/test/test_interleaved_motion_efficiency.py deleted file mode 100644 index 14e94c6..0000000 --- a/src/linkerhand_calibration/test/test_interleaved_motion_efficiency.py +++ /dev/null @@ -1,89 +0,0 @@ -"""Remove duplicated travel/settling while preserving the physical sample gate.""" - -from dataclasses import replace -import math - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.visual_motion import VisualMotionObserver - - -@pytest.mark.parametrize("phase", [ - "sweep", "steady", "baseline", "prepare", "zero_approach", "clearance", - "task_exit", "return", "reference_pose", "reference_return", - "mapping_probe", "mapping_return"]) -def test_o30_scan_and_transition_trajectories_share_requested_peak_speed(phase): - profile = load_bundled_hand_profile("o30_right_18", scope="full") - start = profile.command.baseline_values - target = list(start); target[0] = 255. - segment = MotionExecution(profile, MotionCommand(phase, tuple(target), 200., command_index=0), - initial_command=start, initial_feedback=start, now=0., identity=phase, - visual_observer=VisualMotionObserver(profile)) - segment.sample(0.) - assert math.pi*255/(2*segment.duration) == pytest.approx(30.) - - -def test_same_target_hold_waits_for_fresh_stable_feedback_after_sweep_handoff(): - profile = load_bundled_hand_profile("o30_right_18", scope="full") - profile = replace(profile, acquisition=replace(profile.acquisition, motion_observation="feedback")) - start = profile.command.baseline_values - target = list(start); target[0] = 32. - move = MotionExecution(profile, MotionCommand("sweep", tuple(target), 200., - defer_settling_to_hold=True), initial_command=start, initial_feedback=start, - now=0., identity="move") - normal = MotionExecution(profile, replace(move.command, defer_settling_to_hold=False), - initial_command=start, initial_feedback=start, now=0., identity="ordinary_endpoint") - for index in range(30): - now = index*.02 - command = move.sample(now) - normal.sample(now) - move.observe(command, stamp=index, now=now) - normal.observe(command, stamp=index, now=now) - now = move.duration+.001 - for segment in (move, normal): - segment.sample(now); segment.observe(target, stamp=100, now=now) - assert move.arrived(now) - assert not normal.arrived(now) # Its endpoint hold still applies. - hold = MotionExecution(profile, MotionCommand("steady", tuple(target), 200., command_index=0), - initial_command=target, initial_feedback=target, now=now, identity="hold") - began = now - for index in range(31): - now = began + index*.02 - feedback = target.copy() - if index < 15: - feedback[0] += 4 if index % 2 else -4 - hold.sample(now);hold.observe(feedback, stamp=index, now=now) - if index < 25: - assert not hold.steady_ready(now) - assert hold.steady_ready(now) - - -def test_deferred_settling_cannot_bypass_missing_motion_or_apply_to_avoidance(): - profile = load_bundled_hand_profile("o30_right_18", scope="full") - profile = replace(profile, acquisition=replace(profile.acquisition, motion_observation="feedback")) - start = profile.command.baseline_values - target = list(start); target[0] = 32. - command = MotionCommand("sweep", tuple(target), 200., defer_settling_to_hold=True) - segment = MotionExecution(profile, command, initial_command=start, initial_feedback=start, - now=0., identity="blocked") - for index in range(200): - now = index*.02;segment.sample(now);segment.observe(start, stamp=index, now=now) - assert not segment.arrived(now) - with pytest.raises(ValueError, match="only a sweep"): - MotionExecution(profile, replace(command, phase="clearance"), initial_command=start, - initial_feedback=start, now=0., identity="invalid") - - -def test_four_round_capture_does_not_mix_with_six_round_resume(): - from linkerhand_calibration.core.domain.capture_plan import configure_training, FIXED - profile = configure_training(load_bundled_hand_profile("o30_right_18", scope="full"), FIXED) - former = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate", motion_observation="feedback"), - measurement=replace(profile.measurement, release_basis="feedback_and_command")) - optimized, separate = CalibrationEngine(profile), CalibrationEngine(former) - assert len(optimized.scan_units()) == 144 - assert len(separate.scan_units()) == 216 - assert {unit.cycle for unit in optimized.scan_units()} == {0, 1, 2, 3} - assert optimized.capture_schedule_version != separate.capture_schedule_version diff --git a/src/linkerhand_calibration/test/test_joint_pair_diagnostic.py b/src/linkerhand_calibration/test/test_joint_pair_diagnostic.py deleted file mode 100644 index de78a79..0000000 --- a/src/linkerhand_calibration/test/test_joint_pair_diagnostic.py +++ /dev/null @@ -1,168 +0,0 @@ -"""Known-truth joint image fit; diagnostic results never authorize a model.""" -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points -from linkerhand_calibration.core.geometry.tag_pose.joint_pair_diagnostic import JointPairImages, diagnose_joint_pair - - -@pytest.fixture(scope='module') -def fitted_pair(): - matrix = np.array([[1600.,0,640.],[0,1600.,480.],[0,0,1.]]) - reference = Rotation.from_euler('xy',[5,8],degrees=True) - origin,pivot = np.array([.015,-.04,.005]),np.array([.006,-.02,0.]) - objects = square_object_points(.016) - rows = [] - phases = np.r_[np.zeros(12,dtype=bool),np.ones(12,dtype=bool)] - for i,moving in enumerate(phases): - theta = np.deg2rad((i-12)*75/11) if moving else 0. - parent = Rotation.from_euler('xy',[15,10] if moving else [-25+i*50/11,20*np.sin(i/3)],degrees=True) - motion = Rotation.from_rotvec([0,0,theta]) - child = motion.apply(reference.apply(objects)+origin-pivot)+pivot - tags = {} - for role,points in [('parent',objects),('child',child)]: - camera = parent.apply(points)+[.015,-.02,.5] - projected = camera@matrix.T - tags[role] = dict(tag_size_m=.016,corners_xy=(projected[:,:2]/projected[:,2:]).tolist()) - rows.append(dict(image_stamp_ns=i+1,camera_matrix=matrix.tolist(),tags=tags)) - bundle = JointPairImages(rows,phases,'parent','child') - fit = bundle.fit((0,0,0,0)) - assert fit.success - return bundle,fit,reference,origin,pivot - - -def test_joint_fit_recovers_installation_and_axis_without_frozen_parent(fitted_pair): - bundle,fit,reference,origin,pivot = fitted_pair - assert np.max(np.abs(fit.fun)) < 1e-3 - assert (Rotation.from_rotvec(fit.x[:3]).inv()*reference).magnitude() < 1e-3 - assert np.linalg.norm(fit.x[3:6]-origin) < 1e-5 - axis,estimated = bundle.geometry(fit.x) - assert abs(axis@[0,0,1]) > .9999 - assert np.linalg.norm(estimated-pivot) < 1e-4 - assert all(np.isfinite(bundle.bounds(fit))) - - -def test_schur_covariance_includes_parent_and_angle_nuisance(fitted_pair): - bundle,fit,*_ = fitted_pair - jac = fit.jac.toarray() - scaled = jac*bundle.scale[None,:] - variance = max(.03**2,float(fit.fun@fit.fun/(len(fit.fun)-len(fit.x)))) - full = np.linalg.inv(scaled.T@scaled)[:10,:10]*variance*bundle.scale[:10,None]*bundle.scale[None,:10] - assert np.allclose(bundle.covariance(fit),full,rtol=1e-5,atol=1e-10) - # Treating parent poses as exact would incorrectly reduce uncertainty. - exact = np.linalg.inv(scaled[:,:10].T@scaled[:,:10])*variance*bundle.scale[:10,None]*bundle.scale[None,:10] - assert np.trace(full) > np.trace(exact) - - -def test_validation_keeps_shared_installation_and_hinge_frozen(fitted_pair): - bundle,fit,*_ = fitted_pair - original = fit.x.copy() - other = JointPairImages(bundle.rows,bundle.flexion,'parent','child') - residual = bundle.validate(fit,other) - assert np.max(np.abs(residual)) < 1e-3 - assert np.array_equal(original,fit.x) - - -def test_validation_angle_uncertainty_retains_shared_correlations(fitted_pair, monkeypatch): - bundle, fit, *_ = fitted_pair - rows = [{**row, 'image_stamp_ns': row['image_stamp_ns']+100} for row in bundle.rows] - other = JointPairImages(rows, bundle.flexion, 'parent', 'child') - original = fit.x.copy() - _, _, states = bundle.validate(fit, other, return_local_states=True) - covariance = bundle.validation_angle_covariance(fit, other, states) - assert np.allclose(covariance, covariance.T) - assert np.linalg.eigvalsh(covariance).min() > -1e-12 - assert np.all(np.diag(covariance)[other.flexion] > 0) - assert np.max(np.abs(covariance-np.diag(np.diag(covariance)))) > 1e-10 - monkeypatch.setattr(bundle, 'covariance', lambda fit: np.zeros((10, 10))) - conditional = bundle.validation_angle_covariance(fit, other, states) - assert np.trace(covariance) > np.trace(conditional) - assert np.array_equal(original, fit.x) - with pytest.raises(ValueError, match='independent_images'): - bundle.validation_angle_covariance(fit, bundle, states) - with pytest.raises(ValueError, match='state_invalid'): - bundle.validation_angle_covariance(fit, other, (*states[:-1], np.full(7, np.nan))) - other.roles = ('another_parent', 'another_child') - with pytest.raises(ValueError, match='contract_changed'): - bundle.validation_angle_covariance(fit, other, states) - other.roles = bundle.roles - original_rows = other.rows - other.rows = (*other.rows[:-1], other.rows[0]) - with pytest.raises(ValueError, match='independent_images'): - bundle.validation_angle_covariance(fit, other, states) - other.rows = original_rows - other.matrices[0, 0, 0] += 1 - with pytest.raises(ValueError, match='contract_changed'): - bundle.validation_angle_covariance(fit, other, states) - - -def test_vectorized_projection_matches_independent_per_image_formula(fitted_pair): - bundle,fit,*_ = fitted_pair - parameters = fit.x.copy() - parameters[:10] += np.linspace(-.001,.001,10) - scalar = np.concatenate([bundle.image_residual(parameters[:10], - parameters[offset:offset+(7 if moving else 6)],i) - for i,(moving,offset) in enumerate(zip(bundle.flexion,bundle.offsets))]) - assert np.allclose(bundle.residual(parameters),scalar,rtol=1e-9,atol=1e-9) - - -def test_joint_diagnostic_rejects_missing_or_overlapping_evidence(fitted_pair): - bundle,*_ = fitted_pair - with pytest.raises(ValueError,match='independent_images_missing'): - diagnose_joint_pair(bundle.rows[:12],bundle.rows[12:],'parent','child') - with pytest.raises(ValueError,match='source_order_invalid'): - diagnose_joint_pair(bundle.rows,bundle.rows,'parent','child') - - -def test_failed_candidates_are_retained_and_diagnostic_cannot_authorize(fitted_pair, monkeypatch): - bundle,*_ = fitted_pair - source = list(bundle.rows) - flexion = [{**row,'image_stamp_ns':row['image_stamp_ns']+24} for row in source] - def fail(self, branches): - raise ValueError('bounded_solver_failed') - monkeypatch.setattr(JointPairImages,'fit',fail) - result = diagnose_joint_pair(source,flexion,'parent','child') - assert result['publication_allowed'] is False - assert len(result['candidates']) == 16 - assert len({tuple(c['branches']) for c in result['candidates']}) == 16 - assert all(c['reason']=='bounded_solver_failed' for c in result['candidates']) - assert set(result['training_stamps']).isdisjoint(result['validation_stamps']) - assert not {'model','resolved','selected_hypothesis'} & result.keys() - - -def test_comparison_never_selects_winner_from_holdout(): - from linkerhand_calibration.core.geometry.tag_pose.joint_pair_diagnostic import _diagnostic_comparisons - phases = [False]*24+[True]*24 - motion = [[v,0.,0.,v] for v in np.tile(np.linspace(0.,1.5,24),2)] - common = dict(converged=True,validation_motion_coordinates=motion, - installation_quaternion_xyzw=[0.,0.,0.,1.],axis_parent_xyz=[0.,0.,1.], - pivot_parent_xyz_m=[0.,0.,0.]) - candidates = [dict(common,training_cost=cost,installation_translation_xyz_m=[index*.01,0.,0.], - validation_errors_by_role_px=[[error,error]]*48) - for index,(cost,error) in enumerate(((1.,1.),(2.,.01)))] - report = _diagnostic_comparisons(candidates,phases) - assert report['training_candidate'] == 0 - assert all(test['adjusted_p_value'] == 1. for test in report['phase_tests']) - - -def test_uncertainty_failure_does_not_erase_competing_image_fit(fitted_pair, monkeypatch): - bundle, fit, *_ = fitted_pair - source = list(bundle.rows) - flexion = [{**r, 'image_stamp_ns': r['image_stamp_ns']+24} for r in source] - def fitted(self, branches): - self.axis, self.basis = bundle.axis, bundle.basis - return fit - monkeypatch.setattr(JointPairImages, 'fit', fitted) - monkeypatch.setattr(JointPairImages, 'bounds', lambda *args: (.01, .0001)) - monkeypatch.setattr(JointPairImages, 'validate', lambda self, fit, other, **kwargs: - (np.zeros((other.count, 16)), np.zeros((other.count, 4)), ())) - def unavailable(*args): - raise ValueError('joint_pair_geometry_rank_deficient') - monkeypatch.setattr(JointPairImages, 'validation_angle_covariance', unavailable) - report = diagnose_joint_pair(source, flexion, 'parent', 'child') - assert len(report['candidates']) == 16 - assert all(c['converged'] and c['angle_uncertainty_available'] is False - and c['angle_uncertainty_reason'] == 'joint_pair_geometry_rank_deficient' - for c in report['candidates']) - assert report['publication_allowed'] is False diff --git a/src/linkerhand_calibration/test/test_joint_zero_runtime.py b/src/linkerhand_calibration/test/test_joint_zero_runtime.py deleted file mode 100644 index 9d19765..0000000 --- a/src/linkerhand_calibration/test/test_joint_zero_runtime.py +++ /dev/null @@ -1,291 +0,0 @@ -"""Per-joint references use the production executor without ROS or hardware.""" - -from dataclasses import replace -import numpy as np -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.core.domain.reference import build_joint_zero_reference, read_joint_zero_references -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.scan_quality import observation_streams -from linkerhand_calibration.runtime.joint_resume import JointResume - - -@pytest.mark.parametrize("invalid", ["disabled_approach", "missing_approach", "wrong_baseline"]) -def test_zero_pose_contract_rejects_invalid_motion_before_hardware(invalid): - from linkerhand_calibration.core.domain.profile import validate_profile - profile = load_bundled_hand_profile("g20_right_19") - name, spec = next(iter(profile.motion.joint_zero_references.items())) - if invalid == "disabled_approach": - pose = list(spec.approach_commands[0]) - channel = min(profile.command.disabled_indices) - pose[channel] = 0 if pose[channel] else 255 - changed = replace(spec, approach_commands=(tuple(pose),)) - reason = "moves a disabled channel" - elif invalid == "missing_approach": - changed = replace(spec, approach_commands=()) - reason = "explicit nonzero final arrival" - else: - channel = next(t.command_index for t in profile.motion.tasks if t.key == spec.task_key) - pose = list(spec.command) - pose[channel] = 0 if pose[channel] else 255 - changed = replace(spec, command=tuple(pose)) - reason = "joint baseline, not task start" - profile = replace(profile, motion=replace(profile.motion, - joint_zero_references={**profile.motion.joint_zero_references, name: changed})) - with pytest.raises(ValueError, match=reason): - validate_profile(profile) - - -def reference_rows(profile, name, *, stamp=1, tilt=0): - from scipy.spatial.transform import Rotation - spec = profile.motion.joint_zero_references[name] - channel = next(t.command_index for t in profile.motion.tasks if t.key == spec.task_key) - direction = "increasing" if spec.command[channel] > spec.approach_commands[-1][channel] else "decreasing" - views = {profile.measurement.measurements[name].view} - secondary = profile.measurement.cross_view_sources.get(name) - if secondary: - views.add(profile.measurement.measurements[secondary].view) - pose = {"quaternion_xyzw": [0, 0, 0, 1], - "translation_xyz_m": [0, 0, .5]} - child = {"quaternion_xyzw": Rotation.from_rotvec([tilt, 0, 0]).as_quat().tolist(), "translation_xyz_m": [0, 0, .6]} - rows = [{"view": view, "image_stamp_ns": stamp+i, - "relative_quaternion_xyzw": child["quaternion_xyzw"], "relative_translation_xyz_m": [0, 0, .1], - f"command_vector_{profile.command.unit}": spec.command, f"state_{profile.command.unit}": spec.command, - "command_direction_by_index": [direction]*profile.command.command_count, - "parent_pose_common": pose, "child_pose_common": child} - for view in views for i in range(profile.acquisition.fixed_reference_minimum_frames)] - for row in rows: - primary = profile.measurement.measurements[name] - measurement = primary if primary.view == row["view"] else profile.measurement.measurements[secondary] - row["pnp_observation_evidence"] = { - side: {"pose_source": "current_image", "tag_role": role, - "candidate_diagnostics": {"observation_stamp_ns": row["image_stamp_ns"], - "branch_revision": 0, "branch_status": "tracking"}} - for side, role in (("parent", measurement.parent_role), ("child", measurement.child_role))} - return rows - - -def sweep_feedback_values(start, target, unit): - """Sample only this motion segment, at the same density over a full range.""" - fraction = abs(target-start)/abs(unit.end-unit.start) - return np.linspace(start, target, max(2, int(np.ceil(128*fraction))+1)) - - -@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) -def test_deferred_zero_is_locked_once_before_own_training_in_every_model(layout): - profile = load_bundled_hand_profile(layout) - driver = SessionExecution(profile) - driver.session.device_ready() - driver.session.start() - command, rows, references, stamp = profile.command.baseline_values, [], {}, 100 - trace = [] - for _ in range(6000): - phase = driver.session.phase - if phase == Phase.FIT: - break - if phase == Phase.REFERENCE_LOCKING: - required = driver.session.status().task.required_tag_ids_by_view - assert sum(len(ids) for ids in required.values()) == len(profile.vision.views) - driver.session.reference_locked() - assert driver.session.phase == Phase.PREPARE # No all-finger installation sweep. - continue - if phase == Phase.EVALUATE: - assert driver.evaluate(rows).passed - continue - segment_start = command - motion = driver.motion(command) - command = motion.target - trace.append((motion.phase, motion.task_key, motion.cycle)) - if motion.zero_joints: - for name in motion.zero_joints: - assert name not in references - spec = profile.motion.joint_zero_references[name] - assert command == spec.command - assert command[motion.command_index] == profile.command.baseline_values[motion.command_index] - 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 - elif motion.recording: - task = next(t for t in profile.motion.tasks if t.key == motion.task_key) - assert set(task.joints) <= references.keys() - unit = driver.action.scan_unit - feedback_values = (sweep_feedback_values(segment_start[task.command_index], - command[task.command_index], unit) if motion.phase == "sweep" else - (command[task.command_index],)*profile.acquisition.steady_minimum_samples) - for field, name, spec in observation_streams(profile, task): - for feedback in feedback_values: - stamp += 1 - rows.append({field: name, "view": spec.view, "task_name": task.key, - "cycle": unit.cycle, "direction": unit.direction, "attempt": 1, - "image_stamp_ns": stamp, "sample_phase": motion.phase, - "steady_index": motion.steady_index, "steady_target": command[task.command_index], - f"feedback_{profile.command.unit}": float(feedback)}) - driver.motion_complete() - assert driver.session.phase == Phase.FIT - assert references == driver.zero_references - assert set(references) == set(profile.motion.joint_zero_references) - if layout == "g20_right_19": - index_zero = trace.index(("joint_zero", "index_roll_multiview", 0)) - assert ("sweep", "middle_pip_side", 3) in trace[:index_zero] - reference = references["index_mcp_roll"] - assert reference.baseline_command == 127 # Task starts at 255. - assert any(np.any(np.asarray(s.command) != profile.command.baseline_values) for s in reference.samples) - - -def test_resume_verifies_same_pose_and_preserves_original_zero(): - profile = load_bundled_hand_profile("o6_right_8") - name = next(iter(profile.motion.joint_zero_references)) - old = build_joint_zero_reference(profile, name, reference_rows(profile, name), session_epoch=1, motion_version=2) - current = build_joint_zero_reference(profile, name, reference_rows(profile, name, stamp=100), session_epoch=2, motion_version=4) - resume = JointResume(profile) - resume.references[name] = old - assert resume.verify(current) is old - slipped = build_joint_zero_reference(profile, name, reference_rows(profile, name, stamp=100, tilt=.1), - session_epoch=2, motion_version=5) - with pytest.raises(ValueError, match="joint_resume_reference_changed"): - resume.verify(slipped) - with pytest.raises(ValueError, match="duplicate_or_unknown"): - read_joint_zero_references(profile, [old.as_record(), old.as_record()]) - with pytest.raises(ValueError, match="not_before_training"): - read_joint_zero_references(profile, [old.as_record(), {"kind": "joint_sample", "joint": name, "image_stamp_ns": 1}]) - tampered = old.as_record() - tampered["samples"][0]["sample_id"] = "different_image:1" - with pytest.raises(ValueError, match="joint_zero_image_identity_invalid"): - read_joint_zero_references(profile, [tampered]) - - -def test_incomplete_zero_window_does_not_revalidate_poses_on_every_control_tick(monkeypatch): - from linkerhand_calibration.core.domain.reference import JointZeroSample - profile = load_bundled_hand_profile("o6_right_8") - name = next(iter(profile.motion.joint_zero_references)) - rows = reference_rows(profile, name) - calls = [] - original = JointZeroSample.from_row - - def counted(row, unit): - calls.append(row["image_stamp_ns"]) - return original(row, unit) - - monkeypatch.setattr(JointZeroSample, "from_row", staticmethod(counted)) - for _ in range(20): - with pytest.raises(ValueError, match="joint_zero_samples_missing"): - build_joint_zero_reference(profile, name, rows[:-1], session_epoch=1, motion_version=1) - assert not calls - build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=1) - assert len(calls) == len(rows) - rows[0]["relative_translation_xyz_m"] = [9, 9, 9] - with pytest.raises(ValueError, match="joint_zero_relative_pose_inconsistent"): - build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=1) - - -def test_same_joint_in_later_task_cannot_acquire_a_second_reference(): - profile = load_bundled_hand_profile("o6_right_8") - first = profile.motion.tasks[0] - repeated = replace(first, key="repeat_same_joint") - profile = replace(profile, motion=replace(profile.motion, tasks=profile.motion.tasks+(repeated,))) - driver = SessionExecution(profile) - for name in first.joints: - driver.zero_references[name] = build_joint_zero_reference(profile, name, reference_rows(profile, name), - session_epoch=1, motion_version=1) - before = dict(driver.zero_references) - driver.session._unit_index = next(i for i, u in enumerate(driver.session.engine.scan_units()) if u.task_key == repeated.key) - driver.session.phase = Phase.PREPARE - assert driver.motion(profile.command.baseline_values).phase != "joint_zero" - assert driver.zero_references == before - - -def test_native_resume_imports_training_travel_only_after_reference_verification(tmp_path): - from copy import deepcopy - from pose_branch_fixture import add_ideal_motion_provenance - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - from runtime_host_fixture import coordinator_fixture, ready - from linkerhand_calibration.runtime.steady import steady_targets - host, clock = coordinator_fixture(tmp_path, model="o12") - try: - profile = host.profile - task = profile.motion.tasks[0] - units = [u for u in host.execution.session.engine.scan_units() if u.task_key == task.key and u.cycle == 0] - records, stamp = [{"kind": "session_start", "profile_id": profile.key.profile_id, - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "capture_schedule_version": host.execution.session.engine.capture_schedule_version, - "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION}], 100 - templates = {} - for name in task.joints: - zeros = reference_rows(profile, name, stamp=10) - records.extend(add_ideal_motion_provenance(profile, name, zeros, - session_epoch=1, motion_version=1, source_stamps=(1, 2))) - templates[name] = {row["view"]: row for row in zeros} - reference = build_joint_zero_reference(profile, name, zeros, session_epoch=1, motion_version=2) - records.append(reference.as_record()) - for unit in units: - previous = unit.start - for index, target in enumerate(steady_targets(profile, unit)): - phases = [] - if index: - phases.append(("sweep", sweep_feedback_values(previous, target, unit))) - phases.append(("steady", (target,)*profile.acquisition.steady_minimum_samples)) - for phase, feedback_values in phases: - for field, name, spec in observation_streams(profile, task): - for feedback in feedback_values: - stamp += 1 - evidence = deepcopy(templates[name][spec.view]["pnp_observation_evidence"]) - for item in evidence.values(): - item["candidate_diagnostics"].update(observation_stamp_ns=stamp, branch_frozen=True) - records.append({"kind": "joint_sample" if field == "joint" else "secondary_joint_sample", - field: name, "view": spec.view, "task_name": task.key, "cycle": 0, - "direction": unit.direction, "attempt": 1, "image_stamp_ns": stamp, - "sample_phase": phase, "steady_index": index, "steady_target": target, - "feedback_rad": float(feedback), "pnp_observation_evidence": evidence}) - previous = target - keys = {(u.task_key, u.cycle, u.direction) for u in units} - host.joint_resume.stage(host.execution.session.engine, records, keys) - assert host.joint_resume.take_task(task.key) == ((), ()) - for name in task.joints: - zeros = reference_rows(profile, name, stamp=2000) - add_ideal_motion_provenance(profile, name, zeros, - session_epoch=2, motion_version=2, source_stamps=(1900, 1901)) - current = build_joint_zero_reference(profile, name, zeros, - session_epoch=2, motion_version=3) - host.execution.zero_references[name] = host.joint_resume.verify(current) - ready(host, clock) - assert host.start().success - clock.now += .1 - host.receive_feedback(profile.command.names, profile.command.baseline_values, host.ports.clock_ns()) - host.last_command = profile.command.baseline_values - host.execution.session.phase = Phase.PREPARE - host.tick() - while host._resume_import is not None: - clock.now += .02 - host.receive_feedback(profile.command.names, profile.command.baseline_values, host.ports.clock_ns()) - host.tick() - assert host._resumed_count == 2 and host.execution.session.current_unit.cycle == 1 - assert host.execution.first_cycle_spans - assert all(value == pytest.approx(1) for value in host.execution.first_cycle_spans.values()) - finally: - host.close() - - -def test_zero_capture_recovers_after_transient_bad_pose_without_loosening_stability(tmp_path): - from types import SimpleNamespace - from runtime_host_fixture import coordinator_fixture - host, _clock = coordinator_fixture(tmp_path, model="o6") - try: - name = next(iter(host.profile.motion.joint_zero_references)) - bad = reference_rows(host.profile, name, tilt=.1)[:1] - stable = reference_rows(host.profile, name, stamp=100) - from pose_branch_fixture import confirm_reference_rows - confirm_reference_rows(host, stable) - host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in bad+stable] - host._observation_epoch = host._segment_number = 1 - assert host._lock_joint_zeros(SimpleNamespace(task_key=host.profile.motion.joint_zero_references[name].task_key, - zero_joints=(name,))) - reference = host.execution.zero_references[name] - assert all(sample.image_stamp_ns >= 100 for sample in reference.samples) - assert len(host._steady_rows) == len(bad)+len(stable) # Evidence was not erased. - assert host.execution.session.phase != Phase.PAUSED - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_joint_zero_timing.py b/src/linkerhand_calibration/test/test_joint_zero_timing.py deleted file mode 100644 index 6f0630a..0000000 --- a/src/linkerhand_calibration/test/test_joint_zero_timing.py +++ /dev/null @@ -1,102 +0,0 @@ -"""Irregular control timing must not erase stable joint-zero observations.""" - -import numpy as np -import pytest - -from linkerhand_calibration.runtime.inputs import DetectionInput -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from runtime_host_fixture import coordinator_fixture, ready -from test_joint_zero_runtime import reference_rows -from pose_branch_fixture import assume_motion_confirmation, confirm_reference_rows - - -@pytest.mark.parametrize("feedback_moves,fixed_occluded,recovery", [ - (False, False, False), (True, False, False), (False, True, False), (False, False, True), - (False, False, "slow"), (False, False, "nine_only")], - ids=["timer_jitter", "real_movement", "persistent_fixed_occlusion", "post_geometry_recovery", - "ten_slow_images", "nine_images_never_complete"]) -def test_zero_capture_requires_stable_feedback_and_valid_reference_images( - tmp_path, monkeypatch, feedback_moves, fixed_occluded, recovery): - host, clock = coordinator_fixture(tmp_path, model="o6") - try: - ready(host, clock) - assert host.start().success - task = host.profile.motion.tasks[0] - name = task.joints[0] - spec = host.profile.motion.joint_zero_references[name] - host.execution.session.phase = Phase.PREPARE - host.last_command = spec.command - # The declared approach has completed; exercise the real zero action, - # coordinator timing gates and reference builder from this point. - host.execution.motion(spec.command) - effects = host.execution._effects - index = next(i for i, effect in enumerate(effects) if effect.phase == "joint_zero") - host.execution._effects = effects[index:] - host.commanded_speed = host.execution._effects[0].speed - host._command_directions = ["increasing"]*host.command_count - assume_motion_confirmation(host, task.joints) - - def consume(frame, motion, **kwargs): - if motion is None: - return (), None - assert frame.feedback is not None and frame.command == spec.command - if fixed_occluded: - return ({"kind": "pnp_branch_event", "view": "top", "tag_role": "top_base", "tag_id": 6, - "image_stamp_ns": frame.stamp_ns, "reason": "fixed_reference_pose_unverified", - "pnp_observation_evidence": {"candidate_diagnostics": { - "observation_stamp_ns": frame.stamp_ns, "branch_status": "fixed_reference_unavailable", - "pose_constraint": "fixed_reference", "fixed_reference_verified": False}}},), None - row = reference_rows(host.profile, name, stamp=frame.stamp_ns)[0] - confirm_reference_rows(host, [row]) - return (dict(row, kind="joint_zero_sample", joint=name),), None - - monkeypatch.setattr(host.capture, "consume", consume) - start = clock.now - frames = (.59, .68, .77, .79, .81, .83, .85, .87, .89, .91) - if recovery: - # Real recorded run: tracker recovery consumes the first part of - # the post-solve window, followed by valid images at about 7 Hz. - frames = tuple(1.1+.14*i for i in range(10)) - if recovery in {"slow", "nine_only"}: - frames = tuple(0.8+.35*i for i in range(9 if recovery == "nine_only" else 10)) - times = sorted({*np.arange(0, .51, .02), *frames, frames[-1]+.02, .93, 2.02, 3.5, 7.1}) - if recovery in {"slow", "nine_only"}: - # A failed zero image window now gets one bounded fresh window. - # Keep feedback alive through that window without adding images. - times = sorted({*times, *np.arange(0, 9.11, .02)}) - for elapsed in times: - clock.now = start+float(elapsed) - feedback = list(spec.command) - if feedback_moves and elapsed == .77: - feedback[task.command_index] -= 5 - host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) - host.tick() - if recovery and elapsed == 0: - host._zero_geometry_completed_at = clock.now - if name in host.execution.zero_references: - break - if elapsed in frames: - host.receive_detections(DetectionInput(task.view, host.ports.clock_ns(), ())) - - if feedback_moves or fixed_occluded or recovery == "nine_only": - assert not host.execution.zero_references - assert host.execution.session.phase == Phase.PAUSED - assert "joint_zero_unavailable" in host.reason - assert len([r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"]) == 1 - if fixed_occluded: - assert "fixed_reference_pose_unverified" in host.reason - assert "top:tag=6" in host.reason - assert not host.finalization.started - else: - assert host.execution.session.phase != Phase.PAUSED, host.reason - reference = host.execution.zero_references[name] - assert len(reference.samples) == host.profile.acquisition.fixed_reference_minimum_frames == 10 - assert len([r for r in host.raw_records if r["kind"] == "joint_zero_reference"]) == 1 - if recovery == "slow": - assert host.profile.acquisition.steady_timeout_seconds < clock.now-start < 5.0 - elif recovery: - assert host.profile.acquisition.steady_timeout_seconds < clock.now-start < 3.0 - else: - assert clock.now-start < host.profile.acquisition.steady_timeout_seconds - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_json_urdf_rebuild.py b/src/linkerhand_calibration/test/test_json_urdf_rebuild.py deleted file mode 100644 index 1eb1f08..0000000 --- a/src/linkerhand_calibration/test/test_json_urdf_rebuild.py +++ /dev/null @@ -1,92 +0,0 @@ -"""JSON alone reproduces the written URDF and rejects unsafe edits atomically.""" - -import hashlib -import json - -import numpy as np -import pytest - -from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( - rebuild_urdf_from_json, serialize_urdf_correction, - validate_json_urdf_correspondence, -) - - -@pytest.fixture -def saved_calibration(tmp_path): - source = tmp_path/"source.urdf" - source.write_text(''' - - - - - - - -''') - rights = {name: ("origin.rpy", "limit.lower", "limit.upper", "mimic.multiplier", "mimic.offset") - for name in ("drive", "tip")} - plan = build_standard_correction_plan(source_urdf=source, - source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), - zero_offsets_rad={"drive": .1, "tip": .05}, - measured_ranges_output_rad={"drive": (0., 1.5), "tip": (0., .75)}, - authorized_fields=rights) - payload = {"format": "unified_calibration_v3", "schema_version": 3, - "profile_id": "VIRTUAL/right/rebuild/v1", "input_unit": "u8", "baseline_command": [0], - "joints": {name: {"sdk_channel": 0, "angle_rad": np.linspace(0, upper, 256).tolist()} - for name, upper in (("drive", 1.5), ("tip", .75))}, - "urdf_correction": serialize_urdf_correction(plan)} - path = tmp_path/"calibration.json" - path.write_text(json.dumps(payload)) - return source, rights, plan, path, payload - - -def rebuild(saved, output): - source, rights, _, path, payload = saved - return rebuild_urdf_from_json(path, source, output, - authorized_fields=rights, expected_profile_id=payload["profile_id"]) - - -def test_saved_json_reproduces_bytes_and_is_the_only_edit_source(saved_calibration, tmp_path): - source, _, plan, path, payload = saved_calibration - digest = hashlib.sha256(source.read_bytes()).hexdigest() - expected = plan.write(source, tmp_path/"expected.urdf") - output = rebuild(saved_calibration, tmp_path/"from_json.urdf") - assert output.read_bytes() == expected.read_bytes() - validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) - # A later caller has no in-memory fit. The requested edit is read anew. - payload["urdf_correction"]["joint_patches"]["drive"]["origin_rpy"] = "0 0 0.2" - path.write_text(json.dumps(payload)) - with pytest.raises(ValueError, match="correction_differs_from_urdf"): - validate_json_urdf_correspondence(payload, source, output, expected_profile_id=payload["profile_id"]) - changed = rebuild(saved_calibration, tmp_path/"changed.urdf") - assert not np.allclose(UrdfKinematicModel(changed).joints["drive"].origin, - UrdfKinematicModel(expected).joints["drive"].origin) - assert hashlib.sha256(source.read_bytes()).hexdigest() == digest - with pytest.raises(ValueError, match="overwrite"): - rebuild(saved_calibration, output) - - -@pytest.mark.parametrize("corruption", ["source", "profile", "unknown_field", "nonfinite", "curve_limit", "missing"]) -def test_invalid_json_never_installs_a_destination(saved_calibration, tmp_path, corruption): - source, rights, _, path, payload = saved_calibration - if corruption == "source": - source.write_text(source.read_text()+"\n") - elif corruption == "profile": - payload["profile_id"] = "WRONG" - elif corruption == "unknown_field": - payload["urdf_correction"]["joint_patches"]["drive"]["axis_xyz"] = "1 0 0" - elif corruption == "nonfinite": - payload["urdf_correction"]["joint_patches"]["drive"]["origin_rpy"] = "0 nan 0" - elif corruption == "curve_limit": - payload["urdf_correction"]["joint_patches"]["drive"]["limit_upper"] = "1.4" - else: - del payload["urdf_correction"] - 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="VIRTUAL/right/rebuild/v1") - assert not output.exists() diff --git a/src/linkerhand_calibration/test/test_kinematic_chain_diagnostic.py b/src/linkerhand_calibration/test/test_kinematic_chain_diagnostic.py deleted file mode 100644 index 37b21f5..0000000 --- a/src/linkerhand_calibration/test/test_kinematic_chain_diagnostic.py +++ /dev/null @@ -1,90 +0,0 @@ -"""Stage references are rotations of one chain, never free rigid poses.""" -from copy import deepcopy - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from test_finger_chain_diagnostic import make_chain -from linkerhand_calibration.core.geometry.tag_pose.kinematic_chain_diagnostic import KinematicFingerChainImages - - -@pytest.fixture(scope='module') -def linked_fit(): - original, _, axes, _ = make_chain(parallel=True, full_parallel=True) - def bundle(rows): - return KinematicFingerChainImages(rows, original.phases, original.roles, - pip_dip_distance_m=original.distances[2], mcp_pip_distance_m=original.distances[1]) - canonical = bundle(original.rows) - seed = canonical.fit((0,)*6) - assert seed.success and np.max(np.abs(seed.fun)) < 1e-4 - shared = seed.x[:canonical.static_width].copy() - shared[canonical.stage_columns[0]:] = [.012, -.018, -.009, .015] - angles = np.tile(np.linspace(0., 1.3, 12), 3) - pixels = canonical.project(shared, angles, canonical.phases) - rows = deepcopy(original.rows) - for row, points in zip(rows, pixels): - for role, corners in zip(original.roles, points.reshape(2, 4, 2)): - row['tags'][role]['corners_xy'] = corners.tolist() - training = bundle(rows) - fit = training.fit((0,)*6) - assert fit.success - return training, fit, axes - - -def test_linked_stage_changes_recover_shared_geometry(linked_fit): - training, fit, axes = linked_fit - assert training.static_width == 22 # 18 shared + four physical visit angles. - assert len(training.angle_columns) == training.count-2 - assert np.max(np.abs(fit.fun)) < 1e-4 - geometry = training.physical_geometry(fit.x[:training.static_width]) - assert np.min(np.abs(np.sum(geometry[:, :3]*axes, axis=1))) > .99999 - - -def test_linked_covariance_includes_held_stage_angles(linked_fit): - training, fit, _ = linked_fit - jac = fit.jac*training.scale[None, :] - width = training.static_width - expected = np.linalg.inv(jac.T@jac)[:width, :width]*.03**2 - expected *= training.scale[:width, None]*training.scale[None, :width] - assert np.allclose(training.covariance(fit), expected, atol=1e-9, rtol=1e-3) - - -def test_frozen_linked_validation_rejects_unexplained_stage_translation(linked_fit): - training, fit, _ = linked_fit - rows = deepcopy(training.rows) - for row, phase in zip(rows, training.phases): - row['image_stamp_ns'] += 10000 - if phase == 1: - for tag in row['tags'].values(): - tag['corners_xy'] = (np.asarray(tag['corners_xy'])+[0., 5.]).tolist() - validation = KinematicFingerChainImages(rows, training.phases, training.roles, - pip_dip_distance_m=training.distances[2], mcp_pip_distance_m=training.distances[1]) - before = fit.x.copy() - residual, angles = training.validate(fit, validation) - assert np.array_equal(before, fit.x) - assert np.sqrt(np.mean(residual[training.phases == 1]**2)) > 1.5 - assert np.all(np.isfinite(training.validation_angle_covariance(fit, validation, angles))) - - -def test_camera_geometry_translation_and_correlated_covariance(linked_fit): - training, fit, _ = linked_fit - shared = fit.x[:training.static_width].copy() - before = training.camera_geometry(shared) - displacement = np.array([.01, -.02, .03]) - shared[3:6] += displacement - after = training.camera_geometry(shared) - assert np.allclose(after[:, :3], before[:, :3]) - assert np.allclose(after[:, 3:]-before[:, 3:], displacement) - # A common translation must remain correlated across all three joints. - covariance = np.zeros((training.static_width, training.static_width)) - covariance[3:6, 3:6] = np.eye(3)*1e-6 - propagated = training.camera_geometry_covariance(shared, covariance) - for first in range(3): - for second in range(3): - assert np.allclose(propagated[6*first+3:6*first+6, - 6*second+3:6*second+6], np.eye(3)*1e-6) - assert np.allclose(propagated[:3], 0.) - signature = training.training_signature(shared) - assert np.allclose(signature['stage_poses'][2][:3], - np.column_stack((Rotation.from_rotvec(shared[:3]).as_matrix(), shared[3:6]))) diff --git a/src/linkerhand_calibration/test/test_l6_right_profile.py b/src/linkerhand_calibration/test/test_l6_right_profile.py deleted file mode 100644 index 9d05339..0000000 --- a/src/linkerhand_calibration/test/test_l6_right_profile.py +++ /dev/null @@ -1,345 +0,0 @@ -from __future__ import annotations - -from pathlib import Path -import re - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core import validate_profile -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.motion import cosine_position_trajectory_u8 -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.profile import ( - CALIBRATED_ACTIVE_JOINTS, - COMMAND_NAMES, - ENDPOINT_ANCHOR_BY_JOINT, - KEY, - build_typed_profile, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.runner import render_l6_progress_zh -from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import UrdfKinematicModel -from linkerhand_calibration.product import load_product_config, sha256_file - - -PACKAGE = Path(__file__).resolve().parents[1] -SOURCE = PACKAGE / "urdf/l6_right/linkerhand_l6v3.1_right.urdf" -LEFT_SOURCE = PACKAGE / "urdf/l6_left/linkerhand_l6v3.1_left.urdf" -PRODUCT = PACKAGE / "config/l6_right_product.yaml" - -TRAVELS = { - "rh_thumb_cmc_roll": 1.34, - "rh_thumb_cmc_pitch": 0.78, - "rh_pinky_mcp_pitch": 1.10, -} -MULTIPLIERS = {"rh_thumb_dip": 1.20, "rh_pinky_dip": 1.10} -THUMB_ZERO_OFFSETS = { - "rh_thumb_cmc_roll": 0.04, - "rh_thumb_cmc_pitch": -0.05, -} -KINEMATIC_MODEL = UrdfKinematicModel(SOURCE) - - -def _pose(xyz: list[float], rpy: list[float]) -> np.ndarray: - return transform_matrix(xyz, Rotation.from_euler("xyz", rpy).as_quat()) - - -def _geometric_observation( - joint: str, - feedback: int, - *, - thumb_zero_offsets: dict[str, float] | None = None, -) -> dict[str, object]: - model = KINEMATIC_MODEL - state = [255.0] * 6 - motor = { - "rh_thumb_cmc_pitch": 0, - "rh_thumb_dip": 0, - "rh_thumb_cmc_roll": 1, - "rh_pinky_mcp_pitch": 5, - "rh_pinky_dip": 5, - }[joint] - state[motor] = float(feedback) - active_angles = { - name: travel * (255.0 - state[index]) / 255.0 - for name, travel, index in ( - ("rh_thumb_cmc_pitch", TRAVELS["rh_thumb_cmc_pitch"], 0), - ("rh_thumb_cmc_roll", TRAVELS["rh_thumb_cmc_roll"], 1), - ("rh_pinky_mcp_pitch", TRAVELS["rh_pinky_mcp_pitch"], 5), - ) - } - joint_angles = { - **active_angles, - "rh_thumb_dip": active_angles["rh_thumb_cmc_pitch"] - * MULTIPLIERS["rh_thumb_dip"], - "rh_pinky_dip": active_angles["rh_pinky_mcp_pitch"] - * MULTIPLIERS["rh_pinky_dip"], - } - zero_offsets = { - **( - THUMB_ZERO_OFFSETS - if thumb_zero_offsets is None - else thumb_zero_offsets - ), - "rh_pinky_mcp_pitch": 0.0, - } - base_common = _pose([0.11, -0.04, 0.72], [0.11, -0.08, 0.17]) - base_mounts = { - "front": _pose([0.0, 0.008, 0.055], [0.2, -0.1, 0.3]), - "side": _pose([-0.01, 0.002, 0.045], [-0.1, 0.2, -0.2]), - "top": _pose([0.012, -0.006, 0.05], [0.1, 0.3, 0.15]), - } - link_mounts = { - "rh_thumb_cmc_roll": _pose([0.012, 0.019, 0.006], [0.2, 0.1, -0.1]), - "rh_thumb_cmc_pitch": _pose([0.009, -0.028, 0.011], [-0.2, 0.1, 0.25]), - "rh_thumb_dip": _pose([0.006, -0.025, 0.018], [0.15, -0.1, 0.2]), - "rh_pinky_mcp_pitch": _pose([0.006, 0.004, 0.021], [-0.1, 0.2, 0.1]), - "rh_pinky_dip": _pose([0.004, 0.003, 0.019], [0.12, 0.08, -0.2]), - } - view = { - "rh_thumb_cmc_roll": "top", - "rh_thumb_cmc_pitch": "front", - "rh_thumb_dip": "front", - "rh_pinky_mcp_pitch": "side", - "rh_pinky_dip": "side", - }[joint] - if joint in {"rh_thumb_dip", "rh_pinky_dip"}: - parent_joint = { - "rh_thumb_dip": "rh_thumb_cmc_pitch", - "rh_pinky_dip": "rh_pinky_mcp_pitch", - }[joint] - parent = base_common @ model.link_transform( - parent_joint, - zero_offsets=zero_offsets, - joint_angles=joint_angles, - independent_mimic_angles=True, - ) @ link_mounts[parent_joint] - else: - parent = base_common @ base_mounts[view] - child = base_common @ model.link_transform( - joint, - zero_offsets=zero_offsets, - joint_angles=joint_angles, - independent_mimic_angles=True, - ) @ link_mounts[joint] - relative = np.linalg.inv(parent) @ child - return { - "state_u8": state, - "relative_translation_xyz_m": relative[:3, 3].tolist(), - "relative_quaternion_xyzw": Rotation.from_matrix( - relative[:3, :3] - ).as_quat().tolist(), - "parent_pose_common": matrix_payload(parent), - "child_pose_common": matrix_payload(child), - "view_normal_common_xyz": [0.577350269, 0.577350269, 0.577350269], - "camera_center_common_xyz_m": [0.0, 0.0, -0.5], - } - - -def _synthetic_records( - *, thumb_zero_offsets: dict[str, float] | None = None -) -> list[dict]: - result = [] - task_by_joint = { - "rh_thumb_cmc_roll": "thumb_roll_top", - "rh_thumb_cmc_pitch": "thumb_pitch_dip_front", - "rh_thumb_dip": "thumb_pitch_dip_front", - "rh_pinky_mcp_pitch": "pinky_pitch_dip_side", - "rh_pinky_dip": "pinky_pitch_dip_side", - } - source_by_passive = { - "rh_thumb_dip": "rh_thumb_cmc_pitch", - "rh_pinky_dip": "rh_pinky_mcp_pitch", - } - for name, task in task_by_joint.items(): - travel = ( - TRAVELS[name] - if name in TRAVELS - else TRAVELS[source_by_passive[name]] * MULTIPLIERS[name] - ) - for cycle in range(4): - for direction, commands in ( - ("decreasing", range(255, -1, -4)), - ("increasing", range(0, 256, 4)), - ): - values = list(commands) - if values[-1] not in {0, 255}: - values.append(0 if direction == "decreasing" else 255) - for feedback in values: - angle = travel * (255 - feedback) / 255.0 - geometry = _geometric_observation( - name, - feedback, - thumb_zero_offsets=thumb_zero_offsets, - ) - result.append( - { - "kind": "l6_joint_sample", - "task_name": task, - "joint": name, - "cycle": cycle, - "direction": direction, - "attempt": 1, - "feedback_u8": float(feedback), - **geometry, - } - ) - return result - - -def _synthetic_nonlinear_pinky_records() -> list[dict]: - records = _synthetic_records() - mounted = Rotation.from_euler("xyz", [0.2, -0.1, 0.3]) - for row in records: - if row["joint"] != "rh_pinky_dip": - continue - active = TRAVELS["rh_pinky_mcp_pitch"] * ( - 255.0 - float(row["feedback_u8"]) - ) / 255.0 - angle = 1.2 * active - 0.2 * active * active - observed = mounted * Rotation.from_rotvec([angle, 0.0, 0.0]) - row["relative_quaternion_xyzw"] = observed.as_quat().tolist() - return records - - -def _joint_block(text: str, name: str) -> str: - pattern = re.compile( - rf"]*\btype\s*=)[^>]*\bname=\"{re.escape(name)}\"" - rf"[^>]*>.*?", - re.DOTALL, - ) - match = pattern.search(text) - assert match is not None - return match.group(0) - - -def test_l6_profile_declares_six_channels_eight_tags_and_partial_scope() -> None: - profile = build_typed_profile() - validate_profile(profile) - assert profile.key == KEY - assert profile.namespace == "/l6_calibration" - assert profile.command.names == COMMAND_NAMES - assert profile.command.baseline_u8 == (255,) * 6 - assert profile.command.feedback_name_aliases == { - "thumb_cmc_yaw": "thumb_cmc_roll" - } - assert profile.motion.speed_parameters["preflight_u8"] == 1 - assert profile.motion.speed_parameters["formal_u8"] == 1 - assert profile.motion.speed_parameters["speed_settle_seconds"] == 0.2 - assert ( - profile.motion.speed_parameters["command_trajectory_full_range_seconds"] - == 6.0 - ) - assert all(task.preflight_speed_u8 == 1 for task in profile.motion.tasks) - assert all(task.formal_speed_u8 == 1 for task in profile.motion.tasks) - assert profile.vision.tag_ids == frozenset(range(8)) - assert profile.scope.selected_joints("partial") == CALIBRATED_ACTIVE_JOINTS - assert len(profile.joint_coverage) == 11 - assert profile.artifacts.publication_pointer == "latest_partial_passed" - assert profile.zero.coupling_model_by_joint == { - "rh_thumb_dip": "linear_mimic", - "rh_pinky_dip": "linear_mimic", - "rh_index_dip": "linear_mimic", - "rh_middle_dip": "linear_mimic", - "rh_ring_dip": "linear_mimic", - } - assert profile.zero.endpoint_anchor_by_joint == ENDPOINT_ANCHOR_BY_JOINT - - -def test_l6_operator_progress_names_current_task_round_and_direction() -> None: - text = render_l6_progress_zh( - { - "state": "RUNNING", - "serial_number": "L6_TEST", - "step_index": 42, - "step_count": 61, - "step_fraction": 0.5, - "task_name": "pinky_pitch_dip_side", - "phase": "sweep", - "cycle": 3, - "direction": "decreasing", - "attempt": 2, - "target_u8": 0, - "current_command_u8": 130.0, - "actual_u8": 127.5, - "valid_frames": 50, - "total_frames": 52, - "tag_detection_rate": 50 / 52, - "joint_frame_rate": 50 / 52, - "recognized_tag_ids": [3, 4], - "unrecognized_tag_ids": [5], - "feedback_hz": 29.8, - "formal_speed_u8": 1, - } - ) - assert "[L6_TEST] 标定中" in text - assert "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）" in text - assert "阶段：正式扫描（第 4/4 轮）" in text - assert "方向：递减" in text - assert "已识别 ID3/ID4" in text - assert "未识别/不合格 ID5" in text - assert "已自动重扫 1 次" in text - assert "步骤" not in text - - -def test_l6_six_second_cosine_trajectory_is_monotonic_and_smooth() -> None: - start, start_phase, duration = cosine_position_trajectory_u8( - 255.0, 0.0, 0.0, 6.0 - ) - middle, middle_phase, _ = cosine_position_trajectory_u8( - 255.0, 0.0, 3.0, 6.0 - ) - end, end_phase, _ = cosine_position_trajectory_u8( - 255.0, 0.0, 6.0, 6.0 - ) - assert (start, start_phase, duration) == pytest.approx((255.0, 0.0, 6.0)) - assert (middle, middle_phase) == pytest.approx((127.5, 0.5)) - assert (end, end_phase) == pytest.approx((0.0, 1.0)) - - commands = [ - round(cosine_position_trajectory_u8(255.0, 0.0, tick / 100.0, 6.0)[0]) - for tick in range(601) - ] - assert all(right <= left for left, right in zip(commands, commands[1:])) - assert max(abs(right - left) for left, right in zip(commands, commands[1:])) <= 1 - assert cosine_position_trajectory_u8(255.0, 127.5, 1.5, 6.0)[2] == pytest.approx(3.0) - - -def test_l6_operator_progress_shows_exact_failed_sweep_metric() -> None: - text = render_l6_progress_zh( - { - "state": "PAUSED", - "serial_number": "L6_TEST", - "reason": "sweep_quality_failed:thumb_roll_top:maximum_gap=22", - "phase": "sweep", - } - ) - assert "具体未通过项：maximum_gap=22" in text - - -def test_l6_operator_progress_explains_nonlinear_mimic_failure() -> None: - text = render_l6_progress_zh( - { - "state": "PAUSED", - "serial_number": "L6_TEST", - "reason": ( - "fit_or_publication_failed:mimic_residual_exceeds:" - "joint=rh_pinky_dip:multiplier=1.008793:" - "p95_deg=4.715:maximum_deg=6.476" - ), - } - ) - assert "rh_pinky_dip 的 linear_mimic 耦合模型未达到精度门限" in text - assert "P95=4.715°、最大=6.476°" in text - - -def test_l6_product_config_and_immutable_source_hash_are_valid() -> None: - product = load_product_config(PRODUCT, workspace=PACKAGE.parents[1], check_can=False) - assert product.profile_key == KEY - assert product.source_urdf == SOURCE.resolve() - assert sha256_file(SOURCE) == ( - "298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666" - ) - assert product.camera_extrinsics_sha256 == ( - "dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80" - ) diff --git a/src/linkerhand_calibration/test/test_mapping_input_contract.py b/src/linkerhand_calibration/test/test_mapping_input_contract.py deleted file mode 100644 index ded9946..0000000 --- a/src/linkerhand_calibration/test/test_mapping_input_contract.py +++ /dev/null @@ -1,74 +0,0 @@ -"""Timed feedback remains continuous across fitting and serialized replay.""" - -import math - -import pytest - -from linkerhand_calibration.acquisition import StateSample, interpolate_state_u8 -from linkerhand_calibration.core.domain.result import JointMapping -from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping -from linkerhand_calibration.runtime.artifacts.serializers.unified_v1 import _mapping - - -def mapping(domain): - return JointMapping("joint", 0, domain, (0., 100., 255.), - (0., 1., 4.), (0., 1.2, 4.), (0., .8, 4.)) - - -def serialized(curve, version=3): - kind, unit = curve.input_domain.split("_") - payload = { - "format": "unified_calibration_report_v3" if version == 3 else "unified_calibration_v1", - "schema_version": version, "command_unit": unit, "command_names": ["motor"], - "joints": {"joint": {"urdf_joint": "joint", "passive": False, - kind + "_to_rad": _mapping(curve)}}, - } - return SerializedJointMapping(payload, input_kind=kind) - - -@pytest.mark.parametrize("version", [1, 3]) -@pytest.mark.parametrize("direction, at_100", [("", 1.), ("increasing", 1.2), ("decreasing", .8)]) -def test_fractional_feedback_agrees_with_declared_piecewise_linear_curve(version, direction, at_100): - curve = mapping("feedback_u8") - reader = serialized(curve, version) - # A half-byte measured position must not acquire a new quantization jump. - for value in (99.49, 99.50, 99.51): - expected = value / 100. * at_100 - assert curve.evaluate(value, direction) == pytest.approx(expected) - assert reader.evaluate((value,), direction, active_joints=("joint",))["joint"] == pytest.approx(expected) - - -def test_image_between_two_can_receipts_retains_its_interpolated_position(): - state, _ = interpolate_state_u8( - [StateSample(1_000_000, (99.,)), StateSample(3_000_000, (100.,))], - 2_000_000, maximum_skew_ns=2_000_000) - assert state == (99.5,) - reader = serialized(mapping("feedback_u8")) - assert reader.evaluate(state, "increasing", active_joints=("joint",))["joint"] == pytest.approx(1.194) - - -@pytest.mark.parametrize("domain", ["command_u8", "command_rad", "feedback_rad"]) -def test_wire_commands_and_radian_inputs_keep_their_existing_semantics(domain): - curve = mapping(domain) - value = 99.6 - expected_input = 100. if domain == "command_u8" else value - expected = expected_input / 100. - assert curve.evaluate(value) == pytest.approx(expected) - assert serialized(curve).evaluate((value,), "", active_joints=("joint",))["joint"] == pytest.approx(expected) - - -@pytest.mark.parametrize("domain", ["command_u8", "feedback_u8"]) -@pytest.mark.parametrize("value", [-.1, 255.1, math.nan]) -def test_quantization_cannot_hide_an_input_outside_measured_support(domain, value): - curve = mapping(domain) - with pytest.raises(ValueError): - curve.evaluate(value) - with pytest.raises(ValueError): - serialized(curve).evaluate((value,), "", active_joints=("joint",)) - - -@pytest.mark.parametrize("domain", ["command_u8", "feedback_u8"]) -def test_legacy_byte_table_keeps_its_deployed_lookup(domain): - reader = SerializedJointMapping({"schema_version": 6, "curve_input_domain": domain, - "joints": {"joint": {"motor_index": 0, "angle_rad": list(range(256))}}}) - assert reader.evaluate((99.6,), "", active_joints=("joint",))["joint"] == 100. diff --git a/src/linkerhand_calibration/test/test_measured_feedback_time.py b/src/linkerhand_calibration/test/test_measured_feedback_time.py deleted file mode 100644 index 8cbae77..0000000 --- a/src/linkerhand_calibration/test/test_measured_feedback_time.py +++ /dev/null @@ -1,68 +0,0 @@ -"""Feedback synchronization uses each physical receipt, never a repeated cache.""" - -import json -import pytest - -from linkerhand_calibration.acquisition import StateSample, interpolate_state_u8 -from linkerhand_calibration.runtime.feedback import decode_timed_feedback -from runtime_host_fixture import coordinator_fixture - - -def test_independent_channel_times_recover_a_linear_motion_without_cache_delay(): - samples = ( - StateSample(10_000_000, (0., 2.), (1_000_000, 10_000_000)), - StateSample(20_000_000, (10., 2.), (20_000_000, 10_000_000)), - StateSample(30_000_000, (10., 6.), (20_000_000, 30_000_000)), - ) - values, skew = interpolate_state_u8(samples, 15_000_000, maximum_skew_ns=50_000_000) - assert values == pytest.approx((140/19, 3.)) - assert skew == 5_000_000 - - -def test_recent_cache_cannot_hide_a_stale_channel(): - samples = (StateSample(100_000_000, (10., 20.), (1_000_000, 100_000_000)),) - assert interpolate_state_u8(samples, 100_000_000, maximum_skew_ns=50_000_000) is None - - -def test_conflicting_values_for_one_receipt_are_invalid(): - samples = (StateSample(10, (10., 20.), (1, 10)), StateSample(20, (11., 21.), (1, 20))) - assert interpolate_state_u8(samples, 10, maximum_skew_ns=50) is None - - -@pytest.mark.parametrize('change', [dict(schema='heartbeat'), dict(stamp_ns=21), - dict(channel_stamps_ns=[0, 20]), dict(position=[1.]), dict(channel_stamps_ns=[1.0, 20])]) -def test_malformed_receipt_contract_is_rejected(change): - payload = dict(schema='can_feedback_v1', name=['a','b'], position=[1., 2.], - stamp_ns=20, channel_stamps_ns=[10, 20]) - with pytest.raises(ValueError): - decode_timed_feedback(json.dumps({**payload, **change})) - - -def test_duplicate_and_out_of_order_cache_do_not_refresh_watchdog(tmp_path): - host, clock = coordinator_fixture(tmp_path) - try: - names, values = host.profile.command.names, host.profile.command.baseline_values - stamp = host.ports.clock_ns() - host.receive_feedback(names, values, stamp, channel_stamps_ns=(stamp,)*len(values)) - first = tuple(host.state_receive_times) - clock.now += .5 - host.receive_feedback(names, values, stamp, channel_stamps_ns=(stamp,)*len(values)) - host.receive_feedback(names, values, stamp-1, channel_stamps_ns=(stamp-1,)*len(values)) - assert tuple(host.state_receive_times) == first - assert len(host.state_history) == 1 - finally: - host.close() - - -@pytest.mark.parametrize('displacement_ns', [-2_000_000_000, 2_000_000_000]) -def test_one_stale_or_future_channel_cannot_be_hidden_by_fresh_channels(tmp_path, displacement_ns): - host, clock = coordinator_fixture(tmp_path) - try: - names, values = host.profile.command.names, host.profile.command.baseline_values - stamps = [host.ports.clock_ns()]*len(values) - stamps[0] += displacement_ns - host.receive_feedback(names, values, max(stamps), channel_stamps_ns=stamps) - assert not host.state_history - assert not host.state_receive_times - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_measured_finalization.py b/src/linkerhand_calibration/test/test_measured_finalization.py deleted file mode 100644 index 21978fc..0000000 --- a/src/linkerhand_calibration/test/test_measured_finalization.py +++ /dev/null @@ -1,132 +0,0 @@ -"""Production v3 files replay independent synthetic nonlinear passive motion.""" - -import hashlib -import json -import numpy as np -import pytest -from dataclasses import replace -from copy import deepcopy -import shutil - -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 - - -@pytest.fixture(scope="module", params=[False, True], ids=["original_layout", "virtual_reordered"]) -def measured_release(tmp_path_factory, request): - tmp_path = tmp_path_factory.mktemp("measured_release") - profile, source, records, offsets = measured_capture(tmp_path, reordered=request.param) - hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} - hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() - payload, fit, artifact = finalize_profile_session(profile=profile, session_dir=tmp_path/"session", - serial_number="SYNTHETIC_ONLY", source_urdf=source, protected_inputs=hashes, - records=records, standard_loader=lambda path: None) - return profile, source, records, hashes, payload, fit, artifact - - -def test_nonlinear_full_pair_and_separate_mimic_approximation_are_reported(measured_release): - profile, source, records, hashes, payload, fit, artifact = measured_release - assert payload["schema_version"] == 3 - acceptance = json.loads(artifact.staged_release.validation_json) - assert acceptance["independent_mapping_holdout"] - assert acceptance["urdf_joint_angle_source"] == "calibration_json" - assert acceptance["urdf_mimic_policy"] == "preserve_source_mimic" - assert not fit.mimic_approximations - assert acceptance["urdf_mimic_approximation"] - assert max(v["maximum_deg"] for v in acceptance["full_json_urdf_holdout"].values()) < 1 - reader = load_unified_mapper(artifact.path.parent/"release_manifest.json") - assert np.max(np.abs(reader.map_positions(profile.command.baseline_values))) < 1e-10 - from types import SimpleNamespace - from linkerhand_calibration.runtime.artifacts.completion import finish_online_artifacts - completed = finish_online_artifacts(SimpleNamespace(calibration_contract=SimpleNamespace(typed_profile=profile), - session_root=artifact.path.parent.parent), artifact.path.parent, - {"outputs": {"json": str(artifact.staged_release.calibration_json), "urdf": str(artifact.path)}}) - assert completed["URDF"] == str(artifact.path) - from linkerhand_calibration.runtime.adapters.legacy_byte_sdk import LegacyByteSdkAdapter - from linkerhand_calibration.runtime.adapters.base import HardwareHealth - from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint - writes = [] - sdk = LegacyByteSdkAdapter(profile.command, publish=writes.append, - set_speed_callback=lambda *_: None, health_callback=lambda: HardwareHealth(True, True)) - values = [float(31+7*i) for i in range(profile.command.command_count)] - for index in profile.command.disabled_indices: - values[index] = profile.command.baseline_values[index] - sdk.publish_position(values) - assert writes[-1] == tuple(values) - angles = dict(zip(reader.urdf_joint_names, reader.map_positions(values))) - assert any(joint.mimic_joint for joint in reader.urdf.joints.values()) - assert reader.urdf.resolve_angles(angles, independent_mimic_angles=True) == angles - for name, row in payload["joints"].items(): - assert row["sdk_channel"] == channel_for_joint(profile, name) - assert angles[name] == pytest.approx(row["angle_rad"][int(values[row["sdk_channel"]])], abs=1e-12) - - -@pytest.mark.parametrize("field", ["calibration_json", "corrected_urdf", "calibration_report"]) -def test_v3_modified_artifact_is_rejected(measured_release, tmp_path, field): - *_, artifact = measured_release - shutil.copytree(artifact.path.parent, tmp_path/"copy") - manifest = tmp_path/"copy/release_manifest.json" - target = manifest.parent/json.loads(manifest.read_text())[field] - target.write_text(target.read_text()+"\n") - with pytest.raises(ValueError, match="SHA256 changed"): - load_unified_mapper(manifest) - - -def test_missing_geometry_or_transferred_curve_cannot_be_published(measured_release): - from linkerhand_calibration.core.urdf.result_plan import prepare_standard_result - profile, source, records, hashes, payload, fit, artifact = measured_release - offsets = dict(fit.zero_offsets_rad) - offsets.pop(next(iter(profile.zero.passive_joints))) - with pytest.raises(ValueError, match="absolute_geometry"): - prepare_standard_result(profile, source, hashes["source_urdf_sha256"], replace(fit, zero_offsets_rad=offsets)) - mappings = dict(fit.output_mappings) - name = next(iter(mappings)) - mappings[name] = replace(mappings[name], transferred_from="different_physical_joint") - with pytest.raises(ValueError, match="transferred"): - prepare_standard_result(profile, source, hashes["source_urdf_sha256"], replace(fit, output_mappings=mappings)) - - -def test_v3_native_reader_uses_all_passive_curves_and_rejects_legacy_promotion(measured_release): - from linkerhand_calibration.runtime.artifacts.reader import UnifiedCommandMapper - from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import from_report - profile, source, records, hashes, payload, fit, artifact = measured_release - report = json.loads((artifact.path.parent/"calibration_report.json").read_text()) - native = deepcopy(report) - native["command_unit"] = "rad" - native["baseline_command"] = [v/255 for v in native["baseline_command"]] - for row in native["joints"].values(): - row["zero_reference"]["baseline_command"] /= 255 - for kind in ("command", "feedback"): - data = row[kind+"_to_rad"] - data.update(input_domain=kind+"_rad", input_unit="rad", - knots=[v/255 for v in data["knots"]], valid_input_range=[v/255 for v in data["valid_input_range"]]) - compact = from_report(native) - byte_reader = UnifiedCommandMapper(payload, artifact.path, report=report) - rad_reader = UnifiedCommandMapper(compact, artifact.path, report=native) - command = [100.]*profile.command.command_count - np.testing.assert_allclose(byte_reader.map_positions(command), rad_reader.map_positions([v/255 for v in command]), atol=1e-12) - wrong = deepcopy(report) - name = next(iter(wrong["joints"])) - wrong["joints"][name]["zero_reference"]["channel"] = 999 - with pytest.raises(ValueError, match="zero_evidence"): - from_report(wrong) - wrong["format"] = "unified_calibration_v1" - with pytest.raises(ValueError, match="legacy promotion"): - from_report(wrong) - - -def test_final_table_cannot_hide_fourth_round_zero_shift(measured_release): - from scipy.spatial.transform import Rotation - from linkerhand_calibration.core.urdf.measured_acceptance import validate_measured_joint_holdout - from linkerhand_calibration.runtime.acquisition import accepted_joint_records - profile, source, records, hashes, payload, fit, artifact = measured_release - steady = accepted_joint_records(profile, records, sample_phase="steady") - heldout = [dict(r) for rows in steady.values() for r in rows if r["cycle"] == 3] - name = next(iter(fit.curves)) - for row in heldout: - if row["joint"] == name: - extra = Rotation.from_rotvec(np.asarray(fit.curves[name].circle["axis_xyz"])*np.deg2rad(5)) - row["relative_quaternion_xyzw"] = (Rotation.from_quat(row["relative_quaternion_xyzw"])*extra).as_quat().tolist() - with pytest.raises(ValueError, match="inaccurate"): - validate_measured_joint_holdout(payload, fit.curves, heldout) diff --git a/src/linkerhand_calibration/test/test_measured_transfer.py b/src/linkerhand_calibration/test/test_measured_transfer.py deleted file mode 100644 index db1c3d6..0000000 --- a/src/linkerhand_calibration/test/test_measured_transfer.py +++ /dev/null @@ -1,278 +0,0 @@ -"""Real DIP failure: bounded measured geometry helps fit, not branch certainty.""" - -from copy import deepcopy -from dataclasses import asdict, replace -import gzip -import json -from pathlib import Path - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_model_payload -from linkerhand_calibration.core.geometry.tag_pose.measured_transfer import MEASURED_TRANSFER_POLICY -from linkerhand_calibration.core.geometry.tag_pose.measured_transfer_solver import MeasuredTransferImageHingeBundle -from linkerhand_calibration.core.geometry.tag_pose.motion_image_diagnostics import _validate_frames -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( - ImageMotionParameters, image_motion_model_from_dict, resolve_image_motion, _model, -) -from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import CadImageHingeBundle -from linkerhand_calibration.core.geometry.tag_pose.hinge_uncertainty import hinge_uncertainty -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.branch_initialization import ( - BranchInitialization, BranchInitializationRequest, solve_initialization, initialization_artifacts, -) -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import ( - _initializations, validate_source_geometry, _check_measured_transfer_report, -) -from linkerhand_calibration.runtime.passed_resume import restore_parent_models -from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh -from o30_capture_fixture import SOURCE -from test_transferred_hinge_geometry import recorded_transfer - - -@pytest.fixture(scope="module") -def middle_dip(): - path = Path(__file__).parent / "fixtures/o30_measured_transfer_middle_dip.json.gz" - data = json.loads(gzip.decompress(path.read_bytes())) - rows, failure = data["records"], data["original_failure"] - profile = load_bundled_hand_profile("o30_right_18") - owner = BranchInitialization(profile, source_urdf=SOURCE) - restore_parent_models(owner.parent_references, - [r for r in rows if r["kind"] == "motion_branch_initialization"], failure["session_epoch"]) - verified = next(r for r in rows if r["kind"] == "parent_reference_verified") - owner.parent_references.verified[(failure["session_epoch"], failure["task_name"])] = verified - spec = profile.motion.joint_zero_references["middle_dip"] - motion = MotionCommand("zero_approach", spec.command, 200., - task_key=failure["task_name"], reference_joints=("middle_dip",)) - owner.begin(motion, session_epoch=failure["session_epoch"], motion_version=failure["motion_version"]) - for row in rows: - if row["kind"] == "motion_branch_observation" and row["task_name"] == failure["task_name"]: - owner.observe(row) - request = owner.request("side", replace(motion, phase="joint_zero", zero_joints=("middle_dip",)), - session_epoch=failure["session_epoch"], motion_version=failure["motion_version"]+1) - assert not request.invalid_reason - return data, request, solve_initialization(request) - - -def test_real_middle_dip_fits_but_remains_ambiguous(middle_dip): - data, request, result = middle_dip - assert len(request.image_frames) == 129 - assert all(h["validation_quality"][0]["maximum_frame_rms_px"] > 2.8 - for h in data["original_failure"]["hypotheses"]) - assert not result.resolved and result.reason == "image_motion_families_not_distinguishable" - assert result.model is None - assert len(result.hypotheses) == 2 - for candidate in result.hypotheses: - assert candidate.converged and candidate.training_accepted and not candidate.reason - assert candidate.validation_quality[0].rms_px < .23 - assert candidate.validation_quality[0].maximum_frame_rms_px < .36 - assert all(check.consistent for check in candidate.source_geometry_checks) - check = candidate.source_geometry_checks[0] - assert check.axis_allowance_rad == request.source_hinges[0].child_axis_uncertainty_rad - assert any(p > .01 for h in result.hypotheses for _, p in h.comparison_adjusted_p_values) - frozen, report = initialization_artifacts(request, result) - assert frozen is None and not report["evidence_ids"] - assert "frozen_image_model" not in report - - -def test_real_source_evidence_survives_readback(middle_dip): - data, _, _ = middle_dip - assert len(_initializations(data["records"])) == 2 - validate_source_geometry(load_bundled_hand_profile("o30_right_18"), SOURCE, data["records"]) - - -@pytest.fixture(scope="module") -def middle_fit(middle_dip): - _, request, _ = middle_dip - limits = ImageMotionParameters() - paths, roles = _validate_frames(request.image_frames, request.relations, limits) - poses = {role: paths[role][0][::2] for role in roles} - bundle = MeasuredTransferImageHingeBundle(request.image_frames[::2], request.relations, poses, - constraints=request.geometry_constraints, sources=request.source_hinges) - return request, bundle, bundle.solve(limits), roles - - -def test_uncertainty_restores_unbounded_image_geometry(middle_fit): - _, bundle, fit, _ = middle_fit - free, linearization = bundle.uncertainty_problem(fit) - np.testing.assert_allclose(free.residual(linearization.x), bundle.residual(fit.x), atol=1e-7) - assert not hasattr(free, "lower") - assert hinge_uncertainty(bundle, fit) == hinge_uncertainty(free, linearization) - assert hinge_uncertainty(bundle, fit)[0][0][1] > .001 - - -def test_legacy_exact_transfer_reproduces_failure_and_still_deserializes(middle_fit): - request, bounded, _, roles = middle_fit - exact = CadImageHingeBundle(bounded.frames, bounded.relations, bounded.poses, - constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256, - source_hinges=request.source_hinges) - fit = exact.solve(ImageMotionParameters()) - assert fit.success - residual = exact.residual(fit.x).reshape(len(exact.frames), -1) - assert np.sqrt(np.mean(residual**2, axis=1)).max() > 1.5 - model = _model(exact, fit, request.image_frames, tuple((r, 0) for r in roles), - tuple(range(0, 129, 2)), tuple(range(1, 129, 2))) - assert model.policy == "image_hinge_v3_transferred_parallel_geometry" - assert image_motion_model_from_dict(asdict(model)) == model - - -@pytest.fixture(scope="module") -def ip_report(recorded_transfer): - _, frames, relations, options, result = recorded_transfer - request = BranchInitializationRequest("thumb_ip_front", "front", ("thumb_ip",), 1, 1, 1, - tuple(f.evidence for f in frames), relations, (), ("thumb_ip",), - image_frames=frames, image_geometry=True, **options) - _, report = initialization_artifacts(request, result) - return json.loads(json.dumps(report)) - - -def test_resolved_ip_preserves_full_report_and_uncertainty_binding(ip_report): - _check_measured_transfer_report(ip_report) - model = image_motion_model_from_dict(ip_report["frozen_image_model"]) - assert model.policy == MEASURED_TRANSFER_POLICY - assert model.maximum_reprojection_error_px == 1.5 - assert model.reprojection_tie_px == .03 - assert image_model_sha256(image_model_payload(model)) == ip_report["image_model_sha256"] - - -def test_source_bounds_do_not_authorize_corrupt_current_images(recorded_transfer): - _, original, relations, options, baseline = recorded_transfer - assert baseline.resolved - frames = list(original) - frame = frames[20] - observations = list(frame.observations) - child = observations[-1] - corners = list(child.corners_xy) - corners[0] = (corners[0][0]+20., corners[0][1]) - observations[-1] = replace(child, corners_xy=tuple(corners)) - frames[20] = replace(frame, observations=tuple(observations)) - result = resolve_image_motion(tuple(frames), relations, **options) - assert not result.resolved and result.model is None - assert not any(h.training_accepted for h in result.hypotheses) - - -@pytest.mark.parametrize("change", ["axis", "distance", "precision", "graph", "roles"]) -def test_model_readback_rejects_geometry_outside_source_domain(recorded_transfer, change): - model = recorded_transfer[-1].model - geometry = model.geometry[0] - if change == "axis": - axis = np.asarray(geometry.axis_parent_xyz) - tangent = np.cross(axis, [1, 0, 0]); tangent /= np.linalg.norm(tangent) - axis = Rotation.from_rotvec(tangent*.1).apply(axis) - kwargs = {"geometry": (replace(geometry, axis_parent_xyz=tuple(axis)),)} - elif change == "distance": - kwargs = {"geometry": (replace(geometry, pivot_parent_xyz_m=(1., 1., 1.)),)} - elif change == "precision": - kwargs = {"current_geometry_uncertainty": (("thumb_ip", .2, .001),)} - elif change == "graph": - kwargs = {"constraints": model.constraints*2} - else: - kwargs = {"geometry": (replace(geometry, relation=replace(geometry.relation, parent_role="wrong")),)} - with pytest.raises(ValueError): - replace(model, **kwargs) - - -@pytest.mark.parametrize("change", ["axes", "points", "checks", "child", "accepted", "missing"]) -def test_report_readback_rejects_detached_uncertainty(ip_report, change): - row = deepcopy(ip_report) - winner = next(h for h in row["hypotheses"] if h["branches"] == row["frozen_image_model"]["branches"]) - if change == "axes": winner["axis_uncertainty_95_rad"][0][1] /= 2 - elif change == "points": winner["pivot_uncertainty_95_m"][0][1] /= 2 - elif change == "checks": winner["source_geometry_checks"][0]["axis_allowance_rad"] *= 2 - elif change == "child": row["geometry_uncertainty"][0][1] /= 2 - elif change == "accepted": winner["training_accepted"] = False - else: row["hypotheses"].remove(winner) - with pytest.raises(ValueError, match="measured_transfer_"): - _check_measured_transfer_report(row) - - -def test_ambiguity_has_an_actionable_operator_message(): - code, message, action = reason_zh({"reason": - "joint_zero_motion_unresolved:view=side:joints=middle_dip:image_motion_families_not_distinguishable"}, - model_name="O30") - assert code == "OBS-POSE-AMBIGUOUS-117" - assert "多个" in message and "独立观测" in action and "已通过的数据保留" in action - - -@pytest.fixture(scope="module") -def thumb_ip_uncertain_alternative(): - path = Path(__file__).parent / "fixtures/o30_thumb_ip_uncertain_alternative.json.gz" - data = json.loads(gzip.decompress(path.read_bytes())) - rows, failure = data["records"], data["original_failure"] - profile = load_bundled_hand_profile("o30_right_18") - owner = BranchInitialization(profile, source_urdf=SOURCE) - restore_parent_models(owner.parent_references, - [r for r in rows if r["kind"] == "motion_branch_initialization"], failure["session_epoch"]) - verified = next(r for r in rows if r["kind"] == "parent_reference_verified") - owner.parent_references.verified[(failure["session_epoch"], failure["task_name"])] = verified - spec = profile.motion.joint_zero_references["thumb_ip"] - motion = MotionCommand("zero_approach", spec.command, 200., - task_key=failure["task_name"], reference_joints=("thumb_ip",)) - owner.begin(motion, session_epoch=failure["session_epoch"], motion_version=failure["motion_version"]) - for row in rows: - if row["kind"] == "motion_branch_observation": - owner.observe(row) - request = owner.request("front", replace(motion, phase="joint_zero", zero_joints=("thumb_ip",)), - session_epoch=failure["session_epoch"], motion_version=failure["motion_version"]+1) - assert not request.invalid_reason - assert tuple(f.evidence.stamp_ns for f in request.image_frames) == tuple(failure["source_image_stamps"]) - return request, solve_initialization(request) - - -def test_real_ip_compares_uncertain_alternative_before_authorizing(thumb_ip_uncertain_alternative): - request, result = thumb_ip_uncertain_alternative - assert result.resolved, result.reason - winner, alternative = result.hypotheses - assert winner.training_accepted and not winner.reason - assert alternative.converged and not alternative.training_accepted - assert alternative.reason == "image_motion_geometry_uncertain" - assert alternative.training_solver_evaluations > 0 - assert alternative.training_quality and alternative.validation_quality - assert winner.comparison_adjusted_p_values[0][0] == 1 - assert winner.comparison_adjusted_p_values[0][1] <= .01 - assert winner.validation_quality[0].maximum_frame_rms_px < .464 - assert result.model.maximum_reprojection_error_px == 1.5 - assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) - frozen, report = initialization_artifacts(request, result) - assert frozen is not None - _check_measured_transfer_report(report) - assert image_motion_model_from_dict(report["frozen_image_model"]) == result.model - - -def test_uncertain_alternative_cannot_be_frozen(thumb_ip_uncertain_alternative): - from linkerhand_calibration.core.geometry.tag_pose.measured_transfer import transfer_geometry_checks - - request, result = thumb_ip_uncertain_alternative - alternative = result.hypotheses[1] - source = request.source_hinges[0] - constraint = request.geometry_constraints[0] - # Remove the inherited part to reconstruct the alternative's actual local - # bounds. Reusing valid geometry isolates the precision authorization gate. - bounds = (("thumb_ip", alternative.axis_uncertainty_95_rad[0][1]-source.child_axis_uncertainty_rad, - alternative.pivot_uncertainty_95_m[0][1]-source.child_point_uncertainty_m - -constraint.distance_m*source.child_axis_uncertainty_rad),) - with pytest.raises(ValueError, match="image_motion_geometry_uncertain"): - transfer_geometry_checks(result.model.geometry, request.geometry_constraints, request.source_hinges, bounds) - with pytest.raises(ValueError, match="image_motion_geometry_uncertain"): - replace(result.model, current_geometry_uncertainty=bounds) - - -def test_uncertain_alternative_still_vetoes_when_images_do_not_distinguish( - thumb_ip_uncertain_alternative, monkeypatch): - from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver - - request, result = thumb_ip_uncertain_alternative - # Isolate the selection boundary: equally good images, distinct poses, and - # the actual precision-ineligible hypothesis must not authorize the winner. - monkeypatch.setattr(solver, "equivalent_frame_selections", lambda *args: False) - validation = tuple(i for i, f in enumerate(request.image_frames) - if f.evidence.stamp_ns in result.model.validation_stamps_ns) - rejected = solver.select_image_hypotheses(request.image_frames, validation, - list(result.hypotheses), [result.model, result.model], - [np.zeros(len(validation)), np.zeros(len(validation))], ImageMotionParameters()) - assert not rejected.resolved and rejected.model is None - assert rejected.reason == "image_motion_families_not_distinguishable" diff --git a/src/linkerhand_calibration/test/test_measured_transfer_finalization.py b/src/linkerhand_calibration/test/test_measured_transfer_finalization.py deleted file mode 100644 index 9742957..0000000 --- a/src/linkerhand_calibration/test/test_measured_transfer_finalization.py +++ /dev/null @@ -1,117 +0,0 @@ -"""O6 synthetic records traverse the actual mixed v3 production finalizer. - -The elementary-FK fixture supplies synthetic geometry datums and its existing -virtual limits. This is a software integration check, not O6 hardware evidence. -""" - -import hashlib -import json - -import pytest - -from measured_capture_fixture import measured_capture -from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session -from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper - - -@pytest.mark.parametrize("reordered,separate_mapping", [(False, False), (True, False), (False, True)], - ids=["o6", "reordered_virtual_o6", "separate_command_mapping"]) -def test_o6_records_fit_expand_publish_and_read_back_without_recipient_observations(tmp_path, monkeypatch, reordered, separate_mapping): - profile, source, records, offsets = measured_capture(tmp_path, layout="o6_right_8", reordered=reordered) - if separate_mapping: - # This is synthetic evidence: exercise distinct motion and command - # partitions without claiming an additional physical recording. - from dataclasses import replace - profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode="separate")) - records = [dict(r, cycle={0: 4, 3: 5}[r["cycle"]]) if r.get("sample_phase") == "steady" else r - for r in records if r.get("sample_phase") != "steady" or r["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} - selected = [] - for row in records: - if row.get("sample_phase") != "steady": - selected.append(row) - continue - plan = plans[row["task_name"]] - if row["cycle"] == 4 and row["steady_target"] not in plan["training_nodes"]: - continue - selected.append(dict(row, steady_training_nodes=plan["training_nodes"], sampling_plan_sha256=plan["plan_sha256"])) - records = [*selected, *plans.values()] - from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher - from test_final_image_holdout import independent_pixels - original = ArtifactPublisher.prepare - def prepare(publisher, **kwargs): - validator = kwargs["validate"] - kwargs["validate"] = replace(validator, require_image_holdout=True, - image_observations=independent_pixels(validator.observations), - command_image_observations=independent_pixels(validator.command_observations)) - return original(publisher, **kwargs) - monkeypatch.setattr(ArtifactPublisher, "prepare", prepare) - source_bytes = source.read_bytes() - transfers = dict(profile.measurement.transferred_motion_sources) - assert len(transfers) == 6 - observed = {row["joint"] for row in records if row.get("kind") == "joint_sample"} - assert len(observed) == 5 and not observed & transfers.keys() - assert {row["joint"] for row in records if row.get("kind") == "joint_zero_reference"} == observed - hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} - hashes["source_urdf_sha256"] = hashlib.sha256(source_bytes).hexdigest() - phases = [] - payload, fit, artifact = finalize_profile_session(profile=profile, session_dir=tmp_path/"session", - serial_number="SYNTHETIC_O6_ONLY", source_urdf=source, protected_inputs=hashes, - records=records, standard_loader=lambda _: None, phase_changed=phases.append) - assert source.read_bytes() == source_bytes - assert phases == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"] - moving = {name for name, joint in UrdfKinematicModel(source).joints.items() if joint.kind != "fixed"} - assert len(moving) == 11 and set(payload["joints"]) == moving - assert set(fit.curves) == set(fit.joint_zero_references) == set(fit.command_holdout_metrics) == observed - assert set(fit.command_mappings) == set(fit.output_mappings) == moving - declared = profile.artifacts.directional_command_joints - assert set(payload["directional_command_joints"]) == declared - for name, evidence in fit.command_applicability.items(): - assert (evidence.get("command_mapping") == "directional") == (name in declared) - report = json.loads((artifact.path.parent/"calibration_report.json").read_text()) - acceptance = json.loads(artifact.staged_release.validation_json) - if separate_mapping: - assert acceptance["final_file_image_holdout_verified"] is True - assert set(acceptance["final_file_image_holdout"]) == {"feedback", "command"} - assert (artifact.path.parent/"command_sampling_plans.json").is_file() - assert acceptance["command_holdout_cycle"] == (5 if separate_mapping else 3) - assert acceptance["acceptance"] == "measured_and_transferred_json_urdf_v3" - assert set(acceptance["independent_mapping_holdout"]) == observed - assert acceptance["transferred_motion_sources"] == transfers - observed_roles = {profile.measurement.measurements[name].child_role for name in observed} - assert set(acceptance["full_json_urdf_holdout"]) == observed_roles - assert acceptance["urdf_joint_angle_source"] == "calibration_json" - assert acceptance["urdf_mimic_policy"] == "preserve_source_mimic" - assert not fit.mimic_approximations - assert acceptance["urdf_mimic_approximation"] - original_model, exported_model = UrdfKinematicModel(source), UrdfKinematicModel(artifact.path) - assert {name: j.mimic_joint for name, j in exported_model.joints.items()} == { - name: j.mimic_joint for name, j in original_model.joints.items()} - for name, original in original_model.joints.items(): - if original.mimic_joint: - actual = exported_model.joints[name] - assert (actual.mimic_multiplier, actual.mimic_offset) == ( - original.mimic_multiplier, original.mimic_offset) - for target, donor in transfers.items(): - assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"] - assert payload["joints"][target]["sdk_channel"] == channel_for_joint(profile, target) - assert fit.command_mappings[target].transferred_from == donor - row = report["joints"][target] - assert row["independently_measured"] is False and "zero_reference" not in row - assert "command_holdout" not in row and row["command_holdout_source_joint"] == donor - for name, angle in offsets.items(): - assert fit.zero_offsets_rad[name] == pytest.approx(angle, abs=.004) - mapper = load_unified_mapper(artifact.path.parent/"release_manifest.json") - assert mapper.transferred_motion_sources == transfers - baseline = profile.command.baseline_values - assert max(abs(value) for value in mapper.map_positions(baseline)) < 1e-10 - target = "rh_index_mcp_pitch" - command = list(baseline) - command[channel_for_joint(profile, target)] = 102 - angles = dict(zip(mapper.urdf_joint_names, mapper.map_positions(command))) - assert angles["rh_pinky_mcp_pitch"] == pytest.approx(0, abs=1e-10) - assert angles[target] == payload["joints"][target]["angle_rad"][102] - assert angles["rh_index_dip"] == payload["joints"]["rh_index_dip"]["angle_rad"][102] diff --git a/src/linkerhand_calibration/test/test_measured_zero_mapping.py b/src/linkerhand_calibration/test/test_measured_zero_mapping.py deleted file mode 100644 index 72ecad4..0000000 --- a/src/linkerhand_calibration/test/test_measured_zero_mapping.py +++ /dev/null @@ -1,47 +0,0 @@ -"""Frozen zeros and nonlinear passive motion must survive independent holdout.""" - -import math -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting.anchored_mapping import anchored_mean -from linkerhand_calibration.core.fitting.rotation_curve import RotationObservation, fit_rotation_curve -from linkerhand_calibration.core.fitting.coupling import fit_standard_mimic - - -def observations(*, slip=0.0): - mount = Rotation.from_euler("xyz", [0.3, -.8, .4]) - return [RotationObservation(f"{cycle}:{direction}:{i}", float(x), - tuple((mount * Rotation.from_rotvec([0, 0, .5*x+.3*x*x+(slip if cycle == 3 else 0)])).as_quat()), - cycle, direction) - for cycle in range(4) for direction in ("increasing", "decreasing") - for i, x in enumerate(np.linspace(0, 1, 65))], mount - - -def test_frozen_oblique_zero_preserves_nonlinear_motion_and_detects_holdout_slip(): - rows, mount = observations() - fit = fit_rotation_curve(rows[:390], input_domain="feedback_rad", sdk_to_joint_direction=1, - reference_xyzw=mount.as_quat()) - assert fit.measure(mount.as_quat())[0] == pytest.approx(0, abs=1e-12) - assert max(fit.holdout_errors(rows[390:])) < 1e-10 - slipped, _ = observations(slip=math.radians(5)) - assert min(fit.holdout_errors(slipped[390:])) > math.radians(4.9) - x = [r.input_value for r in rows[:390]] - y = [fit.measure(r.quaternion_xyzw)[0] for r in rows[:390]] - linear = fit_standard_mimic("parent", "child", x, y, - cycles=[r.cycle for r in rows[:390]], offset_rad=0) - assert linear.offset_rad == 0 - assert max(abs(e) for e in linear.residuals_rad) > math.radians(3) - - -def test_baseline_anchor_is_a_constraint_not_branch_recentering(): - increasing = [-.39, .005, .41] - decreasing = [-.41, -.001, .39] - result = anchored_mean([0, 1, 2], increasing, decreasing, 1, 1) - assert result == pytest.approx([-.4, 0, .4]) - assert increasing[1] == .005 and decreasing[1] == -.001 - with pytest.raises(ValueError, match="observed_training_knot"): - anchored_mean([0, 2], [-.4, .4], [-.4, .4], 1, 1) - with pytest.raises(ValueError, match="inconsistent"): - anchored_mean([0, 1, 2], [-.4, .1, .4], [-.4, .1, .4], 1, 1) diff --git a/src/linkerhand_calibration/test/test_motion_branch_evidence.py b/src/linkerhand_calibration/test/test_motion_branch_evidence.py deleted file mode 100644 index 27980b3..0000000 --- a/src/linkerhand_calibration/test/test_motion_branch_evidence.py +++ /dev/null @@ -1,257 +0,0 @@ -"""Physical motion evidence can authorize a branch only before zero freeze.""" - -from dataclasses import FrozenInstanceError, replace -import json -import math -from pathlib import Path - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import ( - MotionEvidenceFrame, MotionEvidenceParameters, MotionRelation, RoleCandidates, - resolve_motion_branches, select_motion_frame, -) -from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - - -RELATIONS = (MotionRelation("joint_a", "base", "parent"), - MotionRelation("joint_b", "parent", "child")) - - -def pose(rotation, translation, error=0.4): - return SquareTagPose(tuple(rotation.as_quat()), tuple(translation), error) - - -def motion_frames(*, span_deg=80, samples=81, bad_alternative=True, radius=0.05): - """Two independently articulated links, each with an image-favored false root.""" - frames = [] - for i, theta in enumerate(np.linspace(0, math.radians(span_deg), samples)): - base = pose(Rotation.identity(), (0, 0, 1), 0.1) - parent_r = Rotation.from_rotvec([0, 0, theta]) - parent_t = np.array([0, 0, 1]) + parent_r.apply([radius, 0, 0]) - child_r = parent_r * Rotation.from_rotvec([theta * 0.4, 0, 0]) - child_t = parent_t + parent_r.apply([0.03, 0, 0]) + child_r.apply([0, radius, 0]) - parent = pose(parent_r, parent_t) - child = pose(child_r, child_t) - # Change orientation, not measured image translation. The alternate - # cannot describe the same hinge axis and pivot over a sufficient arc. - wrong_r = Rotation.from_euler("y", 35, degrees=True) - parent_alt = pose(wrong_r * parent_r, parent_t, 0.1) - child_alt = pose(wrong_r * child_r, child_t, 0.1) - if not bad_alternative: - # An unknown constant Tag mounting rotation is unobservable from - # this motion: both families are physically admissible. - parent_alt = pose(parent_r * Rotation.from_euler("z", 40, degrees=True), parent_t, 0.1) - child_alt = pose(child_r * Rotation.from_euler("z", 40, degrees=True), child_t, 0.1) - frames.append(MotionEvidenceFrame(1_000_000_000 + i * 50_000_000, ( - RoleCandidates("base", (base,), frozen=True), - RoleCandidates("parent", (parent_alt, parent)), - RoleCandidates("child", (child_alt, child)), - ))) - return frames - - -def test_joint_parent_child_geometry_selects_actual_candidates_not_image_minimum(): - frames = motion_frames() - result = resolve_motion_branches(frames, RELATIONS) - assert result.resolved, result - assert len(result.constraints) == 2 - assert [v.pose for v in result.selections] == [v.candidates[-1] for v in frames[-1].roles] - assert sum(h.accepted for h in result.hypotheses) == 1 - assert all(model.source_stamps_ns for model in result.constraints) - with pytest.raises(FrozenInstanceError): - result.constraints[0].maximum_pivot_error_m = 1 - - -def test_postfreeze_selection_uses_same_immutable_model_and_original_current_candidates(): - frames = motion_frames() - result = resolve_motion_branches(frames[::2], RELATIONS) - assert result.resolved - models_before = result.constraints - for frame in frames[1::2]: - selected = select_motion_frame(result.constraints, frame.roles) - assert selected.resolved, selected - assert all(item.pose is frame.roles[i].candidates[-1] for i, item in enumerate(selected.selections)) - assert result.constraints == models_before - - -def test_constant_mount_ambiguity_is_not_resolved_by_lowest_error_or_smoothness(): - result = resolve_motion_branches(motion_frames(bad_alternative=False), RELATIONS) - assert not result.resolved - assert result.reason == "motion_geometry_ambiguous" - assert result.constraints == result.selections == () - - -@pytest.mark.parametrize("span", [0, 3, 10]) -def test_insufficient_rotation_cannot_freeze_even_with_many_frames(span): - result = resolve_motion_branches(motion_frames(span_deg=span, samples=300), RELATIONS) - assert not result.resolved - assert any(q.reason == "insufficient_rotation_evidence" for h in result.hypotheses for q in h.relations) - - -def test_stationary_endpoints_do_not_inflate_motion_evidence(): - samples = motion_frames(samples=5) - repeated = [replace(frame, stamp_ns=1_000_000_000 + i * 10_000_000) - for i, frame in enumerate(frame for frame in samples for _ in range(100))] - result = resolve_motion_branches(repeated, RELATIONS) - assert not result.resolved - assert all(q.motion_bins <= 5 for h in result.hypotheses for q in h.relations) - - -def test_early_late_validation_rejects_a_mid_approach_mount_shift(): - frames = motion_frames() - changed = [] - for index, frame in enumerate(frames): - roles = list(frame.roles) - if index > len(frames) // 2: - roles[1] = replace(roles[1], candidates=tuple(replace(p, - translation_xyz_m=tuple(np.asarray(p.translation_xyz_m) + [0, 0, 0.002])) - for p in roles[1].candidates)) - changed.append(replace(frame, roles=tuple(roles))) - result = resolve_motion_branches(changed, RELATIONS[:1]) - assert not result.resolved - quality = result.hypotheses[1].relations[0] - assert quality.pivot_rms_m < 0.0015 # A pooled fit alone would hide this shift. - assert quality.reason == "motion_split_validation_failed" - - -def test_rotation_without_resolved_position_arc_is_insufficient(): - result = resolve_motion_branches(motion_frames(radius=0), RELATIONS[:1]) - assert not result.resolved - assert all(h.relations[0].reason == "insufficient_pivot_translation" for h in result.hypotheses) - - -def test_image_identity_and_changed_frozen_contract_are_rejected(): - frames = motion_frames() - result = resolve_motion_branches([frames[0], *frames], RELATIONS) - assert result.reason == "motion_evidence_image_identity_invalid" - roles = frames[20].roles - frames[20] = replace(frames[20], roles=(replace(roles[0], frozen=False), roles[1], roles[2])) - assert resolve_motion_branches(frames, RELATIONS).reason == "motion_candidate_family_interrupted" - - -@pytest.mark.parametrize("invalid", ["reprojection", "tilt", "nan"]) -def test_invalid_alternative_does_not_discard_the_other_legal_candidate(invalid): - frames = motion_frames() - first = frames[0] - roles = list(first.roles) - target = roles[1].candidates[0] - if invalid == "reprojection": - target = replace(target, reprojection_error_px=1.50001) - elif invalid == "tilt": - target = replace(target, quaternion_xyzw=tuple(Rotation.from_euler("x", 76, degrees=True).as_quat())) - else: - target = replace(target, translation_xyz_m=(float("nan"), 0, 1)) - roles[1] = replace(roles[1], candidates=(target, roles[1].candidates[-1])) - frames[0] = replace(first, roles=tuple(roles)) - result = resolve_motion_branches(frames, RELATIONS) - assert result.resolved - selected = select_motion_frame(result.constraints, frames[0].roles) - assert selected.resolved and selected.selections[1].pose is frames[0].roles[1].candidates[-1] - bad_only = (*frames[0].roles[:1], replace(roles[1], candidates=(target,)), roles[2]) - assert not select_motion_frame(result.constraints, bad_only).resolved - - -def test_temporarily_missing_root_is_not_copied_to_another_family(): - frames = motion_frames() - roles = frames[20].roles - frames[20] = replace(frames[20], roles=(roles[0], - replace(roles[1], candidates=roles[1].candidates[:1]), roles[2])) - result = resolve_motion_branches(frames, RELATIONS) - assert result.resolved - assert all(frames[20].stamp_ns not in model.source_stamps_ns for model in result.constraints) - - -def test_existing_frozen_candidate_is_never_replaced_by_better_geometry(): - frames = motion_frames() - fixed_wrong = [replace(frame, roles=(frame.roles[0], - RoleCandidates("parent", (frame.roles[1].candidates[0],), frozen=True), frame.roles[2])) for frame in frames] - result = resolve_motion_branches(fixed_wrong, RELATIONS) - assert not result.resolved - assert result.constraints == result.selections == () - - -def test_frozen_role_cannot_offer_two_roots_to_training(): - frames = motion_frames() - frame = frames[0] - frames[0] = replace(frame, roles=(frame.roles[0], replace(frame.roles[1], frozen=True), frame.roles[2])) - assert resolve_motion_branches(frames, RELATIONS).reason == "motion_candidates_missing_or_invalid" - - -def test_postfreeze_tag_slide_is_rejected_without_model_update(): - frames = motion_frames() - result = resolve_motion_branches(frames, RELATIONS) - frame = frames[40] - roles = list(frame.roles) - roles[2] = replace(roles[2], candidates=tuple(replace(p, - translation_xyz_m=tuple(np.asarray(p.translation_xyz_m) + [0, 0, 0.01])) for p in roles[2].candidates)) - selected = select_motion_frame(result.constraints, tuple(roles)) - assert not selected.resolved - assert selected.reason == "motion_geometry_inconsistent" - - -def test_postfreeze_two_physical_choices_remain_ambiguous(): - frames = motion_frames() - result = resolve_motion_branches(frames, RELATIONS[:1]) - frame = frames[40] - # Two distinct points along the same previously fitted hinge both explain - # their candidate poses. Geometry alone must not pick the smaller error. - role = RoleCandidates("parent", (frame.roles[1].candidates[-1], frames[50].roles[1].candidates[-1])) - selected = select_motion_frame(result.constraints, (frame.roles[0], role)) - assert not selected.resolved and selected.reason == "motion_geometry_ambiguous" - - -def real_frames(direction, field="frames"): - source = Path(__file__).with_name("fixtures") / "o6_thumb_20260914_175815_motion_evidence.json" - fixture = json.loads(source.read_text()) - assert fixture["scope"] == "single_round_counterfactual_branch_regression_not_accuracy_certificate" - assert fixture["source_raw_sha256"] == "8bdbf216bd4135f1bac16dee0535a8b72ea0e6b0f31ab2b9a2e8cc4a83be9c0d" - return [MotionEvidenceFrame(frame["stamp_ns"], tuple(RoleCandidates(role["role"], tuple( - SquareTagPose(tuple(p["quaternion_xyzw"]), tuple(p["translation_xyz_m"]), p["reprojection_error_px"]) - for p in role["candidates"]), role["frozen"]) for role in frame["roles"])) - for frame in fixture[field] if frame["direction"] == direction] - - -def test_real_top_yaw_retry_has_unique_motion_evidence(): - frames = real_frames("decreasing", "yaw_retry_frames") - result = resolve_motion_branches(frames, (MotionRelation("yaw", "top_base", "thumb_yaw"),)) - assert result.resolved - assert len(frames) == 210 - assert sum(h.accepted for h in result.hypotheses) == 1 - quality = next(h.relations[0] for h in result.hypotheses if h.accepted) - assert quality.motion_rad > math.radians(65) - assert quality.pivot_rms_m < 0.0015 - - -@pytest.mark.parametrize("direction", ["decreasing", "increasing"]) -def test_real_capture_pitch_has_evidence_but_full_ip_chain_must_remain_unresolved(direction): - frames = real_frames(direction) - pitch = MotionRelation("pitch", "front_base", "thumb_pitch") - ip = MotionRelation("ip", "thumb_pitch", "thumb_ip") - # This is explicitly counterfactual replay of sweep candidates. Production - # must never use it to replace this real session's already frozen zero. - result = resolve_motion_branches(frames, (pitch,)) - assert result.resolved - assert sum(h.accepted for h in result.hypotheses) == 1 - assert all(h.relations[0].pivot_rms_m < 0.0015 for h in result.hypotheses if h.accepted) - joint = resolve_motion_branches(frames, (pitch, ip)) - assert not joint.resolved and joint.reason == "motion_geometry_unresolved" - assert all(h.relations[1].reason == "motion_geometry_residual" for h in joint.hypotheses) - assert joint.constraints == joint.selections == () - - -def test_real_reverse_direction_uses_first_direction_model_without_refitting(): - pitch = MotionRelation("pitch", "front_base", "thumb_pitch") - result = resolve_motion_branches(real_frames("decreasing"), (pitch,)) - assert result.resolved - accepted = [select_motion_frame(result.constraints, frame.roles).resolved for frame in real_frames("increasing")] - assert all(accepted) - - -@pytest.mark.parametrize("field,value", [("maximum_pivot_rms_m", float("nan")), - ("angular_bin_rad", 0), ("minimum_motion_bins", 11), ("minimum_motion_bins", 12.0)]) -def test_invalid_parameters_are_rejected(field, value): - with pytest.raises(ValueError): - MotionEvidenceParameters(**{field: value}) diff --git a/src/linkerhand_calibration/test/test_motion_capture_tracking.py b/src/linkerhand_calibration/test/test_motion_capture_tracking.py deleted file mode 100644 index a15d567..0000000 --- a/src/linkerhand_calibration/test/test_motion_capture_tracking.py +++ /dev/null @@ -1,221 +0,0 @@ -"""Capture uses frozen motion evidence even while recording is not allowed. - -The fixture injects explicit IPPE candidates to isolate the real tracker, -constraint selector and capture boundary. It fits its immutable constraints -from the independently articulated synthetic trajectory in the numeric tests; -these checks do not claim image or hardware accuracy. -""" - -from concurrent.futures import ThreadPoolExecutor -from dataclasses import replace -from types import SimpleNamespace - -import numpy as np -import pytest - -from linkerhand_calibration.core.geometry.tag_pose import tracking -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import ( - MotionRelation, resolve_motion_branches, -) -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime import capture as capture_module -from linkerhand_calibration.runtime.branch_initialization import MotionBranchModel -from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.reference_lock import ReferenceLock - -from test_motion_branch_evidence import motion_frames - - -STEP = 40_000_000 -START = 10_000_000_000 -ROLE_NAMES = {"base": "front_base", "parent": "thumb_pitch", "child": "thumb_ip"} -MOVING = ("thumb_pitch", "thumb_ip") -IDS = {"thumb_pitch": "a" * 64, "thumb_ip": "b" * 64} -JOINTS = ("rh_thumb_cmc_pitch", "rh_thumb_ip") -TASK = "thumb_pitch_ip_front" - - -@pytest.fixture -def rig(monkeypatch): - class Rig: - def __init__(self): - self.profile = load_bundled_hand_profile("o6_right_8") - self.frames = [replace(frame, roles=tuple( - replace(item, role=ROLE_NAMES[item.role]) for item in frame.roles)) - for frame in motion_frames()] - relations = (MotionRelation(JOINTS[0], "front_base", "thumb_pitch"), - MotionRelation(JOINTS[1], "thumb_pitch", "thumb_ip")) - resolution = resolve_motion_branches(self.frames, relations) - assert resolution.resolved - self.model = MotionBranchModel(TASK, "front", resolution.constraints, tuple(IDS.items())) - # Fixed reference image verification is independently covered by - # test_fixed_reference_pose. Here base has one current valid root. - lock = ReferenceLock({view.name: next(tag.tag_id for tag in view.tags if tag.fixed_reference) - for view in self.profile.vision.views}) - self.capture = ObservationCapture(self.profile, reference_lock=lock, - extrinsics=SimpleNamespace(transform=lambda view: np.eye(4))) - self.tracker = self.capture.trackers["front"] - self.current = {item.role: item.candidates for item in self.frames[20].roles} - self.role_by_marker = {index: role for index, role in enumerate(ROLE_NAMES.values(), start=1)} - self.corners = {role: np.array([[index, 0.], [index + .1, 0.], - [index + .1, .1], [index, .1]]) for index, role in self.role_by_marker.items()} - self.motion = MotionCommand("joint_zero", tuple(self.profile.command.baseline_values), 40, - task_key=TASK, command_index=0, cycle=0, zero_joints=JOINTS) - - def solve(self, corners, **kwargs): - return list(self.current[self.role_by_marker[int(corners[0][0])]]) - - def frame(self, counter, *, index=20, omit=(), slide=False): - self.current = {item.role: item.candidates for item in self.frames[index].roles} - if slide: - self.current["thumb_ip"] = tuple(replace(pose, - translation_xyz_m=tuple(np.asarray(pose.translation_xyz_m) + [0, 0, .01])) - for pose in self.current["thumb_ip"]) - values = tuple(map(float, self.profile.command.baseline_values)) - return CaptureFrame("front", START + counter * STEP, np.eye(3), - {role: points for role, points in self.corners.items() if role not in omit}, values, values) - - def establish(self): - observation = self.frame(10) - rows, movement = self.capture.consume(observation, self.motion, allow_recording=False) - assert movement is None and not any(row["kind"] == "joint_zero_sample" for row in rows) - revisions = {role: self.tracker.motion_snapshot(role, observation.stamp_ns).revision for role in MOVING} - assert self.tracker.freeze_roles(revisions, require_motion_evidence=True) - return revisions - - instance = Rig() - monkeypatch.setattr(tracking, "solve_square_tag_ippe", instance.solve) - for counter in range(5): - instance.capture.consume(instance.frame(counter)) - # Both incorrect candidates have decisively smaller reprojection errors. - # The legacy tracker has actually confirmed those candidates during preview. - for role in MOVING: - assert instance.tracker._branches.states[role].pose == instance.current[role][0] - instance.capture.install_motion_model(instance.model) - return instance - - -def _joint_rows(rows): - return [row for row in rows if row["kind"] in {"joint_zero_sample", "joint_sample", "secondary_joint_sample"}] - - -def test_unstable_zero_and_recording_transitions_keep_frozen_geometry_and_identity(rig, monkeypatch): - original_update = rig.tracker._branches.update - - def only_fixed_independent_update(role, *args, **kwargs): - assert role not in MOVING, "an installed motion model must not fall back to image ranking" - return original_update(role, *args, **kwargs) - - monkeypatch.setattr(rig.tracker._branches, "update", only_fixed_independent_update) - revisions = rig.establish() - for counter, allowed in enumerate((False, False, True, False, False, True), start=11): - observation = rig.frame(counter, index=counter + 10) - rows, movement = rig.capture.consume(observation, rig.motion, allow_recording=allowed) - assert movement is None - assert len(_joint_rows(rows)) == (2 if allowed else 0) - for role in MOVING: - state = rig.tracker._branches.states[role] - diagnostics = rig.tracker.observation_snapshot(role, observation.stamp_ns)[1] - assert state.pose == rig.current[role][-1] - assert state.pose.reprojection_error_px > rig.current[role][0].reprojection_error_px + .03 - assert state.revision == revisions[role] and state.frozen - assert state.motion_evidence_id == IDS[role] - assert diagnostics["pose_constraint"] == "motion_geometry" - assert diagnostics["branch_status"] == "tracking" - for row in _joint_rows(rows): - for item in row["pnp_observation_evidence"].values(): - role = item["tag_role"] - if role in MOVING: - assert item["candidate_diagnostics"]["motion_evidence_id"] == IDS[role] - assert item["candidate_diagnostics"]["branch_revision"] == revisions[role] - assert item["selected_pose"]["quaternion_xyzw"] == list(rig.current[role][-1].quaternion_xyzw) - assert rig.capture._motion_models[(TASK, "front")] == rig.model - - -def test_capture_serializes_detached_commit_evidence_after_another_candidate_producer_advances_tracker(rig, monkeypatch): - rig.establish() - observation = rig.frame(11, index=21) - original_snapshot = capture_module.snapshot_pose_evidence - advanced = False - - def snapshot_after_next_candidate_frame(tags, *args, **kwargs): - nonlocal advanced - if "thumb_pitch" in tags and not advanced: - advanced = True - - def advance_candidates(): - for role in MOVING: - rig.tracker.estimate(role, observation.corners[role], tag_size_m=.016, - camera_matrix=observation.camera_matrix, stamp_ns=observation.stamp_ns + STEP, - motion_managed=True) - - # This specifically exercises the tracker/evidence race. Full - # capture-generation invalidation has separate coordinator tests. - with ThreadPoolExecutor(max_workers=1) as executor: - executor.submit(advance_candidates).result(timeout=2) - return original_snapshot(tags, *args, **kwargs) - - monkeypatch.setattr(capture_module, "snapshot_pose_evidence", snapshot_after_next_candidate_frame) - rows, movement = rig.capture.consume(observation, rig.motion) - assert advanced and movement is None - samples = _joint_rows(rows) - assert len(samples) == 2 - for row in samples: - assert row["image_stamp_ns"] == observation.stamp_ns - for item in row["pnp_observation_evidence"].values(): - role = item["tag_role"] - if role in MOVING: - assert rig.tracker.observation_snapshot(role, observation.stamp_ns) is None - diagnostics = item["candidate_diagnostics"] - assert diagnostics["observation_stamp_ns"] == observation.stamp_ns - assert diagnostics["branch_status"] == "tracking" - assert diagnostics["motion_selection_committed"] is True - assert item["selected_pose"]["quaternion_xyzw"] == list(rig.current[role][-1].quaternion_xyzw) - - -def test_one_inconsistent_geometry_frame_is_discarded_without_refreezing_or_latching_conflict(rig): - revisions = rig.establish() - previous = {role: rig.tracker._branches.states[role].pose for role in MOVING} - rows, movement = rig.capture.consume(rig.frame(11, index=21, slide=True), rig.motion) - assert movement is None and not _joint_rows(rows) - events = [row for row in rows if row["kind"] == "pnp_branch_event"] - assert {row["tag_role"] for row in events} == set(MOVING) - assert all(row["reason"] == "pose_branch_motion_geometry_inconsistent" for row in events) - for role in MOVING: - state = rig.tracker._branches.states[role] - assert state.pose == previous[role] - assert state.revision == revisions[role] and state.motion_evidence_id == IDS[role] - assert state.frozen and not state.conflict - rows, movement = rig.capture.consume(rig.frame(12, index=22), rig.motion) - assert movement is None and len(_joint_rows(rows)) == 2 - assert rig.capture._motion_models[(TASK, "front")] == rig.model - assert all(rig.tracker._branches.states[role].revision == revisions[role] for role in MOVING) - - -def test_missing_articulated_tag_does_not_promote_the_other_moving_candidate(rig): - revisions = rig.establish() - observation = rig.frame(11, omit=("thumb_ip",)) - rows, movement = rig.capture.consume(observation, rig.motion) - assert movement is None and not _joint_rows(rows) - assert rig.capture.last_missing["front"] == (2,) - assert not rig.tracker._branches.states["thumb_pitch"].conflict - rows, movement = rig.capture.consume(rig.frame(12, index=21), rig.motion) - assert movement is None and len(_joint_rows(rows)) == 2 - assert all(rig.tracker._branches.states[role].revision == revisions[role] for role in MOVING) - - -def test_two_current_geometric_solutions_are_not_resolved_by_image_error(rig): - revisions = rig.establish() - observation = rig.frame(11, index=21) - alternative = {item.role: item.candidates[-1] for item in rig.frames[25].roles} - for role in MOVING: - # Both pairs lie on the same frozen articulated motion, so neither - # geometric consistency nor lower image error determines physical truth. - rig.current[role] = (rig.current[role][-1], replace(alternative[role], reprojection_error_px=.05)) - rows, movement = rig.capture.consume(observation, rig.motion) - assert movement is None and not _joint_rows(rows) - events = [row for row in rows if row["kind"] == "pnp_branch_event"] - assert {row["tag_role"] for row in events} == set(MOVING) - assert all(row["reason"] == "pose_branch_motion_geometry_ambiguous" for row in events) - assert all(rig.tracker._branches.states[role].revision == revisions[role] for role in MOVING) diff --git a/src/linkerhand_calibration/test/test_motion_capture_transitions.py b/src/linkerhand_calibration/test/test_motion_capture_transitions.py deleted file mode 100644 index b7c284d..0000000 --- a/src/linkerhand_calibration/test/test_motion_capture_transitions.py +++ /dev/null @@ -1,45 +0,0 @@ -"""Model identity belongs to a physical Tag, including between task streams.""" - -from dataclasses import replace - -import pytest - -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from test_motion_capture_tracking import IDS, MOVING, rig - - -@pytest.mark.parametrize("transition", ["idle", "baseline", "return", "reference_return", - "mapping_probe", "clearance", "different_task"]) -def test_installed_tag_never_returns_to_ippe_during_nonrecording_transitions(rig, monkeypatch, transition): - revisions = rig.establish() - update = rig.tracker._branches.update - - def fixed_tags_only(role, *args, **kwargs): - assert role not in MOVING, "a model-authorized Tag must never return to independent IPPE" - return update(role, *args, **kwargs) - - monkeypatch.setattr(rig.tracker._branches, "update", fixed_tags_only) - motion = None - if transition != "idle": - task = rig.profile.motion.tasks[0] if transition == "different_task" else None - motion = MotionCommand("prepare" if task else transition, - rig.profile.command.baseline_values, 40., task_key=None if task is None else task.key) - for counter in range(11, 15): - frame = rig.frame(counter, index=counter + 10) - rows, movement = rig.capture.consume(frame, motion) - assert movement is None - assert not any(row["kind"] in {"joint_sample", "joint_zero_sample", "pnp_candidate_frame"} for row in rows) - for role in MOVING: - state = rig.tracker._branches.states[role] - assert state.pose == rig.current[role][-1] - assert state.revision == revisions[role] and state.motion_evidence_id == IDS[role] - assert state.frozen and not state.conflict - - -def test_a_later_task_still_keeps_prior_model_without_recording_prior_joint(rig): - rig.establish() - other = rig.profile.motion.tasks[0] - motion = replace(rig.motion, phase="sweep", task_key=other.key, direction="decreasing", zero_joints=()) - rows, _ = rig.capture.consume(rig.frame(11, index=21), motion) - assert not any(row.get("joint") in {"rh_thumb_cmc_pitch", "rh_thumb_ip"} for row in rows) - assert all(rig.tracker._branches.states[role].pose == rig.current[role][-1] for role in MOVING) diff --git a/src/linkerhand_calibration/test/test_motion_confirmed_tracking.py b/src/linkerhand_calibration/test/test_motion_confirmed_tracking.py deleted file mode 100644 index a790691..0000000 --- a/src/linkerhand_calibration/test/test_motion_confirmed_tracking.py +++ /dev/null @@ -1,273 +0,0 @@ -"""Geometric branch authorization must remain tied to one image and reference. - -These tests exercise the metadata contract with synthetic geometric decisions; -physical motion-model selection has its own numeric tests. -""" - -from dataclasses import FrozenInstanceError, replace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose import tracking -from linkerhand_calibration.core.geometry.tag_pose.tracking import ( - MotionPoseSelection, SquareTagPoseTracker, -) -from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - - -START = 1_000_000_000 -STEP = 33_000_000 -EVIDENCE = "a" * 64 - - -def pose(angle=0., error=.05, translation=(0., 0., .5)): - return SquareTagPose(tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), - translation, error) - - -@pytest.fixture -def source(monkeypatch): - class Source: - candidates = (pose(0., .20), pose(60., .05)) - - def observe(self, tracker, stamp, role="moving", managed=True): - result = tracker.estimate(role, ((0, 0), (1, 0), (1, 1), (0, 1)), - tag_size_m=.016, camera_matrix=np.eye(3), stamp_ns=stamp, - motion_managed=managed) - return result, tracker.motion_snapshot(role, stamp) - - instance = Source() - monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: list(instance.candidates)) - return instance - - -def selection(source, tracker, stamp, *, role="moving", candidate=0, evidence=EVIDENCE): - result, snapshot = source.observe(tracker, stamp, role) - assert result[0] is None - return MotionPoseSelection(snapshot, source.candidates[candidate], evidence) - - -def establish(source, tracker, stamp=START, *, role="moving"): - choice = selection(source, tracker, stamp, role=role) - result = tracker.commit_motion_frame((choice,), establish=True) - assert result.accepted - return tracker.motion_snapshot(role, stamp) - - -def test_motion_managed_images_never_confirm_an_image_winner_without_geometry(source): - tracker = SquareTagPoseTracker() - for i in range(20): - outcome, snapshot = source.observe(tracker, START + i * STEP) - assert outcome == (None, "pose_motion_evidence_required") - assert snapshot.revision == 0 and snapshot.evidence_id is None - assert not tracker.freeze_roles({"moving": 0}, require_motion_evidence=True) - - -def test_establishment_authorizes_current_candidate_without_increasing_point03_tolerance(source): - tracker = SquareTagPoseTracker() - choice = selection(source, tracker, START) - assert not tracker.commit_motion_frame((choice,)).accepted - # The failed authorization request consumed that decision's frame. - choice = selection(source, tracker, START + STEP) - result = tracker.commit_motion_frame((choice,), establish=True) - assert result.accepted and result.selections[0].pose == source.candidates[0] - detail = result.observation_snapshots[0].diagnostics - assert detail["motion_evidence_verified"] is True - assert detail["motion_evidence_id"] == EVIDENCE - assert detail["branch_revision"] == 1 - assert detail["branch_status"] == "tracking" - assert tracker.reprojection_tie_px == .03 - assert tracker.freeze_roles({"moving": 1}, require_motion_evidence=True) - - -def test_geometry_correction_is_explicit_and_only_before_freeze(source): - tracker = SquareTagPoseTracker() - for i in range(5): - result, _ = source.observe(tracker, START + i * STEP, managed=False) - assert result[0] == source.candidates[1] - old_revision = tracker.motion_snapshot("moving", START + 4 * STEP).revision - choice = selection(source, tracker, START + 5 * STEP) - result = tracker.commit_motion_frame((choice,), establish=True) - assert result.accepted - assert result.observation_snapshots[0].diagnostics["branch_revision"] == old_revision + 1 - assert result.observation_snapshots[0].corrections == 1 - assert not tracker.freeze_roles({"moving": old_revision}, require_motion_evidence=True) - - -def test_existing_image_only_frozen_reference_cannot_be_reauthorized(source): - tracker = SquareTagPoseTracker() - for i in range(5): - source.observe(tracker, START + i * STEP, managed=False) - assert not tracker.freeze_roles({"moving": 1}, require_motion_evidence=True) - assert tracker.freeze_roles({"moving": 1}) - choice = selection(source, tracker, START + 5 * STEP) - assert not tracker.commit_motion_frame((choice,), establish=True).accepted - assert tracker.motion_snapshot("moving", START + 5 * STEP).evidence_id is None - - -def test_frozen_geometry_keeps_current_candidate_when_image_winner_changes(source): - tracker = SquareTagPoseTracker() - initial = establish(source, tracker) - assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) - for i in range(1, 190): - source.candidates = (pose(.001 * i, .20), pose(60., .05)) - result = tracker.commit_motion_frame((selection(source, tracker, START + i * STEP),)) - assert result.accepted - detail = result.observation_snapshots[0].diagnostics - assert detail["branch_revision"] == initial.revision - assert detail["branch_frozen"] is True - assert result.observation_snapshots[0].corrections == 0 - - -@pytest.mark.parametrize("candidate", [pose(0., 1.50001), pose(76., .05)]) -def test_geometry_cannot_bypass_reprojection_or_tilt_gate(source, candidate): - tracker = SquareTagPoseTracker() - source.candidates = (candidate,) - choice = selection(source, tracker, START) - assert choice.snapshot.candidates == () - result = tracker.commit_motion_frame((choice,), establish=True) - assert not result.accepted and result.reason == "pose_motion_selection_invalid" - - -def test_geometry_cannot_insert_an_unobserved_pose(source): - tracker = SquareTagPoseTracker() - choice = selection(source, tracker, START) - result = tracker.commit_motion_frame((replace(choice, pose=pose(1.)),), establish=True) - assert not result.accepted and result.reason == "pose_motion_selection_invalid" - - -def test_frozen_role_cannot_change_evidence_id_or_jump_branch(source): - tracker = SquareTagPoseTracker() - initial = establish(source, tracker) - assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) - changed = selection(source, tracker, START + STEP, evidence="b" * 64) - assert tracker.commit_motion_frame((changed,), establish=True).reason == "pose_motion_evidence_changed" - discontinuous = selection(source, tracker, START + 2 * STEP, candidate=1) - assert tracker.commit_motion_frame((discontinuous,)).reason == "pose_motion_branch_discontinuous" - snapshot = tracker.motion_snapshot("moving", START + 2 * STEP) - assert snapshot.evidence_id == EVIDENCE and snapshot.revision == initial.revision - result = tracker.commit_motion_frame((selection(source, tracker, START + 3 * STEP),)) - assert result.accepted - - -@pytest.mark.parametrize("interruption", ["reset", "new_image", "freeze"]) -def test_late_geometry_result_cannot_commit_after_reference_or_image_changes(source, interruption): - tracker = SquareTagPoseTracker() - initial = establish(source, tracker) - choice = selection(source, tracker, START + STEP) - if interruption == "reset": - tracker.reset() - elif interruption == "new_image": - source.observe(tracker, START + 2 * STEP) - else: - # Freeze needs a committed frame. This simulates a different authorized - # consumer committing and then freezing while an old solve was running. - assert tracker.commit_motion_frame((choice,)).accepted - assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) - result = tracker.commit_motion_frame((choice,)) - assert not result.accepted and result.reason == "pose_observation_superseded" - - -def test_partial_geometry_batch_never_establishes_only_one_role(source): - tracker = SquareTagPoseTracker() - first = selection(source, tracker, START, role="parent") - second = selection(source, tracker, START, role="child") - invalid = replace(second, pose=pose(2.)) - assert not tracker.commit_motion_frame((first, invalid), establish=True).accepted - assert tracker.motion_snapshot("parent", START).evidence_id is None - assert tracker.motion_snapshot("child", START).evidence_id is None - assert tracker.commit_motion_frame((first, second), establish=True).accepted - - -def test_batch_checks_all_versions_before_any_mutation(source): - first, second = SquareTagPoseTracker(), SquareTagPoseTracker() - choices = (selection(source, first, START, role="parent"), - selection(source, second, START, role="child")) - second.reset() - assert not first.commit_motion_frame(choices, establish=True).accepted - assert first.motion_snapshot("parent", START).evidence_id is None - - -def test_real_time_gap_retains_frozen_recovery_and_unique_image_requirement(source): - tracker = SquareTagPoseTracker() - initial = establish(source, tracker) - assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) - source.candidates = (pose(2., .20), pose(60., .05)) - stamp = START + 6_000_000_000 - result = tracker.commit_motion_frame((selection(source, tracker, stamp),)) - assert result.reason == "pose_branch_recovery_required" - source.candidates = (pose(.2, .20), pose(60., .05)) - for i in range(1, 6): - choice = selection(source, tracker, stamp + i * STEP) - result = tracker.commit_motion_frame((choice,)) - assert result.accepted is (i == 5) - repeated = tracker.commit_motion_frame((choice,)) - assert not repeated.accepted and repeated.reason == "pose_observation_superseded" - assert tracker.motion_snapshot("moving", stamp + 5 * STEP).revision == initial.revision - - -def test_commit_returns_detached_pose_and_diagnostics_for_exact_image(source): - tracker = SquareTagPoseTracker() - result = tracker.commit_motion_frame((selection(source, tracker, START),), establish=True) - old = result.observation_snapshots[0] - source.observe(tracker, START + STEP) - assert tracker.observation_snapshot("moving", START) is None - assert old.stamp_ns == START - assert old.diagnostics["observation_stamp_ns"] == START - assert old.diagnostics["branch_status"] == "tracking" - assert result.selections[0].pose == source.candidates[0] - with pytest.raises(FrozenInstanceError): - result.selections[0].snapshot.revision = 99 - - -def test_rejected_geometric_frame_reports_reason_and_preserves_reference(source): - tracker = SquareTagPoseTracker() - initial = establish(source, tracker) - assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) - choice = selection(source, tracker, START + STEP) - reason = "pose_branch_motion_geometry_ambiguous" - result = tracker.reject_motion_frame((choice.snapshot,), reason) - assert not result.accepted and result.reason == reason - detail = result.observation_snapshots[0].diagnostics - assert detail["branch_reason"] == reason - assert detail["branch_revision"] == initial.revision - assert detail["motion_evidence_id"] == EVIDENCE - assert detail["branch_frozen"] is True - assert tracker.commit_motion_frame((choice,)).reason == "pose_observation_superseded" - next_choice = selection(source, tracker, START + 2 * STEP) - assert tracker.commit_motion_frame((next_choice,)).accepted - - -def test_late_geometric_rejection_cannot_modify_a_newer_frame(source): - tracker = SquareTagPoseTracker() - establish(source, tracker) - old = selection(source, tracker, START + STEP) - current = selection(source, tracker, START + 2 * STEP) - result = tracker.reject_motion_frame((old.snapshot,), "pose_branch_motion_geometry_ambiguous") - assert result.reason == "pose_observation_superseded" - assert tracker.commit_motion_frame((current,)).accepted - - -def test_rejection_batch_is_atomic_when_another_view_has_reset(source): - first, second = SquareTagPoseTracker(), SquareTagPoseTracker() - snapshots = (selection(source, first, START).snapshot, selection(source, second, START).snapshot) - second.reset() - result = first.reject_motion_frame(snapshots, "pose_branch_motion_geometry_inconsistent") - assert result.reason == "pose_observation_superseded" - assert first.observation_snapshot("moving", START)[1]["branch_reason"] == "pose_motion_evidence_required" - - -def test_geometric_rejection_cannot_hide_a_preexisting_frozen_conflict(source): - tracker = SquareTagPoseTracker() - initial = establish(source, tracker) - assert tracker.freeze_roles({"moving": initial.revision}, require_motion_evidence=True) - source.candidates = (pose(60., .05), pose(70., .2)) - for i in range(1, 6): - source.observe(tracker, START + i * STEP, managed=False) - assert tracker._branches.states["moving"].conflict - choice = selection(source, tracker, START + 6 * STEP) - result = tracker.reject_motion_frame((choice.snapshot,), "pose_branch_motion_geometry_inconsistent") - assert result.reason == "pose_branch_conflict" - assert result.observation_snapshots[0].diagnostics["branch_status"] == "frozen_conflict" diff --git a/src/linkerhand_calibration/test/test_motion_image_diagnostics.py b/src/linkerhand_calibration/test/test_motion_image_diagnostics.py deleted file mode 100644 index 039db3d..0000000 --- a/src/linkerhand_calibration/test/test_motion_image_diagnostics.py +++ /dev/null @@ -1,169 +0,0 @@ -"""Image-space diagnosis stays separate from trusted branch/zero decisions.""" -from dataclasses import asdict, replace -import json - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -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, resolve_motion_branches, -) -from linkerhand_calibration.core.geometry.tag_pose.motion_image_diagnostics import ( - ImageMotionFrame, ImageRoleObservation, MotionImageDiagnosticParameters, analyze_motion_images, -) -from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - -MATRIX = ((1600., 0., 640.), (0., 1600., 480.), (0., 0., 1.)) -RELATIONS = (MotionRelation("hinge", "root", "moving"),) - - -def _pose(rotation, translation): - return SquareTagPose(tuple(rotation.as_quat()), tuple(translation), .1) - - -def frames(*, chain=False, alternatives=False, depth_error=0., count=49): - result = [] - for index, theta in enumerate(np.linspace(0, np.deg2rad(80), count)): - root_rotation, root_translation = Rotation.identity(), np.array([0., 0., .8]) - rotation = Rotation.from_rotvec([0., 0., theta]) - translation = root_translation + [0.01, 0.02, 0.0] + rotation.apply([.035, 0., 0.]) - actual = [("root", root_rotation, root_translation), ("moving", rotation, translation)] - if chain: - child_rotation = rotation * Rotation.from_rotvec([theta * .45, 0., 0.]) - child_translation = translation + rotation.apply([.025, 0, 0]) + child_rotation.apply([0., .035, 0.]) - actual.append(("terminal", child_rotation, child_translation)) - roles, observations = [], [] - for role, rotation, translation in actual: - points = rotation.apply(square_object_points(.016)) + translation - pixels = points @ np.asarray(MATRIX).T - corners = pixels[:, :2] / pixels[:, 2:] - observations.append(ImageRoleObservation(role, .016, tuple(map(tuple, corners)))) - error = depth_error * np.sin(theta * 5) if role != "root" else 0. - pose = _pose(rotation, translation + [0., 0., error]) - candidates = (pose,) - if alternatives and role != "root": - wrong = _pose(Rotation.from_euler("y", 25, degrees=True) * rotation, - translation + [0., 0., error]) - candidates = (pose, wrong) - roles.append(RoleCandidates(role, candidates, frozen=role == "root")) - result.append(ImageMotionFrame(MotionEvidenceFrame(1_000_000_000 + index * 50_000_000, - tuple(roles)), MATRIX, tuple(observations))) - return tuple(result) - - -def test_image_fit_explains_noisy_depth_without_upgrading_branch_authorization(): - source = frames(depth_error=.008) - before = resolve_motion_branches([f.evidence for f in source], RELATIONS) - assert not before.resolved - report = analyze_motion_images(source, RELATIONS) - assert report.status == "available", report - assert report.scope == "diagnostic_not_branch_authorization" - assert not report.branch_authorization_allowed and not report.calibration_accuracy_verified - result = report.hypotheses[0] - assert result.pnp_geometry[0].translation_rms_m > .003 - assert result.pnp_geometry[0].camera_depth_squared_error_fraction > .99 - assert max(q.rms_px for interval in result.intervals for q in interval.validation_quality) < 1e-5 - assert resolve_motion_branches([f.evidence for f in source], RELATIONS) == before - json.dumps(asdict(report), allow_nan=False) - - -def test_unseen_shift_cannot_be_hidden_by_refitting_validation_geometry(): - source = list(frames()) - for index in range(len(source) // 2, len(source)): - observations = list(source[index].observations) - observations[1] = replace(observations[1], corners_xy=tuple( - (x + 8., y) for x, y in observations[1].corners_xy)) - source[index] = replace(source[index], observations=tuple(observations)) - report = analyze_motion_images(source, RELATIONS) - interval = next(v for v in report.hypotheses[0].intervals if v.name == "early_to_late") - assert interval.converged, interval - assert interval.training_quality[0].rms_px < 1e-5 - assert interval.validation_quality[0].rms_px > 2 - assert set(interval.training_stamps_ns).isdisjoint(interval.validation_stamps_ns) - - -def test_serial_graph_and_all_candidate_families_survive_as_diagnostics(): - source = frames(chain=True, alternatives=True) - relations = (*RELATIONS, MotionRelation("distal", "moving", "terminal")) - report = analyze_motion_images(source, tuple(reversed(relations))) - assert len(report.hypotheses) == 4 - assert all(dict(h.branches)["root"] == 0 for h in report.hypotheses) - assert any(h.intervals and h.intervals[0].converged for h in report.hypotheses) - assert not report.branch_authorization_allowed - for hypothesis in report.hypotheses: - for interval in hypothesis.intervals: - assert set(interval.training_stamps_ns).isdisjoint(interval.validation_stamps_ns) - assert len(interval.training_stamps_ns) <= 36 - assert len(interval.validation_stamps_ns) <= 72 - - -@pytest.mark.parametrize("change, reason", [ - ("missing", "image_observation_missing_or_duplicate"), - ("size", "tag_size_changed"), - ("matrix", "camera_model_changed"), - ("identity", "image_identity_invalid"), - ("unfrozen", "root_pose_not_frozen"), - ("nan", "tag_geometry_invalid"), -]) -def test_incomplete_or_changed_optics_are_explicitly_unavailable(change, reason): - source = list(frames()) - target = source[-1] - if change == "missing": - target = replace(target, observations=target.observations[:1]) - elif change == "size": - target = replace(target, observations=(target.observations[0], - replace(target.observations[1], tag_size_m=.017))) - elif change == "matrix": - target = replace(target, camera_matrix=((1500., 0., 640.), *MATRIX[1:])) - elif change == "identity": - target = replace(target, evidence=replace(target.evidence, stamp_ns=source[0].evidence.stamp_ns)) - elif change == "unfrozen": - target = replace(target, evidence=replace(target.evidence, roles=( - replace(target.evidence.roles[0], frozen=False), *target.evidence.roles[1:]))) - elif change == "nan": - target = replace(target, observations=(target.observations[0], replace(target.observations[1], - corners_xy=((float("nan"), 1.), *target.observations[1].corners_xy[1:])))) - source[-1] = target - report = analyze_motion_images(source, RELATIONS) - assert report.status == "not_available" and report.reason == reason - assert not report.hypotheses - - -def test_graph_and_solver_complexity_are_bounded(): - source = frames() - cyclic = (MotionRelation("a", "root", "moving"), MotionRelation("b", "moving", "root")) - assert analyze_motion_images(source, cyclic).reason == "cyclic_motion_graph" - params = MotionImageDiagnosticParameters(maximum_solver_evaluations=1) - report = analyze_motion_images(frames(depth_error=.003), RELATIONS, parameters=params) - assert report.status == "partial" - assert all(i.solver_evaluations <= 1 for i in report.hypotheses[0].intervals) - for kwargs in ({"maximum_hypotheses": 100}, {"maximum_solver_evaluations": 151}, - {"minimum_frames": 3}, {"maximum_relations": 10}): - with pytest.raises(ValueError): - MotionImageDiagnosticParameters(**kwargs) - - -def test_stationary_data_is_not_evidence_of_a_unique_image_branch(): - source = frames() - still = tuple(replace(source[0], evidence=replace(source[0].evidence, - stamp_ns=f.evidence.stamp_ns)) for f in source) - report = analyze_motion_images(still, RELATIONS) - assert report.status == "not_available" - assert report.hypotheses[0].reason == "insufficient_rotation_evidence" - - -def test_previously_frozen_moving_parent_uses_its_current_frame_pose(): - source = frames(chain=True) - changed = [] - for frame in source: - roles = tuple(replace(role, frozen=True) if role.role == "moving" else role - for role in frame.evidence.roles) - changed.append(replace(frame, evidence=replace(frame.evidence, roles=roles))) - report = analyze_motion_images(changed, (MotionRelation("distal", "moving", "terminal"),)) - assert report.status == "available", report - for interval in report.hypotheses[0].intervals: - assert interval.converged - assert interval.validation_quality[0].rms_px < 1e-5 - assert not report.branch_authorization_allowed diff --git a/src/linkerhand_calibration/test/test_motion_reference_provenance.py b/src/linkerhand_calibration/test/test_motion_reference_provenance.py deleted file mode 100644 index cdbdb87..0000000 --- a/src/linkerhand_calibration/test/test_motion_reference_provenance.py +++ /dev/null @@ -1,277 +0,0 @@ -"""Motion authorization survives zero serialization, holdout and resume.""" - -from copy import deepcopy -from dataclasses import replace -import hashlib -import json - -import pytest - -from linkerhand_calibration.core.domain.reference import build_joint_zero_reference, read_joint_zero_references -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.joint_resume import JointResume -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256, validate_motion_provenance -from test_joint_zero_runtime import reference_rows - - -def motion_capture(*, zero_stamp=100, revision=7, source_stamps=(10, 20), layout="o6_right_8", task_index=0): - profile = load_bundled_hand_profile(layout) - task = profile.motion.tasks[task_index] - name = task.joints[0] - profile = replace(profile, motion=replace(profile.motion, tasks=(task,))) - measurement = profile.measurement.measurements[name] - role = measurement.child_role - payload = {"task_name": task.key, "view": measurement.view, "tag_role": role, - "zero_joints": [name], "session_epoch": 1, "motion_version": 1, - "source_image_stamps": list(source_stamps), "constraints": [{"child_role": role, "axis": [1., 0., 0.]}]} - sources = [{"kind": "motion_branch_observation", "image_stamp_ns": stamp, - "command_vector": list(profile.command.baseline_values), "feedback_vector": list(profile.command.baseline_values), - "tags": {role: {"corners_xy": [[1., 1.], [2., 1.], [2., 2.], [1., 2.]], - "reprojection_valid_candidates": [{"quaternion_xyzw": [0., 0., 0., 1.], - "translation_xyz_m": [0., 0., .5]}]}}, - **{key: payload[key] for key in ("task_name", "view", "zero_joints", "session_epoch", "motion_version")}} - for stamp in source_stamps] - payload["source_frame_hashes"] = {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in sources} - identity = hashlib.sha256(json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() - audit = {"kind": "motion_branch_initialization", "status": "resolved", "reason": "", - **{key: value for key, value in payload.items() if key != "tag_role"}, - "evidence_ids": {role: identity}, "evidence_payloads": {role: payload}} - zeros = reference_rows(profile, name, stamp=zero_stamp) - for row in zeros: - row.update(kind="joint_zero_sample", joint=name) - row["pnp_observation_evidence"]["child"]["candidate_diagnostics"].update( - branch_revision=revision, motion_evidence_verified=True, motion_evidence_id=identity, branch_frozen=False) - reference = build_joint_zero_reference(profile, name, zeros, session_epoch=1, motion_version=2) - scans = [] - for cycle in range(4): - row = deepcopy(zeros[-1]) - row.update(kind="joint_sample", sample_phase="sweep", cycle=cycle, image_stamp_ns=zero_stamp+20+cycle) - for item in row["pnp_observation_evidence"].values(): - item["candidate_diagnostics"].update(observation_stamp_ns=row["image_stamp_ns"], branch_frozen=True) - scans.append(row) - return profile, reference, [*sources, audit, *zeros, reference.as_record(), *scans] - - -@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) -def test_motion_zero_roundtrip_and_all_four_cycles_remain_bound_without_header(layout): - profile, reference, records = motion_capture(layout=layout) - restored = read_joint_zero_references(profile, records)[reference.joint] - assert restored == reference - assert len(restored.branch_references) == 1 - report = validate_motion_provenance(profile, records, require_complete=True) - assert report["motion_evidence_verified"] is True - assert report["is_accuracy_certificate"] is False - - -@pytest.mark.parametrize("mutation", ["missing_audit", "missing_source", "model_tampered", "sample_id", - "holdout_revision", "holdout_not_frozen", "zero_provenance_removed", "false_verification"]) -def test_missing_or_changed_provenance_is_rejected_even_without_policy_header(mutation): - profile, reference, records = motion_capture() - if mutation == "missing_audit": - records = [row for row in records if row["kind"] != "motion_branch_initialization"] - elif mutation == "missing_source": - records.pop(0) - elif mutation == "model_tampered": - audit = next(row for row in records if row["kind"] == "motion_branch_initialization") - next(iter(audit["evidence_payloads"].values()))["constraints"][0]["axis"] = [0., 1., 0.] - elif mutation == "zero_provenance_removed": - next(row for row in records if row["kind"] == "joint_zero_reference").pop("branch_references") - else: - diagnostics = records[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"] - field, value = {"sample_id": ("motion_evidence_id", "a"*64), - "holdout_revision": ("branch_revision", 8), "holdout_not_frozen": ("branch_frozen", False), - "false_verification": ("motion_evidence_verified", False)}[mutation] - diagnostics[field] = value - with pytest.raises(ValueError, match="motion_branch_provenance_"): - validate_motion_provenance(profile, records, require_complete=True) - - -def test_motion_support_must_precede_first_frozen_zero_image(): - profile, _, records = motion_capture(source_stamps=(100, 101)) - with pytest.raises(ValueError, match="initialization_not_before_zero"): - validate_motion_provenance(profile, records) - - -@pytest.mark.parametrize("field", ["candidate", "corners", "command_vector", "feedback_vector"]) -def test_source_frame_content_cannot_change_without_invalidating_motion_evidence(field): - profile, _, records = motion_capture() - frame = records[0] - if field in {"command_vector", "feedback_vector"}: - frame[field][0] += 1 - else: - tag = next(iter(frame["tags"].values())) - if field == "candidate": - tag["reprojection_valid_candidates"][0]["translation_xyz_m"][0] += .01 - else: - tag["corners_xy"][0][0] += 1 - with pytest.raises(ValueError, match="source_content_changed"): - validate_motion_provenance(profile, records) - - -@pytest.mark.parametrize("mutation", ["missing", "extra"]) -def test_source_hashes_must_cover_exactly_the_declared_support_frames(mutation): - profile, _, records = motion_capture() - audit = next(row for row in records if row["kind"] == "motion_branch_initialization") - # Mutate only the top-level declaration so the immutable payload still - # independently detects its binding; it cannot authorize a subset instead. - audit["source_frame_hashes"] = dict(audit["source_frame_hashes"]) - if mutation == "missing": - audit["source_frame_hashes"].pop("10") - else: - audit["source_frame_hashes"]["999"] = "a"*64 - with pytest.raises(ValueError, match="initialization_binding_changed|source_hash_coverage_invalid"): - validate_motion_provenance(profile, records) - - -def test_zero_cannot_mix_different_authorizations_across_reference_images(): - profile, reference, records = motion_capture() - rows = [row for row in records if row["kind"] == "joint_zero_sample"] - rows[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"]["motion_evidence_id"] = "a"*64 - with pytest.raises(ValueError, match="joint_zero_motion_evidence_changed"): - build_joint_zero_reference(profile, reference.joint, rows, session_epoch=1, motion_version=2) - - -def test_legacy_zero_remains_readable_without_claiming_motion_confirmation(): - profile, reference, _ = motion_capture() - legacy = build_joint_zero_reference(profile, reference.joint, - reference_rows(profile, reference.joint, stamp=100), session_epoch=1, motion_version=2) - row = legacy.as_record() - row.pop("branch_references") - assert read_joint_zero_references(profile, [row])[reference.joint].branch_references == () - assert validate_motion_provenance(profile, [row])["motion_evidence_verified"] is False - with pytest.raises(ValueError, match="zero_evidence_missing"): - validate_motion_provenance(profile, [row], required=True) - with pytest.raises(ValueError, match="zero_evidence_missing"): - validate_motion_provenance(profile, [{"kind": "session_start", - "pose_tracking_policy_version": "confirmed_branch_v4_motion_geometry"}, row]) - - -def test_legacy_branch_events_are_diagnostics_and_new_events_cannot_downgrade(): - profile, _, _ = motion_capture() - event = {"kind": "pnp_branch_event", "pnp_observation_evidence": { - "tag_role": "thumb_yaw", "corners_xy": [[0, 0]]*4, - "candidate_diagnostics": {"branch_frozen": True}}} - assert validate_motion_provenance(profile, [event])["motion_evidence_verified"] is False - event["pnp_observation_evidence"]["candidate_diagnostics"]["motion_evidence_verified"] = True - with pytest.raises(ValueError, match="joint_zero_references_missing"): - validate_motion_provenance(profile, [event]) - - -def test_motion_confirmation_does_not_require_fixed_tag_motion_evidence(): - profile, _, records = motion_capture() - for row in records: - for side, item in row.get("pnp_observation_evidence", {}).items(): - if side == "parent": - item["candidate_diagnostics"].update(motion_evidence_verified=False, motion_evidence_id="") - assert validate_motion_provenance(profile, records, require_complete=True)["motion_evidence_verified"] is True - - -def test_repeated_physical_tag_keeps_one_original_evidence_payload(): - profile, _, records = motion_capture() - first = next(row for row in records if row["kind"] == "motion_branch_initialization") - repeated = {**first, "task_name": "later_task", "source_image_stamps": [500], - "evidence_payloads": {}} - source = {**records[0], "task_name": "later_task", "image_stamp_ns": 500} - repeated["source_frame_hashes"] = {"500": source_frame_sha256(source)} - records.append(source) - records.append(repeated) - assert validate_motion_provenance(profile, records)["evidence_count"] == 1 - - -def test_resume_rechecks_pose_and_preserves_old_zero_with_distinct_new_evidence_ids(): - profile, previous, original = motion_capture() - _, current, fresh = motion_capture(zero_stamp=300, revision=9, source_stamps=(210, 220)) - resume = JointResume(profile) - resume.references = {previous.joint: previous} - resume.rows = tuple(original) - assert resume.verify(current) is previous - authorized = resume.take_motion_provenance_records() - assert any(row["kind"] == "joint_zero_motion_reference_verified" for row in authorized) - assert resume.take_motion_provenance_records() == () - merged = [row for row in fresh if row["kind"] != "joint_zero_reference"] - merged.extend(authorized) - merged.append(previous.as_record()) - assert validate_motion_provenance(profile, merged, require_complete=True)["motion_evidence_verified"] is True - mapping = next(row for row in merged if row["kind"] == "joint_zero_motion_reference_verified") - # Swapping an unrelated held pose cannot authorize reuse merely because IDs exist. - mapping["current_reference"]["samples"][0]["parent_pose_common"]["translation_xyz_m"] = (1., 0., .5) - with pytest.raises(ValueError): - validate_motion_provenance(profile, merged) - - -def test_resume_imports_source_evidence_once_but_keeps_each_joint_verification(): - # G20 first two tasks each observe one independent motion role. This test - # checks journal delivery across joints, not the geometry solver itself. - profile = load_bundled_hand_profile("g20_right_19") - _, first, first_rows = motion_capture(layout="g20_right_19") - _, second, second_rows = motion_capture(layout="g20_right_19", task_index=1, - zero_stamp=200, source_stamps=(110, 120)) - resume = JointResume(profile) - resume.references = {first.joint: first, second.joint: second} - resume.rows = tuple([*first_rows, *second_rows]) - assert resume.verify(first) is first - first_batch = resume.take_motion_provenance_records() - assert any(row["kind"] == "motion_branch_observation" for row in first_batch) - assert resume.verify(second) is second - second_batch = resume.take_motion_provenance_records() - assert all(row["kind"] in {"joint_zero_sample", "joint_zero_motion_reference_verified"} for row in second_batch) - assert all(row["joint"] == second.joint for row in second_batch) - assert sum(row["kind"] == "joint_zero_motion_reference_verified" for row in second_batch) == 1 - assert resume.verify(first) is first - assert resume.take_motion_provenance_records() == () - # Failed group freezing may defer draining; retry replaces the pending - # verification and still does not copy the checkpoint sources again. - later = replace(second, motion_version=second.motion_version+1) - resume.verify(later) - resume.verify(later) - final_batch = resume.take_motion_provenance_records() - assert len(final_batch) == 1 and final_batch[0]["joint"] == second.joint - - -def test_partial_resume_of_an_already_resumed_session_keeps_unverified_joint_mappings_out(): - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine - - profile = load_bundled_hand_profile("g20_right_19") - _, first, first_rows = motion_capture(layout="g20_right_19") - _, second, second_rows = motion_capture(layout="g20_right_19", task_index=3, - zero_stamp=200, source_stamps=(110, 120)) - original = {first.joint: first, second.joint: second} - previous = JointResume(profile) - previous.references, previous.rows = original, tuple([*first_rows, *second_rows]) - previous_session = [] - for index, stamp in ((0, 1000), (3, 1200)): - _, current, fresh = motion_capture(layout="g20_right_19", task_index=index, - zero_stamp=stamp, source_stamps=(stamp-100, stamp-90)) - previous.verify(current) - previous_session.extend(row for row in fresh if row["kind"] != "joint_zero_reference") - previous_session.extend(previous.take_motion_provenance_records()) - previous_session.append(original[current.joint].as_record()) - assert validate_motion_provenance(profile, previous_session, required=True)["motion_evidence_verified"] is True - header = {"kind": "session_start", "profile_id": profile.key.profile_id, - "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "capture_schedule_version": CalibrationEngine(profile).capture_schedule_version} - resuming = JointResume(profile) - resuming.stage(CalibrationEngine(profile), [header, *previous_session], set()) - _, current, fresh = motion_capture(layout="g20_right_19", zero_stamp=2000, source_stamps=(1900, 1910)) - preserved = resuming.verify(current) - partial = [header, *(row for row in fresh if row["kind"] != "joint_zero_reference"), - *resuming.take_motion_provenance_records(), preserved.as_record()] - assert all(row.get("joint") == first.joint for row in partial - if row.get("kind") in {"joint_zero_sample", "joint_zero_motion_reference_verified"}) - assert validate_motion_provenance(profile, partial, required=True)["joint_count"] == 1 - # An abort at this point must leave a valid next checkpoint. It must not - # require the second joint, whose installation has not been checked yet. - subsequent = JointResume(profile) - subsequent.stage(CalibrationEngine(profile), partial, set()) - assert set(subsequent.references) == {first.joint} - - -def test_motion_gate_requires_provenance_outside_articulated_candidate_selection(): - _, _, records = motion_capture() - profile = load_bundled_hand_profile("o6_right_8") - records = [row for row in records if row["kind"] != "motion_branch_initialization"] - profile = replace(profile, measurement=replace(profile.measurement, candidate_selection_tasks=frozenset())) - with pytest.raises(ValueError, match="motion_branch_provenance_"): - validate_motion_provenance(profile, records, required=True, require_complete=True) diff --git a/src/linkerhand_calibration/test/test_motion_transfer.py b/src/linkerhand_calibration/test/test_motion_transfer.py deleted file mode 100644 index 65d6894..0000000 --- a/src/linkerhand_calibration/test/test_motion_transfer.py +++ /dev/null @@ -1,315 +0,0 @@ -"""Declared finger transfers preserve frozen motion and recipient CAD geometry. - -These synthetic frozen results exercise integration contracts, not hardware -accuracy or the acquisition/optimization stages covered by capture tests. -""" - -from dataclasses import replace -import hashlib -from pathlib import Path -import xml.etree.ElementTree as ET - -import numpy as np -import pytest - -from measured_capture_fixture import reordered_profile -from linkerhand_calibration.core.domain.measurement import JointCurveFit -from linkerhand_calibration.core.domain.profile import KnownBaselineGeometry, ProfileValidationError -from linkerhand_calibration.core.domain.reference import JointZeroReference -from linkerhand_calibration.core.domain.result import CalibrationResult, JointMapping, evaluate_mappings -from linkerhand_calibration.core.fitting.coupling import LinearMimicFit -from linkerhand_calibration.core.fitting.mimic_approximation import MimicApproximation -from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint -from linkerhand_calibration.core.fitting.motion_transfer import expand_profile_transfers -from linkerhand_calibration.core.fitting.session import fit_profile_calibration -from linkerhand_calibration.core.fitting.spatial import ZeroSolveResult -from linkerhand_calibration.core.urdf.result_plan import prepare_standard_result -from linkerhand_calibration.profiles.loader import load_bundled_hand_profile - - -PACKAGE = Path(__file__).resolve().parents[1] -O6_SOURCE = PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf" -O12_SOURCE = PACKAGE / "urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" - - -def _digest(path): - return hashlib.sha256(path.read_bytes()).hexdigest() - - -def _geometry_only_o6_profile(): - """Keep default CAD safety bounds for metadata-only structure fixtures. - - The production nominal-envelope policy needs actual training/reference - evidence. Full measured O6 capture tests cover that separate contract. - """ - return replace(load_bundled_hand_profile("o6_right_8"), urdf_limit_policies={}) - - -def _frozen_fit(profile): - observed = {name for name, spec in profile.measurement.measurements.items() - if spec.kind != "validation_only" and name in profile.motion.joint_zero_references} - outputs, commands, curves, references = {}, {}, {}, {} - for name in sorted(observed): - channel = channel_for_joint(profile, name) - lo, hi = profile.command.minimum_values[channel], profile.command.maximum_values[channel] - baseline = profile.command.baseline_values[channel] - knots = np.arange(256.) if profile.command.unit == "u8" else np.unique([lo, (lo+hi)/2, baseline, hi]) - travel = (knots-baseline)/(hi-lo)*profile.command.joint_directions[channel] - # The passive table is deliberately nonlinear and not equal to mimic. - y = (.18*travel+.07*travel**2 if name in profile.zero.passive_joints - else .35*travel+.05*travel**2) - increasing, decreasing = tuple(y+.001*np.abs(travel)), tuple(y-.001*np.abs(travel)) - outputs[name] = JointMapping(name, channel, f"feedback_{profile.command.unit}", - tuple(knots), tuple(y), increasing, decreasing) - command_y = tuple(y+.002*travel**2) - commands[name] = replace(outputs[name], input_domain=f"command_{profile.command.unit}", - angle_rad=command_y) - curves[name] = JointCurveFit(tuple(y), decreasing, increasing, {}, 0., .002, {}) - spec = profile.motion.joint_zero_references[name] - # Samples are intentionally absent: these references test ownership, - # and this helper is never used as a publishable capture fixture. - references[name] = JointZeroReference(name, spec.task_key, baseline, channel, 1, 3, ()) - offsets = {name: .02 for name in profile.zero.direct_zero_joints} - methods = {name: "urdf_serial_axis_geometry" for name in offsets} - for name, geometry in profile.zero.known_baseline_geometry.items(): - offsets[name] = geometry.cad_angle_rad - methods[name] = "independent_known_baseline_geometry" - for name in profile.zero.cad_zero_assumptions: - offsets[name] = 0. - methods[name] = "assumed_source_cad_zero" - spatial = ZeroSolveResult(direct_offsets_rad={}, all_active_offsets_rad={}, - base_translation_xyz_m=(0., 0., 0.), base_quaternion_xyzw=(0., 0., 0., 1.), - validation_errors_rad=(), validation_error_by_joint_rad={}, validation_line_error_by_joint_m={}, - axis_line_rms_m=0., passed=True, cycle_offsets_rad={}, offset_uncertainty_rad={}, - offset_confidence_half_width_rad={}, training_cycles=(0, 1, 2), validation_cycle=3, - validation_original_error_by_joint_rad={}, validation_improvement_by_joint_rad={}, - validation_improvement_confidence_lower_rad={}, observability_rank=0, - observability_parameter_count=0, observability_condition_number=1., offset_covariance_rad2={}, - axis_cone_mismatch_by_joint_rad={}, axis_cone_bias_classification_by_joint={}, failure_reasons={}) - mimics = {name: MimicApproximation( - LinearMimicFit(parent, name, .6, 0., False, (0, 1, 2), (.001, -.002)), - (f"{name}:train",), (f"{name}:holdout",), (.003,)) - for name, parent in profile.zero.mimic_source_by_joint.items() - if name in observed and parent in observed} - return CalibrationResult(curves, outputs, offsets, methods, {}, - {name: (.003,) for name in observed}, spatial, - reference_inputs={name: ref.baseline_command for name, ref in references.items()}, - command_mappings=commands, - command_holdout_metrics={name: {"independently_measured": True} for name in observed}, - command_applicability={name: {"scope": "single_channel_at_declared_pose"} for name in observed}, - joint_zero_references=references, mimic_approximations=mimics) - - -@pytest.mark.parametrize("layout,count", [ - ("o6_right_8", 6), ("l6_right_8", 6), ("o12_right_16", 3), ("g20_right_19", 0), -]) -@pytest.mark.parametrize("reordered", [False, True], ids=["product", "virtual_reordered"]) -def test_three_observation_strategies_use_recipient_channels_without_refitting(layout, count, reordered): - profile = load_bundled_hand_profile(layout) - if reordered: - profile = reordered_profile(profile) - original = _frozen_fit(profile) - expanded = expand_profile_transfers(profile, original) - transfers = profile.measurement.transferred_motion_sources - assert len(transfers) == count - assert set(expanded.output_mappings) == set(profile.zero.active_joints) | set(profile.zero.passive_joints) - for field in ("curves", "joint_zero_references", "holdout_errors_rad", "command_holdout_metrics"): - assert getattr(expanded, field) == getattr(original, field) - assert not set(transfers) & getattr(expanded, field).keys() - for target, source in transfers.items(): - recipient_channel = channel_for_joint(profile, target) - assert recipient_channel != channel_for_joint(profile, source) - for field in ("output_mappings", "command_mappings"): - donor = getattr(original, field)[source] - mapping = getattr(expanded, field)[target] - assert mapping == replace(donor, joint=target, motor_index=recipient_channel, transferred_from=source) - assert mapping.evaluate(profile.command.baseline_values[recipient_channel]) == 0. - positions = list(profile.command.baseline_values) - positions[recipient_channel] = donor.knots[len(donor.knots)//2] - actual = evaluate_mappings(getattr(expanded, field), positions, {}) - assert actual[target] == donor.evaluate(positions[recipient_channel]) - assert actual[source] == 0. - assert target not in original.output_mappings - if target in profile.zero.passive_joints: - approximate = expanded.mimic_approximations[target] - source_fit = original.mimic_approximations[source] - assert approximate.transferred_from_joint == source - assert approximate.training_sample_ids == source_fit.training_sample_ids - assert approximate.holdout_sample_ids == source_fit.holdout_sample_ids - assert approximate.holdout_errors_rad == source_fit.holdout_errors_rad - assert approximate.output_fit.source_joint == profile.zero.mimic_source_by_joint[target] - parent = expanded.command_mappings[approximate.output_fit.source_joint] - child = expanded.command_mappings[target] - assert np.max(np.abs(np.asarray(child.angle_rad)-.6*np.asarray(parent.angle_rad))) > .005 - - -def test_old_zero_and_mimic_transfer_declarations_do_not_authorize_v3_motion_copy(): - profile = load_bundled_hand_profile("o6_right_8") - profile = replace(profile, measurement=replace(profile.measurement, transferred_motion_sources={})) - original = _frozen_fit(profile) - expanded = expand_profile_transfers(profile, original) - assert expanded == original - assert "rh_index_mcp_pitch" not in expanded.output_mappings - with pytest.raises(ValueError, match="unmeasured"): - prepare_standard_result(profile, O6_SOURCE, _digest(O6_SOURCE), expanded) - - -def test_o12_motion_transfer_does_not_invent_passive_pip_zero(): - profile = load_bundled_hand_profile("o12_right_16") - profile = replace(profile, zero=replace(profile.zero, cad_zero_assumptions={ - name: value for name, value in profile.zero.cad_zero_assumptions.items() - if name not in {"pinky_pip", "ring_pip", "thumb_mcp"}})) - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - for name in ("pinky_pip", "ring_pip"): - assert name in expanded.output_mappings - assert name not in expanded.zero_offsets_rad - assert name not in profile.zero.cad_zero_assumptions - with pytest.raises(ValueError, match="absolute_geometry.*pinky_pip.*ring_pip"): - prepare_standard_result(profile, O12_SOURCE, _digest(O12_SOURCE), expanded) - - -def test_o12_declared_cad_baselines_do_not_become_measured_zero_evidence(): - profile = load_bundled_hand_profile("o12_right_16") - fit = expand_profile_transfers(profile, _frozen_fit(profile)) - for name in ("thumb_mcp", "pinky_pip", "ring_pip"): - assert name in profile.zero.cad_zero_assumptions - assert name not in profile.zero.direct_zero_joints - assert fit.zero_offsets_rad[name] == 0 - assert fit.zero_method_by_joint[name] == "assumed_source_cad_zero" - assert fit.command_mappings["ring_pip"].transferred_from == "pinky_pip" - assert "ring_pip" not in fit.joint_zero_references - - -def test_passive_pip_zero_uses_separate_datums_while_motion_is_copied(): - profile = load_bundled_hand_profile("o12_right_16") - profile = replace(profile, zero=replace(profile.zero, cad_zero_assumptions={ - name: value for name, value in profile.zero.cad_zero_assumptions.items() - if name not in {"pinky_pip", "ring_pip"}})) - geometry = dict(profile.zero.known_baseline_geometry, - pinky_pip=KnownBaselineGeometry(.07, .001, "synthetic_pinky_datum")) - profile = replace(profile, zero=replace(profile.zero, known_baseline_geometry=geometry)) - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - assert expanded.zero_offsets_rad["pinky_pip"] == .07 - assert "ring_pip" not in expanded.zero_offsets_rad - geometry = dict(geometry, ring_pip=KnownBaselineGeometry(.11, .001, "synthetic_ring_datum")) - profile = replace(profile, zero=replace(profile.zero, known_baseline_geometry=geometry)) - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - assert expanded.zero_offsets_rad["pinky_pip"] == .07 - assert expanded.zero_offsets_rad["ring_pip"] == .11 - assert expanded.zero_method_by_joint["ring_pip"] == "independent_known_baseline_geometry" - assert "ring_pip" not in profile.zero.transferred_zero_sources - assert expanded.command_mappings["ring_pip"].angle_rad == expanded.command_mappings["pinky_pip"].angle_rad - assert "ring_pip" not in expanded.joint_zero_references - - -def test_o6_urdf_keeps_terminal_origins_and_each_recipient_link_geometry(tmp_path): - profile = _geometry_only_o6_profile() - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - source_hash = _digest(O6_SOURCE) - prepared = prepare_standard_result(profile, O6_SOURCE, source_hash, expanded) - destination = prepared.plan.write(O6_SOURCE, tmp_path/"corrected.urdf") - before, after = ET.parse(O6_SOURCE).getroot(), ET.parse(destination).getroot() - before_joints = {j.get("name"): j for j in before.findall("joint")} - after_joints = {j.get("name"): j for j in after.findall("joint")} - for name in profile.zero.cad_zero_assumptions: - assert expanded.zero_offsets_rad[name] == 0. - assert after_joints[name].find("origin").attrib == before_joints[name].find("origin").attrib - assert after_joints[name].find("mimic").get("joint") == before_joints[name].find("mimic").get("joint") - assert prepared.plan.mimic_policy == "preserve_source_mimic" - assert after_joints[name].find("mimic").attrib == before_joints[name].find("mimic").attrib - for name, original in before_joints.items(): - current = after_joints[name] - for tag in ("parent", "child", "axis"): - assert current.find(tag).attrib == original.find(tag).attrib - assert current.find("origin").get("xyz") == original.find("origin").get("xyz") - assert [ET.tostring(link) for link in before.findall("link")] == [ET.tostring(link) for link in after.findall("link")] - assert after_joints["rh_index_mcp_pitch"].find("origin").get("xyz") != after_joints["rh_pinky_mcp_pitch"].find("origin").get("xyz") - assert _digest(O6_SOURCE) == source_hash - - -@pytest.mark.parametrize("field", ["output_mappings", "command_mappings"]) -def test_changed_recipient_curve_cannot_pass_as_exact_transfer(field): - profile = load_bundled_hand_profile("o6_right_8") - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - mappings = dict(getattr(expanded, field)) - target = "rh_index_dip" - mappings[target] = replace(mappings[target], angle_rad=tuple(v*1.01 for v in mappings[target].angle_rad)) - with pytest.raises(ValueError, match="differs_from_declared_donor"): - prepare_standard_result(profile, O6_SOURCE, _digest(O6_SOURCE), replace(expanded, **{field: mappings})) - - -def test_transferred_curve_cannot_expand_recipient_mechanical_range(tmp_path): - # Without an explicit nominal-envelope declaration, the recipient's own - # CAD upper bound remains a mandatory physical limit after zero conversion. - profile = _geometry_only_o6_profile() - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - source = tmp_path/"narrow_recipient.urdf" - tree = ET.parse(O6_SOURCE) - tree.getroot().find("joint[@name='rh_index_mcp_pitch']/limit").set("upper", "0.2") - tree.write(source, encoding="utf-8", xml_declaration=True) - with pytest.raises(ValueError, match="range.*(mechanical|physical)|mapping exceeds"): - prepare_standard_result(profile, source, _digest(source), expanded) - - -def test_metadata_only_transfer_fit_cannot_authorize_measured_motion_envelopes(): - profile = load_bundled_hand_profile("o6_right_8") - expanded = expand_profile_transfers(profile, _frozen_fit(profile)) - assert profile.urdf_limit_policies - with pytest.raises(ValueError, match="motion_range_requires_training_provenance"): - prepare_standard_result(profile, O6_SOURCE, _digest(O6_SOURCE), expanded) - - -@pytest.mark.parametrize("problem", ["missing_donor", "wrong_channel", "already_expanded"]) -def test_invalid_donor_result_cannot_be_transferred(problem): - profile = load_bundled_hand_profile("o6_right_8") - fit = _frozen_fit(profile) - mappings = dict(fit.command_mappings) - if problem == "missing_donor": - mappings.pop("rh_pinky_dip") - fit = replace(fit, command_mappings=mappings) - elif problem == "wrong_channel": - mappings["rh_pinky_dip"] = replace(mappings["rh_pinky_dip"], motor_index=0) - fit = replace(fit, command_mappings=mappings) - else: - fit = expand_profile_transfers(profile, fit) - with pytest.raises(ValueError, match="motion_transfer"): - expand_profile_transfers(profile, fit) - - -@pytest.mark.parametrize("policy,joint,reason", [ - ("transferred", "rh_index_mcp_pitch", "transferred_zero_differs_from_declared_donor"), - ("assumed", "rh_pinky_dip", "assumed_cad_zero_differs_from_profile"), - ("known", "rh_thumb_ip", "known_baseline_geometry_differs_from_profile"), -]) -@pytest.mark.parametrize("changed", ["offset", "method"]) -def test_result_plan_rejects_zero_parameters_that_change_the_declared_evidence(policy, joint, reason, changed): - profile = _geometry_only_o6_profile() - if policy == "known": - assumptions = {name: value for name, value in profile.zero.cad_zero_assumptions.items() if name != joint} - geometry = dict(profile.zero.known_baseline_geometry, - **{joint: KnownBaselineGeometry(0., .001, "synthetic_independent_datum")}) - profile = replace(profile, zero=replace(profile.zero, - known_baseline_geometry=geometry, cad_zero_assumptions=assumptions)) - fit = expand_profile_transfers(profile, _frozen_fit(profile)) - # Verify the valid starting point, then change one field only. A copied - # dynamic curve must not authorize a new geometric zero or evidence label. - prepare_standard_result(profile, O6_SOURCE, _digest(O6_SOURCE), fit) - if changed == "offset": - offsets = dict(fit.zero_offsets_rad) - offsets[joint] += .04 - fit = replace(fit, zero_offsets_rad=offsets) - else: - methods = dict(fit.zero_method_by_joint) - methods[joint] = "urdf_serial_axis_geometry" - fit = replace(fit, zero_method_by_joint=methods) - with pytest.raises(ValueError, match=reason): - prepare_standard_result(profile, O6_SOURCE, _digest(O6_SOURCE), fit) - - -def test_public_fitting_rejects_conflicting_zero_sources_before_reading_inputs(tmp_path): - profile = load_bundled_hand_profile("o6_right_8") - geometry = dict(profile.zero.known_baseline_geometry, - rh_pinky_dip=KnownBaselineGeometry(.03, .001, "synthetic_independent_datum")) - profile = replace(profile, zero=replace(profile.zero, known_baseline_geometry=geometry)) - with pytest.raises(ProfileValidationError, match="CAD zero assumption conflicts.*rh_pinky_dip"): - fit_profile_calibration(profile, tmp_path/"nonexistent.urdf", {}) diff --git a/src/linkerhand_calibration/test/test_native_position_feedback.py b/src/linkerhand_calibration/test/test_native_position_feedback.py deleted file mode 100644 index 6690c79..0000000 --- a/src/linkerhand_calibration/test/test_native_position_feedback.py +++ /dev/null @@ -1,387 +0,0 @@ -"""Command completion is not an unmeasured command-to-encoder calibration.""" - -from dataclasses import replace - -import numpy as np -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.adapters import HardwareHealth -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.runtime.safety import SafetyPolicy, SafetySample -from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit - - -def feedback_profile(): - """Exercise the production O30 contract, not a test-only override.""" - profile = load_bundled_hand_profile("o30_right_18", scope="full") - assert profile.acquisition.motion_observation == "feedback" - assert profile.command_based_release - assert not profile.acquisition.feedback_travel_matches_command - return profile - - -@pytest.mark.parametrize("phase", ["baseline", "prepare", "steady"]) -def test_endpoint_dead_zone_does_not_create_an_encoder_target(phase): - profile = feedback_profile() - start = list(profile.command.baseline_values) - start[15] = 11 - feedback = start.copy() - feedback[15] = 6 # Observed at native command zero; never relabel this as zero. - motion = MotionCommand(phase, profile.command.baseline_values, 200., command_index=15) - segment = MotionExecution(profile, motion, initial_command=start, - initial_feedback=feedback, now=0., identity="endpoint") - safety = SafetyPolicy(profile) - for stamp in range(150): - now = stamp / 30 - command = segment.sample(now) - segment.observe(feedback, stamp=stamp, now=now) - assert safety.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)).safe - assert command[15] == 0 and feedback[15] == 6 - assert segment.arrived(now) - assert not segment.goals - - -def test_declared_common_scale_keeps_stall_protection(): - profile = feedback_profile() - profile = replace(profile, acquisition=replace(profile.acquisition, feedback_travel_matches_command=True)) - start = list(profile.command.baseline_values) - start[15] = 100 - segment = MotionExecution(profile, MotionCommand("prepare", profile.command.baseline_values, 200.), - initial_command=start, initial_feedback=start, now=0., identity="stopped") - safety = SafetyPolicy(profile) - decisions = [] - for stamp in range(150): - now = stamp / 30 - command = segment.sample(now) - segment.observe(start, stamp=stamp, now=now) - decisions.append(safety.evaluate(SafetySample(now, now, command, tuple(start), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals))) - assert any(result.code == "mechanical_stall" for result in decisions) - assert not segment.arrived(now) - - -def test_unmapped_full_return_cannot_finish_without_real_feedback_motion(): - profile = feedback_profile() - start = list(profile.command.baseline_values) - start[0] = 253 - segment = MotionExecution(profile, MotionCommand("baseline", profile.command.baseline_values, 200.), - initial_command=start, initial_feedback=start, now=0., identity="recorded_blocked_return") - safety = SafetyPolicy(profile) - stopped_at = None - requested_at = None - for stamp in range(900): - now = stamp / 30 - command = segment.sample(now) - segment.observe(start, stamp=stamp, now=now) - if segment.motion_expected and requested_at is None: - requested_at = now - result = safety.evaluate(SafetySample(now, now, command, tuple(start), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)) - if not result.safe and stopped_at is None: - assert result.code == "mechanical_stall" - stopped_at = now - assert stopped_at is not None and requested_at is not None - expected_stop = requested_at + profile.acquisition.stall_timeout_seconds - assert expected_stop <= stopped_at <= expected_stop + 1/30 - assert not segment.arrived(now) - - -def test_unmapped_group_starts_each_motion_watchdog_from_its_own_demand(): - profile = feedback_profile() - start = list(profile.command.baseline_values) - target = start.copy() - target[0], target[2] = 255, 235 - segment = MotionExecution(profile, MotionCommand("prepare", tuple(target), 200.), - initial_command=start, initial_feedback=start, now=0., identity="coupled") - segment.sample(4.) - assert {goal.channel for goal in segment.goals} == {0} - segment.sample(16.) - assert {goal.channel for goal in segment.goals} == {0, 2} - - -@pytest.mark.parametrize("phase", ["steady", "joint_zero"]) -@pytest.mark.parametrize("common_scale", [False, True]) -def test_short_capture_cannot_advance_or_record_without_requested_motion(phase, common_scale): - profile = feedback_profile() - profile = replace(profile, acquisition=replace(profile.acquisition, - feedback_travel_matches_command=common_scale)) - start = list(profile.command.baseline_values) - start[0] = 255 - target, feedback = start.copy(), start.copy() - target[0], feedback[0] = 239, 253 - segment = MotionExecution(profile, MotionCommand(phase, tuple(target), 200., command_index=0), - initial_command=start, initial_feedback=feedback, now=0., identity="blocked_steady_return") - safety = SafetyPolicy(profile) - for stamp in range(300): - now = stamp/30 - command = segment.sample(now) - segment.observe(feedback, stamp=stamp, now=now) - assert not segment.steady_ready(now) - assert not segment.arrived(now) - decision = safety.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)) - if not decision.safe: - assert decision.code == "mechanical_stall" - assert now < profile.acquisition.stall_timeout_seconds + 2. - break - else: - pytest.fail("short steady segment reset its no-motion watchdog") - - -@pytest.mark.parametrize("sent_zero", [False, True]) -@pytest.mark.parametrize("measured_zero", [3., 10.]) -def test_measured_zero_return_needs_the_observed_encoder_zero(sent_zero, measured_zero): - profile = feedback_profile() - target = list(profile.command.baseline_values) - target[2] = 0 - start, feedback = target.copy(), target.copy() - start[0], feedback[0] = (0 if sent_zero else 255), 253 - segment = MotionExecution(profile, MotionCommand("task_exit", tuple(target), 200., - feedback_targets=((0, measured_zero),)), initial_command=start, initial_feedback=feedback, - now=0., identity="return_before_index_clearance") - safety = SafetyPolicy(profile) - for stamp in range(900): - now = stamp/30 - command = segment.sample(now) - feedback[0] = 240 # Some travel is insufficient for a known zero return. - segment.observe(feedback, stamp=stamp, now=now) - assert not segment.arrived(now) - decision = safety.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)) - if not decision.safe: - assert decision.code == "mechanical_stall" - break - else: - pytest.fail("a partial return released the avoidance joint") - # Recovery compares to the independently observed zero, not command 0. - for stamp in range(900, 1000): - now = stamp/30 - segment.sample(now) - feedback[0] = measured_zero - segment.observe(feedback, stamp=stamp, now=now) - assert segment.arrived(now) - - -@pytest.mark.parametrize("phase", ["baseline", "prepare", "steady", "joint_zero"]) -def test_ten_unit_tracking_offset_allows_motion_and_stable_capture(phase): - profile = feedback_profile() - start = profile.command.baseline_values - target = list(start) - target[0] = 80 - feedback = list(start) - feedback[0] = 10 - segment = MotionExecution(profile, MotionCommand(phase, tuple(target), 200., command_index=0), - initial_command=start, initial_feedback=feedback, now=0., identity="offset10") - safety = SafetyPolicy(profile) - for stamp in range(240): - now = stamp/30 - command = segment.sample(now) - feedback[0] = command[0]+10 - segment.observe(feedback, stamp=stamp, now=now) - assert safety.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)).safe - assert segment.arrived(now) - assert feedback[0]-command[0] == 10 - if phase in {"steady", "joint_zero"}: - assert segment.steady_ready(now) - - -def test_production_o30_requires_fresh_feedback_before_start_and_during_motion(tmp_path): - from runtime_host_fixture import coordinator_fixture - host, clock = coordinator_fixture(tmp_path, model="o30") - host.parameters = replace(host.parameters, reuse_camera_calibration=True) - try: - for view in host.profile.vision.view_names: - host.cameras.matrices[view] = np.eye(3) - host.cameras.image_sizes[view] = (640,480) - host.cameras.info_received_at[view] = clock.now - host.cameras.detections_received_at[view] = clock.now - host.tick() - assert not host.start().success - host.receive_feedback(host.command_names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.tick() - assert host.start().success - clock.now += 1.01 - host.tick() - assert host.state == "PAUSED" - assert "feedback_stale" in host.reason - assert host.visual_motion is None - finally: - host.close() - - -def test_task_exit_binds_its_own_zero_before_releasing_index_clearance(): - from types import SimpleNamespace - from linkerhand_calibration.runtime.execution import SessionExecution - from linkerhand_calibration.runtime.session import CalibrationPhase - profile = feedback_profile() - driver = SessionExecution(profile) - task = profile.motion.tasks[0] - zero_command = profile.motion.joint_zero_references["thumb_cmc_roll"].command - observed = list(zero_command) - observed[0] = 3. - driver.zero_references["thumb_cmc_roll"] = SimpleNamespace(task_key=task.key, channel=0, - samples=(SimpleNamespace(command=zero_command, feedback=tuple(observed)),)) - driver._entered_task = task.key - driver.session.phase = CalibrationPhase.PREPARE - driver.session._unit_index = next(i for i, unit in enumerate(driver.session.engine.scan_units()) - if unit.task_key == profile.motion.tasks[1].key) - # The command was already zero, but this provides no arrival evidence. - first = driver.motion(zero_command) - assert first.phase == "task_exit" and first.target[2] == 0 - assert first.feedback_targets == ((0, 3.),) - assert driver.motion(zero_command) is first # Nothing releases the second waypoint automatically. - driver.motion_complete() - second = driver.motion(first.target) - assert second.phase == "task_exit" and second.target[2] == 255 - assert not second.feedback_targets # A different complete pose cannot borrow the zero. - - -def test_capture_before_motion_completion_fix_is_not_resumable(): - from linkerhand_calibration.runtime.engine import capture_schedule_version - from linkerhand_calibration.core.domain.profile import ACQUISITION_POLICY_VERSION - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - profile = feedback_profile() - engine = CalibrationEngine(profile) - header = dict(profile_id=profile.key.profile_id, acquisition_policy_version=ACQUISITION_POLICY_VERSION, - pose_tracking_policy_version=POSE_TRACKING_POLICY_VERSION, - capture_schedule_version="unified_schedule_v4_segmented_motion") - assert not engine.resume_compatible(header) - header["capture_schedule_version"] = capture_schedule_version(profile) - assert engine.resume_compatible(header) - - -def test_unmoving_encoder_cannot_pass_sweep_quality(): - profile = feedback_profile() - unit = CalibrationEngine(profile).scan_units()[0] - rows = [dict(joint="thumb_cmc_roll", view="front", task_name=unit.task_key, - cycle=unit.cycle, direction=unit.direction, attempt=1, image_stamp_ns=stamp, - sample_phase="sweep", feedback_u8=6.) for stamp in range(1, 100)] - quality = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}) - assert not quality.passed - assert any("span" in reason or "bins" in reason for reason in quality.failures) - - -def test_independent_feedback_must_settle_before_preparation_completes(): - profile = feedback_profile() - start = profile.command.baseline_values - target = list(start) - target[0] = 40 - segment = MotionExecution(profile, MotionCommand("prepare", tuple(target), 200.), - initial_command=start, initial_feedback=start, now=0., identity="unsettled") - for stamp in range(300): - now = stamp / 30 - segment.sample(now) - feedback = list(start) - feedback[0] = 40 + (3 if stamp % 2 else -3) - segment.observe(feedback, stamp=stamp, now=now) - assert not segment.arrived(now) - - -def _observed_sweep(unit, joint, *, missing_command=None): - return [dict(joint=joint, view="front", task_name=unit.task_key, - cycle=unit.cycle, direction=unit.direction, attempt=1, image_stamp_ns=command + 1, - segment_key=unit.segment_key, sample_phase="sweep", command_u8=float(command), - feedback_u8=3. + 247. * command / 255.) - for command in range(256) if command != missing_command] - - -def test_native_encoder_endpoints_are_not_command_endpoints(): - profile = feedback_profile() - unit = CalibrationEngine(profile).scan_units()[0] - rows = _observed_sweep(unit, "thumb_cmc_roll") - result = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}, include_steady=False) - assert result.passed, result.failures - metrics = next(iter(result.metrics.values())) - assert metrics["input_span"] == 1. - assert metrics["coverage_input_domain"] == "command_u8" - assert metrics["feedback_motion"]["observed_span"] == pytest.approx(247.) - assert metrics["endpoint_observation_domain"] == "command" - assert metrics["endpoint_observed_minimum_01"] == 0. - assert metrics["endpoint_observed_maximum_01"] == 1. - assert (rows[0]["feedback_u8"], rows[-1]["feedback_u8"]) == (3., 250.) - common = replace(profile, acquisition=replace(profile.acquisition, - feedback_travel_matches_command=True)) - assert any("endpoint_observation" in failure for failure in - evaluate_capture_unit(common, unit, 1, rows, first_cycle_spans={}).failures) - - -@pytest.mark.parametrize("missing", ["command", "endpoint_image", "feedback_motion"]) -def test_native_endpoint_gate_requires_both_command_observation_and_actual_travel(missing): - profile = feedback_profile() - unit = CalibrationEngine(profile).scan_units()[0] - rows = _observed_sweep(unit, "thumb_cmc_roll") - if missing == "command": - for row in rows: - row.pop("command_u8") - elif missing == "endpoint_image": - rows = [row for row in rows if 10 <= row["command_u8"] <= 245] - else: - for row in rows: - row["feedback_u8"] = 6. - result = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}) - assert not result.passed - expected = "feedback_motion_feedback_span" if missing == "feedback_motion" else "endpoint_observation" - assert any(expected in failure for failure in result.failures) - - -def test_group_endpoint_observations_belong_to_each_moving_channel(): - profile = feedback_profile() - unit = next(unit for unit in CalibrationEngine(profile).scan_units() - if unit.segment_key == "linked_increasing") - rows = [] - for finger in ("index", "middle", "ring", "pinky"): - selected = _observed_sweep(unit, finger + "_mcp_roll") - if finger == "middle": - for row in selected: - row["command_u8"] = 80 + 175 * row["command_u8"] / 255. - row["feedback_u8"] = row["command_u8"] - elif finger == "index": - selected = [row for row in selected if row["command_u8"] < 245] - rows.extend(selected) - result = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}) - failures = [failure for failure in result.failures if "endpoint_observation" in failure] - assert len(failures) == 1 and "index_mcp_roll" in failures[0] - - -@pytest.mark.parametrize("offset", [-10, 10]) -def test_each_group_segment_covers_commands_and_its_own_biased_feedback(offset): - from linkerhand_calibration.core.domain.motion_path import task_segment - from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint - profile = feedback_profile() - task = next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - for unit in (u for u in CalibrationEngine(profile).scan_units() if u.task_key == task.key and u.cycle == 0): - segment = task_segment(task, unit.segment_key) - rows = [] - for joint in segment.joints: - channel = channel_for_joint(profile, joint) - for i, progress in enumerate(np.linspace(0,1,256)): - start, end = dict(segment.start_commands)[channel], dict(segment.end_commands)[channel] - command = round(start+(end-start)*progress) - rows.append(dict(joint=joint, view="front", task_name=task.key, cycle=unit.cycle, - direction=unit.direction, segment_key=unit.segment_key, attempt=1, - image_stamp_ns=i+1, sample_phase="sweep", command_u8=command, - feedback_u8=max(0,min(255,command+offset)))) - quality = evaluate_capture_unit(profile,unit,1,rows,first_cycle_spans={},include_steady=False) - assert quality.passed, (segment.key, quality.failures) - assert all(m['coverage_input_domain']=='command_u8' and m['input_span']==1. - and m['feedback_motion']['observed_span']>0 for m in quality.metrics.values()) - missing_joint = segment.joints[0] - stuck = [dict(row,feedback_u8=77.) if row['joint']==missing_joint else row for row in rows] - quality = evaluate_capture_unit(profile,unit,1,stuck,first_cycle_spans={},include_steady=False) - assert not quality.passed - assert all(missing_joint in failure for failure in quality.failures) - - -def test_small_encoder_noise_cannot_be_normalized_into_full_motion(): - profile = feedback_profile() - unit = CalibrationEngine(profile).scan_units()[0] - rows = _observed_sweep(unit, "thumb_cmc_roll") - for row in rows: - row['feedback_u8'] = 6.+row['command_u8']/255. - quality = evaluate_capture_unit(profile,unit,1,rows,first_cycle_spans={},include_steady=False) - assert not quality.passed - assert any('below_motion_resolution' in reason for reason in quality.failures) diff --git a/src/linkerhand_calibration/test/test_native_rotation_curve.py b/src/linkerhand_calibration/test/test_native_rotation_curve.py deleted file mode 100644 index 65fd756..0000000 --- a/src/linkerhand_calibration/test/test_native_rotation_curve.py +++ /dev/null @@ -1,72 +0,0 @@ -import math -from dataclasses import replace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting.rotation_curve import RotationObservation, fit_rotation_curve - - -def observations(unit="rad", slip_cycle=None, hysteresis=0.024): - # Independent generator: arbitrary fixed Tag installation on both links. - parent = Rotation.from_euler("xyz", (0.27, -0.18, 0.31)) - child = Rotation.from_euler("xyz", (-0.16, 0.24, -0.21)) - rows = [] - for cycle in range(4): - for direction in ("increasing", "decreasing"): - for index, phase in enumerate(np.linspace(0, 1, 100)): - angle = 0.13 + 0.9*phase + 0.08*phase**2 - angle += hysteresis/2 if direction == "increasing" else -hysteresis/2 - q = parent.inv() * Rotation.from_euler("z", angle) * child - if cycle == slip_cycle: - q = q * Rotation.from_euler("x", math.radians(5)) - value = -0.94 + 0.89*phase if unit == "rad" else 255*phase - rows.append(RotationObservation(f"{cycle}/{direction}/{index}", value, - tuple(q.as_quat()), cycle, direction)) - return rows - - -@pytest.mark.parametrize("unit", ["rad", "u8"]) -def test_native_domain_recovers_motion_with_frozen_arbitrary_tag_mounts(unit): - rows = observations(unit) - fit = fit_rotation_curve([row for row in rows if row.cycle < 3], - input_domain="feedback_" + unit, sdk_to_joint_direction=1) - assert max(fit.holdout_errors([row for row in rows if row.cycle == 3])) < math.radians(0.01) - assert len(fit.knots) == 65 - if unit == "rad": - assert fit.knots[0] == -0.94 - assert fit.knots[-1] == pytest.approx(-0.05) - assert np.max(np.subtract(fit.increasing_rad, fit.decreasing_rad)) == pytest.approx(0.024, abs=1e-9) - - -def test_fourth_cycle_tag_slip_cannot_be_absorbed_into_a_new_reference(): - rows = observations(slip_cycle=3) - fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=1) - assert min(fit.holdout_errors(rows[600:])) > math.radians(4.9) - - -def test_holdout_never_changes_training_domain_or_axis(): - rows = observations() - with pytest.raises(ValueError, match="partition"): - fit_rotation_curve(rows, input_domain="feedback_rad", sdk_to_joint_direction=1) - fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=1) - with pytest.raises(ValueError, match="outside"): - fit.holdout_errors([replace(rows[-1], input_value=0.1)]) - with pytest.raises(ValueError, match="overlaps"): - fit.holdout_errors([replace(rows[-1], sample_id=rows[0].sample_id)]) - - -def test_relative_tag_reference_is_not_published_as_cad_zero(): - rows = observations() - fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=-1) - assert fit.decreasing_rad[-1] < fit.decreasing_rad[0] - # The primitive exposes a measurement reference, not a fabricated zero. - assert not hasattr(fit, "zero_offset_rad") - - -def test_repeatable_hysteresis_keeps_two_branches_and_passes_independent_holdout(): - rows = observations(hysteresis=math.radians(4.2)) - fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=1) - assert np.max(np.subtract(fit.increasing_rad, fit.decreasing_rad)) == pytest.approx(math.radians(4.2)) - assert max(fit.holdout_errors(rows[600:])) < math.radians(0.01) diff --git a/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py b/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py deleted file mode 100644 index 7585f22..0000000 --- a/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py +++ /dev/null @@ -1,60 +0,0 @@ -"""Observable line residuals must not be confused with axial PnP drift.""" - -import inspect -import math -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting.spatial import ( - _fit_axis_point_from_pose_trajectory, fit_joint_axis_measurement, -) - - -def axis_fit(*, axial_drift=0., transverse_drift=0., separate=False): - axis = np.array([0., 0., 1.]) - point = np.array([.012, -.017, 0.]) - reference = np.array([.047, .009, .02]) - rows = [] - for index, angle in enumerate(np.linspace(0., 1.2, 128)): - rotation = Rotation.from_rotvec(axis * angle) - translation = point + rotation.apply(reference - point) - translation += axis * axial_drift * math.sin(angle * 3.) - translation[0] += transverse_drift * math.sin(angle * 11.) - rows.append({ - 'command_u8': 255 - 2 * index, 'direction': 'decreasing', - 'relative_translation_xyz_m': translation.tolist(), - 'relative_quaternion_xyzw': rotation.as_quat().tolist(), - }) - diagnostics = {} - fitted, rms, _ = _fit_axis_point_from_pose_trajectory( - rows, zero_command_u8=255, axis_parent_xyz=axis, - angle_axis_parent_xyz=axis, phase_reference_point_parent_xyz=point, - view_normal_common_xyz=None, canonical_zero_direction='decreasing', - allow_axial_translation=separate, residual_diagnostics=diagnostics, - ) - return fitted, rms, diagnostics - - -def test_axial_component_is_reported_but_not_used_as_transverse_fit_error(): - baseline, baseline_rms, _ = axis_fit(separate=True) - observed, rms, diagnostic = axis_fit(axial_drift=.015, separate=True) - assert observed == pytest.approx(baseline, abs=1.e-9) - assert rms == pytest.approx(baseline_rms, abs=1.e-9) - assert diagnostic['axial_rms_m'] > .005 - assert diagnostic['raw_rms_m'] > rms - assert diagnostic['transverse_rms_m'] == pytest.approx(rms, abs=1.e-9) - - -def test_transverse_model_error_is_not_discarded_by_projection(): - _, rms, diagnostic = axis_fit(transverse_drift=.01, separate=True) - assert rms > .0015 - assert diagnostic['transverse_rms_m'] > .0015 - - -def test_legacy_default_does_not_silently_enable_projection(): - assert inspect.signature(fit_joint_axis_measurement).parameters[ - 'separate_axial_residual'].default is False - _, rms, diagnostic = axis_fit(axial_drift=.015) - assert rms > .0015 - assert diagnostic['raw_rms_m'] == pytest.approx(rms) diff --git a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py deleted file mode 100644 index e15bfd3..0000000 --- a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py +++ /dev/null @@ -1,243 +0,0 @@ -"""Non-zero recovery through G20 geometry, not zero-only CAD fixtures.""" - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import JointCurveFit -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import UrdfKinematicModel -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import ( - CALIBRATED_ACTIVE_JOINTS, - GEOMETRIC_ZERO_JOINTS, - STATIC_ZERO_EXCLUDED_JOINTS, - build_typed_profile, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.zero import ( - AXIS_JOINTS, PHASE_PARENT, ZERO_OBSERVER, O12SpatialZeroError, - full_hand_zero_profile, motor_index, solve_full_hand_zero, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.kinematics import PASSIVE_SDK_SOURCE_BY_JOINT - -from test_o12_right_profile import SOURCE_URDF, _synthetic_records - - -def pose(t): - return {"translation_xyz_m": t[:3, 3].tolist(), - "quaternion_xyzw": Rotation.from_matrix(t[:3, :3]).as_quat().tolist()} - - -def transform(xyz, rpy): - t = np.eye(4) - t[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() - t[:3, 3] = xyz - return t - - -def observations(truth, *, coupled_passive=False): - model = UrdfKinematicModel(SOURCE_URDF) - common = transform([.03, -.02, .6], [.15, -.2, .3]) - angle = tuple(np.linspace(.7, 0., 256)) - curve = JointCurveFit(angle, angle, angle, {}, 0., 0., {}) - rows = {} - for i, name in enumerate(AXIS_JOINTS): - mount = transform([.013, -.011, .024], [.21, -.1 * i, .3]) - # Passive trajectories live in the parent Tag frame. A constant - # arbitrary mount must not be mistaken for an encoder zero. - parent = common @ transform([-.01, .015, -.02], [-.2, .11, -.15]) - rows[name] = [] - for cycle in range(4): - for direction, indices in (("decreasing", range(255, -1, -4)), - ("increasing", range(3, 256, 4))): - for index in indices: - state = [255.] * 12 - # A passive axis is independently rotated here to test - # the spatial kernel, not the model's SDK adapter. - if name in CALIBRATED_ACTIVE_JOINTS: - state[motor_index(name)] = index - joint_angles = {name: angle[index]} - sample_parent = parent - if coupled_passive and name in PASSIVE_SDK_SOURCE_BY_JOINT: - state[motor_index(name)] = index - joint_angles[PASSIVE_SDK_SOURCE_BY_JOINT[name]] = angle[index] - sample_parent = common @ model.link_transform( - PHASE_PARENT[name], zero_offsets=truth, joint_angles=joint_angles, - independent_mimic_angles=True, - ) @ transform([.01, .004, .014], [.2, -.1, .3]) - child = common @ model.link_transform( - name, zero_offsets=truth, joint_angles=joint_angles, - independent_mimic_angles=True, - ) @ mount - relative = np.linalg.inv(sample_parent) @ child - rows[name].append({ - "cycle": cycle, "direction": direction, "command_u8": index, - "state_u8": state, - "relative_quaternion_xyzw": pose(relative)["quaternion_xyzw"], - "relative_translation_xyz_m": relative[:3, 3].tolist(), - "parent_pose_common": pose(sample_parent), "child_pose_common": pose(child), - "view_normal_common_xyz": common[:3, 0].tolist(), - "camera_center_common_xyz_m": [0., 0., 0.], - }) - return rows, {name: curve for name in AXIS_JOINTS} - - -def test_profile_authorizes_every_observed_active_zero_without_endpoint_assumptions(): - zero = build_typed_profile().zero - assert set(zero.direct_zero_joints) == GEOMETRIC_ZERO_JOINTS == set(ZERO_OBSERVER) - assert CALIBRATED_ACTIVE_JOINTS - GEOMETRIC_ZERO_JOINTS == STATIC_ZERO_EXCLUDED_JOINTS - assert STATIC_ZERO_EXCLUDED_JOINTS == {"thumb_mcp"} - assert not zero.mechanical_endpoint_joints - assert zero.cad_frozen_joints & CALIBRATED_ACTIVE_JOINTS == STATIC_ZERO_EXCLUDED_JOINTS - from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import get_right_19_thumb_zero_profile - assert not get_right_19_thumb_zero_profile().accept_validated_zero_in_confidence_interval - assert full_hand_zero_profile().accept_validated_zero_in_confidence_interval - assert full_hand_zero_profile().hand.stable_cross_view_cone_bias - model = UrdfKinematicModel(SOURCE_URDF) - for child, parent in PHASE_PARENT.items(): - a, _ = model.axis_line(child, zero_offsets={}, joint_angles={}) - b, _ = model.axis_line(parent, zero_offsets={}, joint_angles={}) - assert abs(float(a @ b)) == pytest.approx(1., abs=1.e-8), (parent, child) - - -@pytest.fixture(scope="module") -def full_solution(): - truth = {name: (.04 if i % 2 else -.06) for i, name in enumerate(ZERO_OBSERVER)} - rows, curves = observations(truth) - result = solve_full_hand_zero(SOURCE_URDF, rows, curves) - return truth, result - - -def test_all_ten_observable_nonzero_offsets_recovered_with_arbitrary_mounts(full_solution): - truth, result = full_solution - assert result.passed - assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) - assert len(result.axis_residual_diagnostics) == 4 * len(AXIS_JOINTS) - assert all(item["axial_component_separated"] - for item in result.axis_residual_diagnostics.values()) - - -def test_coupled_passive_parent_motion_is_not_mistaken_for_a_static_zero(): - truth = {name: .04 for name in ZERO_OBSERVER} - rows, curves = observations(truth, coupled_passive=True) - result = solve_full_hand_zero(SOURCE_URDF, rows, curves) - assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) - - -@pytest.mark.parametrize("move_base,remount", [(False, False), (True, False), - (False, True), (True, True)]) -def test_end_on_phase_is_invariant_to_pre_capture_base_and_tag_changes(move_base, remount): - # Previously the synthetic camera faced CAD X, perpendicular to the - # finger Y axes; that never exercised the real side-camera image branch. - truth = {name: .04 for name in ZERO_OBSERVER} - rows, curves = observations(truth, coupled_passive=True) - common = transform([.03, -.02, .6], [.15, -.2, .3]) - normal = common[:3, :3] @ np.array([.1, .99, .1]) - normal /= np.linalg.norm(normal) - body = transform([.03, -.02, .01], [.07, -.08, .05]) if move_base else np.eye(4) - parent_mount = transform([-.005, .004, .012], [-.1, .05, -.11]) if remount else np.eye(4) - child_mount = transform([.008, -.005, .011], [.1, -.12, .2]) if remount else np.eye(4) - def matrix(payload): - t = np.eye(4) - t[:3, :3] = Rotation.from_quat(payload["quaternion_xyzw"]).as_matrix() - t[:3, 3] = payload["translation_xyz_m"] - return t - for samples in rows.values(): - for row in samples: - parent = body @ matrix(row["parent_pose_common"]) @ parent_mount - child = body @ matrix(row["child_pose_common"]) @ child_mount - relative = np.linalg.inv(parent) @ child - row.update(parent_pose_common=pose(parent), child_pose_common=pose(child), - relative_translation_xyz_m=relative[:3, 3].tolist(), - relative_quaternion_xyzw=Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(), - view_normal_common_xyz=normal.tolist()) - result = solve_full_hand_zero(SOURCE_URDF, rows, curves) - assert result.passed - assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) - - -def test_active_parallel_axis_direction_bias_does_not_become_a_zero(): - truth = {name: .04 for name in ZERO_OBSERVER} - rows, curves = observations(truth, coupled_passive=True) - common = transform([.03, -.02, .6], [.15, -.2, .3]) - normal = common[:3, :3] @ np.array([.1, .99, .1]) - normal /= np.linalg.norm(normal) - for samples in rows.values(): - for row in samples: - row["view_normal_common_xyz"] = normal.tolist() - bias = Rotation.from_euler("xyz", [.06, -.04, .07]) - for name in ("index_pip", "middle_pip"): - ref = Rotation.from_quat(rows[name][0]["relative_quaternion_xyzw"]) - for row in rows[name]: - observed = Rotation.from_quat(row["relative_quaternion_xyzw"]) - delta = observed * ref.inv() - changed = bias * delta * bias.inv() * ref - row["relative_quaternion_xyzw"] = changed.as_quat().tolist() - parent = Rotation.from_quat(row["parent_pose_common"]["quaternion_xyzw"]) - row["child_pose_common"]["quaternion_xyzw"] = (parent * changed).as_quat().tolist() - result = solve_full_hand_zero(SOURCE_URDF, rows, curves) - assert result.passed - assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) - - -def test_rotation_only_data_cannot_be_published_as_full_spatial_calibration(): - from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.fitting import fit_o12_session - with pytest.raises(ValueError, match="requires identified common-frame pose observations"): - fit_o12_session(SOURCE_URDF, _synthetic_records(), require_full_hand_spatial_zero=True) - - -def test_small_uncertain_zero_is_kept_only_when_frozen_zero_passes_holdout(): - truth = {name: .04 for name in ZERO_OBSERVER} - rows, curves = observations({**truth, "index_mcp_roll": .003}) - for cycle, value in enumerate((.001, .005, .003, 0.)): - changed, _ = observations({**truth, "index_mcp_roll": value}) - for name in rows: - rows[name] = [r for r in rows[name] if r["cycle"] != cycle] + [ - r for r in changed[name] if r["cycle"] == cycle] - result = solve_full_hand_zero(SOURCE_URDF, rows, curves) - assert result.passed - assert result.direct_offsets_rad["index_mcp_roll"] == 0. - assert result.validation_error_by_joint_rad["index_mcp_pitch"] < 1.e-5 - changed, _ = observations({**truth, "index_mcp_roll": .15}) - for name in rows: - rows[name] = [r for r in rows[name] if r["cycle"] != 3] + [ - r for r in changed[name] if r["cycle"] == 3] - with pytest.raises(O12SpatialZeroError): - solve_full_hand_zero(SOURCE_URDF, rows, curves) - - -def test_independent_fourth_cycle_cannot_define_or_hide_a_bad_zero(): - truth = {name: .04 for name in ZERO_OBSERVER} - rows, curves = observations(truth) - moved, _ = observations({**truth, "index_pip": .18}) - for name in rows: - rows[name] = [r for r in rows[name] if r["cycle"] < 3] + [r for r in moved[name] if r["cycle"] == 3] - with pytest.raises(O12SpatialZeroError, match="spatial zero solve failed") as error: - solve_full_hand_zero(SOURCE_URDF, rows, curves) - assert error.value.diagnostics["passed"] is False - - - - -def test_failed_spatial_solve_writes_evidence_without_replacing_published_result(monkeypatch, tmp_path): - from linkerhand_calibration.runtime.artifacts import finalization - import json - previous = tmp_path / "previous" - previous.mkdir() - pointer = tmp_path / "latest_passed" - pointer.symlink_to(previous, target_is_directory=True) - diagnostic = {"passed": False, "stage": "axis_observation", "joint": "thumb_mcp"} - def fail(*args, **kwargs): - raise O12SpatialZeroError("unreliable geometry", diagnostic) - # This test isolates failure after the image-evidence boundary; frozen - # branch acceptance and rejection are exercised with actual candidates. - monkeypatch.setattr(finalization, "resolve_chain_observations", - lambda rows, **kwargs: (rows, {"status": "resolved"})) - monkeypatch.setattr(finalization, "fit_profile_calibration", fail) - session = tmp_path / "new" - with pytest.raises(O12SpatialZeroError): - finalization.finalize_profile_session(profile=build_typed_profile(), session_dir=session, serial_number="FULL", - source_urdf=SOURCE_URDF, protected_inputs={}, records=[], publish=True) - assert pointer.resolve() == previous - assert not list(session.glob("*.urdf")) - report = json.loads((session / "fit_diagnostics.json").read_text()) - assert not report["passed"] and not report["publication_allowed"] - assert "unreliable geometry" in report["reason"] diff --git a/src/linkerhand_calibration/test/test_o12_observation_resolution.py b/src/linkerhand_calibration/test/test_o12_observation_resolution.py deleted file mode 100644 index 67a30c2..0000000 --- a/src/linkerhand_calibration/test/test_o12_observation_resolution.py +++ /dev/null @@ -1,131 +0,0 @@ -import copy -from pathlib import Path -import numpy as np -import pytest -from scipy.spatial.transform import Rotation as R - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12 import observations as module -from linkerhand_calibration.pnp import SquareTagPose - -SOURCE_URDF = Path(__file__).resolve().parents[1]/"urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" - -def build_data(monkeypatch, *, wrong_holdout=False): - """Ambiguous initial poses: the wrong distal branch has lower image error.""" - rows=[]; pose_lookup={}; idx=0 - camera=R.from_euler('xyz',[.2,-.3,.4]) - mounts=[R.from_euler('xyz',angles) for angles in ([.1,.3,.2],[-.3,.5,-.4],[.2,.6,-.7])] - def payload(rotation,t,error): - return dict(quaternion_xyzw=(camera*rotation).as_quat().tolist(), - translation_xyz_m=(camera.apply(t)+[0,0,1]).tolist(),reprojection_error_px=error) - for cycle in range(4): - for theta in np.linspace(0,.9,24): - idx+=1 - driver=R.from_euler('z',theta) - good=[payload(mounts[0],[0,0,0],.05), - payload(driver*mounts[1],driver.apply([.03,0,0]),.05), - payload(R.from_euler('z',2*theta)*mounts[2],driver.apply([.04,0,0])+R.from_euler('z',2*theta).apply([.02,0,0]),.08)] - bad=payload(R.from_euler('x',2*theta)*R.from_euler('y',.8)*mounts[2],[0,0,0],.01) - bad['translation_xyz_m']=good[2]['translation_xyz_m'] - # Holdout is allowed to disagree; it may never choose the training branch. - if wrong_holdout and cycle==3: - good[2]['quaternion_xyzw']=bad['quaternion_xyzw'] - evidence={'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','cycle':cycle,'direction':'decreasing','attempt':1, - 'image_stamp_ns':idx,'camera_matrix':np.eye(3).tolist(), - 'camera_matrix_source':module.MATRIX_SOURCE,'tag_size_m':.016,'roles':{}} - for j,role in enumerate(module.THUMB_ROLES): - marker=idx*10+j - candidates=[good[j],good[j]] if j<2 else [bad,good[j]] - pose_lookup[marker]=[SquareTagPose(tuple(p['quaternion_xyzw']),tuple(p['translation_xyz_m']),p['reprojection_error_px']) for p in candidates] - evidence['roles'][role]={'corners_xy':[[marker,0]]*4,'selected':candidates[0], - 'maximum_reprojection_error_px':1.5} - rows.append(evidence) - for j,name in enumerate(('thumb_mcp','thumb_dip')): - pa=evidence['roles'][module.THUMB_ROLES[j]]['selected'] - ch=evidence['roles'][module.THUMB_ROLES[j+1]]['selected'] - q,t=module._relative(pa,ch) - rows.append({'kind':'o12_joint_sample','task_name':'thumb_mcp_dip_front', - 'joint':name,'cycle':cycle,'direction':'decreasing','attempt':1, - 'image_stamp_ns':idx,'parent_pose_common':pa,'child_pose_common':ch, - 'relative_quaternion_xyzw':q.tolist(),'relative_translation_xyz_m':t.tolist()}) - monkeypatch.setattr(module,'solve_square_tag_ippe',lambda xy,**kw:pose_lookup[xy[0][0]]) - return rows - - -def test_complete_training_can_reconsider_wrong_initial_branch(monkeypatch): - rows=build_data(monkeypatch);original=copy.deepcopy(rows) - resolved,report=module.resolve_thumb_observations(rows) - assert rows==original - assert report['status']=='resolved' and report['selected_branches'][2]==1 - assert report['training_frames']==72 and report['holdout_frames']==24 - assert report['changed_joint_rows']>0 - assert resolved!=rows - - -def test_holdout_cannot_change_training_branch_or_scores(monkeypatch): - _,a=module.resolve_thumb_observations(build_data(monkeypatch)) - _,b=module.resolve_thumb_observations(build_data(monkeypatch,wrong_holdout=True)) - assert a['selected_branches']==b['selected_branches'] - assert a['hypotheses']==b['hypotheses'] - - -def test_legacy_projection_is_not_silently_relabelled(monkeypatch): - rows=build_data(monkeypatch) - for r in rows:r.pop('camera_matrix_source',None) - result,report=module.resolve_thumb_observations(rows) - assert report['status']=='legacy_projection_unverified' and result==rows - assert module.resolve_thumb_observations([])[1]['status']=='legacy_no_corners' - - -def test_missing_evidence_cannot_be_silently_dropped(monkeypatch): - rows=build_data(monkeypatch) - rows=[r for r in rows if not(r['kind']=='o12_pnp_candidate_frame' and r['image_stamp_ns']==20)] - with pytest.raises(ValueError,match='lacks evidence'):module.resolve_thumb_observations(rows) - - -def test_other_task_evidence_is_not_mixed_into_thumb(monkeypatch): - rows=build_data(monkeypatch) - rows.append({'kind':'o12_pnp_candidate_frame','task_name':'pinky_chain_side','image_stamp_ns':1}) - assert module.resolve_thumb_observations(rows)[1]['status']=='resolved' - - -def test_partial_external_projection_cannot_finalize_whole_hand(tmp_path): - from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.pipeline import finalize_o12_session - with pytest.raises(ValueError,match='diagnostic-only'): - finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf=SOURCE_URDF, - protected_inputs={},records=[{'projection_reprocessing_scope':'thumb_only_external_projection'}],publish=False) - - -def test_known_unverified_projection_cannot_publish_even_if_fit_passes(monkeypatch,tmp_path): - from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12 import pipeline - from linkerhand_calibration.runtime.artifacts import finalization - def must_not_fit(*args, **kwargs): - raise AssertionError('unverified projection must be rejected before fitting') - monkeypatch.setattr(finalization,'fit_profile_calibration',must_not_fit) - with pytest.raises(ValueError, match='camera_projection_unverified'): - pipeline.finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf=SOURCE_URDF, - protected_inputs={},records=[{'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','image_stamp_ns':1}]) - assert not (tmp_path/'latest_passed').exists() - - -def test_frozen_joint_zero_is_never_reselected_from_training_or_holdout(monkeypatch): - rows = build_data(monkeypatch) - zero = dict(next(r for r in rows if 'joint' in r), kind='joint_zero_sample', - sample_phase='joint_zero', image_stamp_ns=10000, direction=None) - result, report = module.resolve_thumb_observations([zero]+rows) - assert result[0] == zero - assert report['training_frames'] == 72 and report['holdout_frames'] == 24 - - -def test_only_declared_fixed_roles_may_use_a_cached_pose(monkeypatch): - from linkerhand_calibration.core.geometry.candidate_selection import resolve_chain_observations - rows = build_data(monkeypatch) - role = module.THUMB_ROLES[0] - for row in rows: - if 'roles' in row: - row['roles'][role].update(locked_reference=True, corners_xy=None) - kwargs = dict(task_key='thumb_mcp_dip_front', - role_pairs={'thumb_mcp': module.THUMB_ROLES[:2], 'thumb_dip': module.THUMB_ROLES[1:]}, - solve_pose=module.solve_square_tag_ippe) - with pytest.raises(ValueError, match='cached fixed reference'): - resolve_chain_observations(rows, **kwargs) - assert resolve_chain_observations(rows, fixed_roles=(role,), **kwargs)[1]['status'] == 'resolved' diff --git a/src/linkerhand_calibration/test/test_o12_readonly_bridge.py b/src/linkerhand_calibration/test/test_o12_readonly_bridge.py deleted file mode 100644 index 30b362a..0000000 --- a/src/linkerhand_calibration/test/test_o12_readonly_bridge.py +++ /dev/null @@ -1,37 +0,0 @@ -"""Transport tests never construct a hand or open HCAN.""" - -from types import SimpleNamespace - -import pytest - -from linkerhand_calibration.runtime.adapters.o12_bridge import ERROR_FIELDS, read_health - - -def test_hardware_health_is_read_only_and_distinguishes_historical_bit(): - reports = [SimpleNamespace(**{name: False for name in ERROR_FIELDS}) for _ in range(12)] - reports[1].commu_except = True - reads = [] - hand = SimpleNamespace( - get_all_control_modes=lambda: reads.append("mode") or [0]*12, - get_all_error_reports=lambda: reads.append("errors") or reports) - value = read_health(hand) - assert reads == ["mode", "errors"] - assert value["position_mode"] - assert value["communication_latches"] == [1] - assert not value["active_faults"] - reports[2].overheat = True - assert read_health(hand)["active_faults"] == ["channel=2:overheat"] - - -def test_partial_report_is_not_treated_as_good_health(): - hand = SimpleNamespace(get_all_control_modes=lambda: [0]*12, get_all_error_reports=lambda: []) - with pytest.raises(ValueError, match="incomplete"): - read_health(hand) - - -def test_vendor_package_is_pinned_before_any_import(tmp_path): - from linkerhand_calibration.runtime.adapters.vendor_package import load_vendor_sdk - path = tmp_path/"untrusted.whl" - path.write_bytes(b"not a wheel") - with pytest.raises(ValueError, match="changed"): - load_vendor_sdk(path, "0"*64) diff --git a/src/linkerhand_calibration/test/test_o12_recorded_replay.py b/src/linkerhand_calibration/test/test_o12_recorded_replay.py deleted file mode 100644 index e743258..0000000 --- a/src/linkerhand_calibration/test/test_o12_recorded_replay.py +++ /dev/null @@ -1,49 +0,0 @@ -"""Optional local-data replay: deterministic recovery, NOT physical certification. - -Set O12_REPLAY_RAW and O12_REPLAY_REFERENCE to use a captured session and an -independently reviewed model. The test never publishes or launches hardware. -""" - -import os -from pathlib import Path - -import pytest - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.pipeline import finalize_o12_session, load_o12_raw_samples -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.zero import O12SpatialZeroError -from linkerhand_calibration.product import sha256_file -from linkerhand_calibration.storage import atomic_write_json -from linkerhand_calibration.urdf_comparison import compare_urdfs -from test_o12_right_profile import SOURCE_URDF - - -@pytest.mark.skipif(not os.environ.get("O12_REPLAY_RAW") or not os.environ.get("O12_REPLAY_REFERENCE"), - reason="local raw recording/reference not supplied") -def test_recorded_replay_matches_reviewed_geometry_without_overriding_quality(tmp_path): - raw = Path(os.environ["O12_REPLAY_RAW"]).resolve(strict=True) - reference = Path(os.environ["O12_REPLAY_REFERENCE"]).resolve(strict=True) - output = Path(os.environ.get("O12_REPLAY_OUTPUT", str(tmp_path / "replay"))).resolve() - assert not output.exists(), "use a new output directory" - hashes = {str(p): sha256_file(p) for p in (raw, reference, SOURCE_URDF)} - try: - _, fit, correction = finalize_o12_session( - session_dir=output, serial_number="O12_RIGHT_001", source_urdf=SOURCE_URDF, - protected_inputs=hashes, records=load_o12_raw_samples(raw), publish=False, - ) - candidate = correction.path - assert fit.full_hand_zero_result.passed - except O12SpatialZeroError as error: - assert error.review_fit is not None - assert not error.review_fit.full_hand_zero_result.passed - assert error.review_fit.full_hand_zero_result.failure_reasons - candidate = Path(error.diagnostics["review_urdf"]) - assert "REVIEW_ONLY" in candidate.name - assert not list(output.glob("*calibration.json")) - report = compare_urdfs(reference, candidate) - atomic_write_json(output / "reference_geometry_comparison.json", report) - assert report["maximum_origin_distance_mm"] < 1.e-5 - assert report["maximum_orientation_difference_deg"] < 1.e-5 - assert report["link_elements_identical"] - for ranges in report["active_joint_ranges_rad"].values(): - assert ranges["reference"] == pytest.approx(ranges["candidate"], abs=1.e-10) - assert all(sha256_file(p) == digest for p, digest in hashes.items()) diff --git a/src/linkerhand_calibration/test/test_o12_right_profile.py b/src/linkerhand_calibration/test/test_o12_right_profile.py deleted file mode 100644 index 38dff29..0000000 --- a/src/linkerhand_calibration/test/test_o12_right_profile.py +++ /dev/null @@ -1,465 +0,0 @@ -from __future__ import annotations - -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.health import ( - assess_o12_error_report, - decoded_faults, - historical_communication_latch_confirmed, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.motion import ( - cosine_position_trajectory_rad, - cosine_ramp_velocity_trajectory_rad, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.kinematics import ( - evaluate_vendor_passive_joint, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.runner import ( - _log_exception_summary, - render_o12_progress_zh, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.runner import _launch_command -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import ( - CALIBRATED_ACTIVE_JOINTS, - COMMAND_NAMES, - EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION, - ENDPOINT_ANCHOR_BY_JOINT, - FEEDBACK_DOMAIN_MARGIN_RAD, - FEEDBACK_LOWER_RAD, - FEEDBACK_UPPER_RAD, - FORMAL_SPEED_CAP_RAD_S, - INITIAL_FEEDBACK_SPAN_FRACTION, - MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, - MEASURED_PASSIVE_JOINTS, - NORMALIZED_SWEEP_BIN_COUNT, - OUTER_CLEARANCE_FRACTION, - PARK_FINGER_RAD, - PARK_MIDDLE_MCP_RAD, - PARK_MIDDLE_PIP_RAD, - PARK_PINKY_MCP_RAD, - PARK_RING_MCP_RAD, - ROLL_CROSS_VIEW_BY_TASK, - SAFE_UPPER_RAD, - SDK_TO_URDF_JOINT, - build_typed_profile, -) - - - - -ROOT = Path(__file__).resolve().parents[1] -SOURCE_URDF = ROOT / "urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" - - -def _synthetic_records(): - profile = build_typed_profile() - task_by_joint = { - joint: task for task in profile.motion.tasks for joint in task.joints - } - ratios = { - "thumb_dip": 1.01, - "index_dip": 0.88, - "middle_dip": 0.77, - "pinky_pip": 1.11, - "pinky_dip": 0.95, - } - source = { - "thumb_dip": "thumb_mcp", - "index_dip": "index_pip", - "middle_dip": "middle_pip", - "pinky_pip": "pinky_mcp_pitch", - "pinky_dip": "pinky_pip", - } - travel: dict[str, float] = {} - for name in CALIBRATED_ACTIVE_JOINTS: - task = task_by_joint[name] - travel[name] = abs(task.end_value - task.start_value) - # SDK coordinates and physical URDF joint travel are not interchangeable - # on O12. Use the reviewed CAD/mechanical travel in this fixture, just as - # the real Tag curves identify it independently of the commanded range. - travel.update({ - "thumb_cmc_roll": 0.720, - "thumb_cmc_yaw": 0.987, - "thumb_cmc_pitch": 0.588, - "thumb_mcp": 1.184, - "index_mcp_pitch": 1.290, - "index_pip": 1.635, - "middle_mcp_pitch": 1.318, - "middle_pip": 1.604, - "pinky_mcp_pitch": 1.472, - }) - travel["pinky_pip"] = ratios["pinky_pip"] * travel["pinky_mcp_pitch"] - travel["pinky_dip"] = ratios["pinky_dip"] * travel["pinky_pip"] - for name in ("thumb_dip", "index_dip", "middle_dip"): - travel[name] = ratios[name] * travel[source[name]] - result = {name: [] for name in CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS} - for name, rows in result.items(): - task = task_by_joint[name] - for cycle in range(4): - for direction, phases in ( - ("decreasing", np.linspace(0.0, 1.0, 65)), - ("increasing", np.linspace(1.0, 0.0, 65)), - ): - for sample_index, phase in enumerate(phases): - feedback = task.start_value + phase * (task.end_value - task.start_value) - angle = phase * travel[name] - rows.append({ - "sample_id": f"{task.key}:{cycle}:{direction}:{sample_index}", - "cycle": cycle, - "direction": direction, - "feedback_rad": float(feedback), - "relative_quaternion_xyzw": Rotation.from_rotvec( - [0.0, angle, 0.0] - ).as_quat().tolist(), - }) - return result - - -def test_o12_fixed_mapping_tags_and_radian_domain() -> None: - profile = build_typed_profile() - assert profile.key.profile_id == "O12/right/o12_right_16/v1" - assert profile.command.unit == "rad" - assert profile.command.feedback_by_index is True - assert profile.command.names == COMMAND_NAMES - assert SDK_TO_URDF_JOINT[:4] == ( - "thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp" - ) - assert profile.vision.tag_ids == frozenset(range(16)) - assert len(profile.motion.tasks) == 11 - assert profile.motion.speed_parameters["command_rate_hz"] == 50.0 - assert NORMALIZED_SWEEP_BIN_COUNT == 64 - assert OUTER_CLEARANCE_FRACTION == pytest.approx(1.0) - assert PARK_RING_MCP_RAD == pytest.approx(SAFE_UPPER_RAD[10]) - assert PARK_PINKY_MCP_RAD == pytest.approx(SAFE_UPPER_RAD[11]) - assert PARK_FINGER_RAD == pytest.approx(PARK_PINKY_MCP_RAD) - assert PARK_MIDDLE_MCP_RAD == pytest.approx(SAFE_UPPER_RAD[8]) - assert PARK_MIDDLE_PIP_RAD == pytest.approx(SAFE_UPPER_RAD[9]) - assert INITIAL_FEEDBACK_SPAN_FRACTION == pytest.approx(0.99) - assert EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION == pytest.approx(0.90) - assert MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX == pytest.approx(2.0) - assert profile.vision.extrinsics_quality_limits[ - "reprojection_rms_px" - ] == pytest.approx(2.0) - assert FORMAL_SPEED_CAP_RAD_S[4] == pytest.approx(0.12) - assert FORMAL_SPEED_CAP_RAD_S[7] == pytest.approx(0.12) - assert ENDPOINT_ANCHOR_BY_JOINT == {} - assert [task.formal_speed for task in profile.motion.tasks] == [ - 0.10, 0.16, 0.32, 0.16, 0.32, 0.12, 0.32, 0.32, 0.12, 0.32, 0.32 - ] - index_mcp_pitch = profile.measurement.measurements["index_mcp_pitch"] - assert ( - index_mcp_pitch.view, - index_mcp_pitch.parent_role, - index_mcp_pitch.child_role, - ) == ("side", "side_base", "index_pip") - side_tag_ids = { - tag.role: tag.tag_id - for view in profile.vision.views - if view.name == "side" - for tag in view.tags - } - assert side_tag_ids["side_base"] == 4 - assert side_tag_ids["index_pip"] == 10 - assert side_tag_ids["index_dip"] == 11 - assert { - key: (value.view, value.parent_role, value.child_role) - for key, value in ROLL_CROSS_VIEW_BY_TASK.items() - } == { - "middle_roll_front": ("side", "side_base", "middle_pip"), - "index_roll_front": ("side", "side_base", "index_pip"), - } - - -def test_o12_cosine_speed_is_bounded() -> None: - speed = 0.03 - _, _, duration = cosine_position_trajectory_rad(0.0, -0.8, 0.0, speed) - samples = np.asarray([ - cosine_position_trajectory_rad(0.0, -0.8, t, speed)[0] - for t in np.linspace(0.0, duration, 2001) - ]) - measured = np.max(np.abs(np.diff(samples))) / (duration / 2000.0) - assert measured <= speed + 1.0e-6 - - -def test_o12_fast_trajectory_has_bounded_speed_and_zero_endpoint_velocity() -> None: - speed = 0.10 - _, _, old_duration = cosine_position_trajectory_rad( - 0.0, -0.827286065, 0.0, speed - ) - _, _, duration = cosine_ramp_velocity_trajectory_rad( - 0.0, -0.827286065, 0.0, speed, 0.4 - ) - times = np.linspace(0.0, duration, 5001) - samples = np.asarray([ - cosine_ramp_velocity_trajectory_rad( - 0.0, -0.827286065, t, speed, 0.4 - )[0] - for t in times - ]) - velocity = np.diff(samples) / np.diff(times) - assert duration < 0.70 * old_duration - assert np.max(np.abs(velocity)) <= speed + 1.0e-6 - assert abs(velocity[0]) < 1.0e-4 - assert abs(velocity[-1]) < 1.0e-4 - - -def test_o12_hcan_launch_does_not_emit_empty_socketcan_argument(tmp_path: Path) -> None: - profile = build_typed_profile() - config = SimpleNamespace( - model="O12", side="right", tag_layout="o12_right_16", - serial_number="O12_TEST", can_interface="", source_urdf=SOURCE_URDF, - source_urdf_sha256="a" * 64, - camera_extrinsics=tmp_path / "extrinsics.yaml", - camera_extrinsics_sha256="b" * 64, - calibration_config=tmp_path / "calibration.yaml", - calibration_config_sha256="c" * 64, - tag_config=tmp_path / "tags.yaml", tag_config_sha256="d" * 64, - sdk_config=tmp_path / "sdk.yaml", sdk_config_sha256="e" * 64, - output_root=tmp_path, cameras={ - view: { - "serial_number": f"camera-{view}", - "camera_name": f"camera_{view}", - "camera_info": str(tmp_path / f"{view}.yaml"), - } - for view in ("front", "side", "top") - }, - calibration_contract=SimpleNamespace(typed_profile=profile), - ) - command = _launch_command( - config, tmp_path / "session", record_bag=False, commands_enabled=False - ) - assert not any(item.startswith("can_interface:=") for item in command) - - resumed = _launch_command( - config, - tmp_path / "new_session", - record_bag=False, - commands_enabled=False, - resume_from=tmp_path / "old_session", - ) - assert ( - f"resume_raw_samples_path:={tmp_path / 'old_session/raw_samples.jsonl'}" - in resumed - ) - - -def test_o12_vendor_node_is_launched_in_documented_namespace() -> None: - launch_text = (ROOT / "launch/three_camera_calibration.launch.py").read_text() - assert 'executable="o12_sdk_bridge"' in launch_text - assert 'sdk_package_expected_sha256' in launch_text - from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics - topics = sdk_topics(build_typed_profile()) - assert topics.command == "/o12/right/joint_cmd" - assert topics.feedback == "/o12/right/joint_states" - - -def test_o12_vendor_stream_is_not_artificially_throttled() -> None: - import hashlib - import yaml - from linkerhand_calibration.compat.paths import resolve_renamed_package_path - - product = yaml.safe_load((ROOT/"config/o12_right_product.yaml").read_text()) - sdk_path = resolve_renamed_package_path(product["sdk"]["config"], ROOT.parent.parent) - assert sdk_path == ROOT/"config/o12_sdk.yaml" - assert hashlib.sha256(sdk_path.read_bytes()).hexdigest() == product["sdk"]["config_sha256"] - parameters = yaml.safe_load(sdk_path.read_text())["/**"]["ros__parameters"] - assert "left_hand" not in parameters - assert parameters["right_hand"]["request_interval_ms"] == 0 - assert parameters["right_hand"]["connection_type"] == product["sdk"]["transport"] - - -def test_o12_error_health_separates_active_faults_from_historical_comm_latch( -) -> None: - communication = assess_o12_error_report([0, 16, *([0] * 10)]) - assert communication.communication_only - assert communication.communication_channels == (1,) - assert not communication.active_fault_channels - assert historical_communication_latch_confirmed( - assessment=communication, - matching_report_count=3, - observation_seconds=2.0, - feedback_hz=49.0, - feedback_age_seconds=0.01, - minimum_feedback_hz=15.0, - ) - assert not historical_communication_latch_confirmed( - assessment=communication, - matching_report_count=3, - observation_seconds=2.0, - feedback_hz=49.0, - feedback_age_seconds=1.1, - minimum_feedback_hz=15.0, - ) - - active = assess_o12_error_report([0, 16 | 2, *([0] * 10)]) - assert not active.communication_only - assert active.active_fault_channels == (1,) - assert decoded_faults(active.codes)[0]["flags"] == ( - "overheat", "commu_except" - ) - - -def test_o12_log_exception_summary_reports_child_traceback( - tmp_path: Path, -) -> None: - log_path = tmp_path / "calibration.log" - log_path.write_text( - "Traceback (most recent call last):\n" - "[node-1] AttributeError: missing motion metadata\n" - "[ERROR] [node-1]: process has died\n", - encoding="utf-8", - ) - assert _log_exception_summary(log_path) == ( - "AttributeError: missing motion metadata" - ) - - - - - - - - - - - - - - -def test_o12_feedback_domain_contains_but_does_not_expand_command_domain() -> None: - command = build_typed_profile().command - assert command.minimum_values[2] == pytest.approx(-0.8272860654453121) - assert command.minimum_feedback_values == pytest.approx(FEEDBACK_LOWER_RAD) - assert command.maximum_feedback_values == pytest.approx(FEEDBACK_UPPER_RAD) - assert command.minimum_feedback_values[2] == pytest.approx( - command.minimum_values[2] - FEEDBACK_DOMAIN_MARGIN_RAD - ) - assert all( - feedback <= commanded - for feedback, commanded in zip( - command.minimum_feedback_values, command.minimum_values - ) - ) - assert all( - feedback >= commanded - for feedback, commanded in zip( - command.maximum_feedback_values, command.maximum_values - ) - ) - - -def test_o12_operator_progress_uses_unified_layout_and_radians() -> None: - text = render_o12_progress_zh({ - "state": "RUNNING", - "serial_number": "O12_TEST", - "step_index": 20, - "step_count": 190, - "step_fraction": 0.5, - "task_name": "thumb_pitch_front", - "phase": "sweep", - "cycle": 1, - "direction": "decreasing", - "attempt": 1, - "command_unit": "rad", - "target_rad": -0.827, - "current_command_u8": -0.400, - "actual_u8": -0.395, - "speed_rad_s": 0.060, - "command_trajectory_duration_seconds": 21.7, - "valid_frames": 80, - "total_frames": 82, - "tag_detection_rate": 80 / 82, - "joint_frame_rate": 80 / 82, - "recognized_tag_ids": [0, 1], - "unrecognized_tag_ids": [], - "feedback_hz": 20.0, - "position_mode_verified": True, - "error_report_verified": True, - "temperature_fallback_active": True, - "motion_speed_scale": 2.0, - }) - assert "[O12_TEST] 标定中" in text - assert "拇指 CMC pitch（正面 ID0→ID1）" in text - assert "阶段：正式扫描（第 2/4 轮）" in text - assert "命令/反馈：-0.400 rad/-0.395 rad" in text - assert "峰值 0.060 rad/s；本段 21.7 秒平滑限速轨迹" in text - assert "温度策略=错误码 bit1 过热保护；速度倍率=2.0x" in text - - roll_status = { - "state": "RUNNING", "serial_number": "O12_TEST", - "step_index": 50, "step_count": 190, "step_fraction": 0.2, - "task_name": "middle_roll_front", "phase": "sweep", - "cycle": 0, "direction": "decreasing", "attempt": 1, - "command_unit": "rad", "target_rad": -0.26, - "current_command_u8": 0.1, "actual_u8": 0.09, - "speed_rad_s": 0.12, "valid_frames": 50, - "total_frames": 50, "tag_detection_rate": 1.0, - "joint_frame_rate": 1.0, "recognized_tag_ids": [0, 12], - "unrecognized_tag_ids": [], - "roll_cross_view": { - "active": True, "recognized_tag_ids": [4, 8], - "unrecognized_tag_ids": [], "valid_frames": 48, - "total_frames": 50, "joint_frame_rate": 0.96, - }, - } - roll_text = render_o12_progress_zh(roll_status) - assert "正面 ID0→ID12 + 侧面 ID4→ID8" in roll_text - assert "侧摆双机位：侧面已识别 ID4/ID8" in roll_text - - roll_status["locked_reference_tag_ids"] = [0] - locked_text = render_o12_progress_zh(roll_status) - assert "固定基准：ID0 已锁定，避让遮挡期间复用" in locked_text - - coupling_status = dict(roll_status) - coupling_status["feedback_coupling"] = { - "active": True, - "coupled_channel": "middle_pip", - "reference_kind": "training_observation", - "displacement_rad": 0.028, - "residual_rad": 0.004, - "hard_displacement_limit_rad": 0.0855, - } - coupling_text = render_o12_progress_zh(coupling_status) - assert ( - "O12耦合：middle_pip 位移 0.028/0.086 rad；" - "训练采集；vendor偏差 0.004 rad（仅诊断）" - ) in coupling_text - - - - -def test_o12_vendor_passive_solver_regression() -> None: - assert evaluate_vendor_passive_joint( - "thumb_dip", -1.29, sdk_to_urdf_sign=-1.0 - ) == pytest.approx(1.094445515, abs=1.0e-9) - assert evaluate_vendor_passive_joint( - "pinky_pip", 1.54, sdk_to_urdf_sign=1.0 - ) == pytest.approx(1.360545431, abs=1.0e-9) - assert evaluate_vendor_passive_joint( - "pinky_dip", 1.54, sdk_to_urdf_sign=1.0 - ) == pytest.approx(1.291363184, abs=1.0e-9) - - - - - - - - - - - - - - - - -def test_immutable_o12_cad_ring_mimic_conflict_is_not_waived(): - from linkerhand_calibration.core.urdf.patch import validate_urdf_mimic_ranges - with pytest.raises(ValueError, match="ring_pip"): - validate_urdf_mimic_ranges(SOURCE_URDF) diff --git a/src/linkerhand_calibration/test/test_o12_thumb_pnp.py b/src/linkerhand_calibration/test/test_o12_thumb_pnp.py deleted file mode 100644 index 9d95cef..0000000 --- a/src/linkerhand_calibration/test/test_o12_thumb_pnp.py +++ /dev/null @@ -1,61 +0,0 @@ -import pytest -from scipy.spatial.transform import Rotation as R - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.pnp import O12ThumbPoseTracker, THUMB_ROLES -from linkerhand_calibration.pnp import SquareTagPose - - -def pose(rotation, error=0.05): - return SquareTagPose(tuple(rotation.as_quat()), (0., 0., 1.), error) - - -@pytest.mark.parametrize('remount', [False, True]) -@pytest.mark.parametrize('passive_travel', [12, 28, 45]) -def test_parallel_branch_disambiguation_without_angle_ratio(remount, passive_travel): - tracker=O12ThumbPoseTracker() - mounts=[R.identity()]*3 if not remount else [ - R.from_euler('xyz', angles) for angles in - ([.3,-.7,.2],[-.5,.4,.8],[.6,.2,-.4])] - camera=R.from_euler('xyz',[.4,-.8,.7]) - baseline={role:(pose(camera*mount),) for role,mount in zip(THUMB_ROLES,mounts)} - for i in range(8): - selected,reason=tracker.select(baseline,stamp_ns=1_000_000_000+i*30_000_000) - assert selected is not None - driver=R.from_euler('z',20,degrees=True) - follower=R.from_euler('z',passive_travel,degrees=True) - wrong=R.from_euler('x',passive_travel,degrees=True) - good_pose=pose(camera*driver*follower*mounts[2],.08) - candidates={THUMB_ROLES[0]:baseline[THUMB_ROLES[0]], - THUMB_ROLES[1]:(pose(camera*driver*mounts[1]),), - THUMB_ROLES[2]:(pose(camera*driver*wrong*mounts[2]),good_pose)} - selected,reason=tracker.select(candidates,stamp_ns=1_250_000_000) - assert reason == '' - assert selected[THUMB_ROLES[2]] == good_pose - # Angles are observed candidates, not replaced with driver*mimic. - assert tracker.coupled_rotation_pairs == () - - -def test_no_credible_parallel_candidate_does_not_stop_or_invent_pose(): - tracker=O12ThumbPoseTracker() - baseline={role:(pose(R.identity()),) for role in THUMB_ROLES} - for i in range(8): - tracker.select(baseline,stamp_ns=i*30_000_000) - bad=pose(R.from_euler('zx',[20,45],degrees=True)) - selected,reason=tracker.select({THUMB_ROLES[0]:baseline[THUMB_ROLES[0]], - THUMB_ROLES[1]:(pose(R.from_euler('z',20,degrees=True)),), - THUMB_ROLES[2]:(bad,)},stamp_ns=250_000_000) - assert reason == '' - assert selected[THUMB_ROLES[2]] == bad - tracker.reset() - assert tracker.reference is None - - -def test_missing_tag_discards_observation_and_gap_reinitializes(): - tracker=O12ThumbPoseTracker() - baseline={role:(pose(R.identity()),) for role in THUMB_ROLES} - for i in range(8): - tracker.select(baseline,stamp_ns=i*30_000_000) - selected,reason=tracker.select({},stamp_ns=250_000_000) - assert selected is None and reason == 'group_missing_pose_candidates' - selected,_=tracker.select(baseline,stamp_ns=10_000_000_000) - assert selected is None and tracker.reference is None diff --git a/src/linkerhand_calibration/test/test_o30_artifacts.py b/src/linkerhand_calibration/test/test_o30_artifacts.py deleted file mode 100644 index fa41887..0000000 --- a/src/linkerhand_calibration/test/test_o30_artifacts.py +++ /dev/null @@ -1,124 +0,0 @@ -"""Twenty independently observed active channels through JSON and URDF read-back.""" - -from dataclasses import replace -import hashlib -import json -import xml.etree.ElementTree as ET - -import pytest - -from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session -from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json - - -def test_o30_nonlinear_twenty_channel_finalization(tmp_path, o30_capture): - from o30_formal_fixture import formal_capture - profile, source, records, hashes, camera_file, offsets = formal_capture(o30_capture, tmp_path, adaptive=True) - source_hash = hashes['source_urdf_sha256'] - result, fit, artifact = finalize_profile_session(profile=profile, source_urdf=source, - records=records, protected_inputs=hashes, serial_number='SYNTHETIC_FORMAL_O30', - session_dir=tmp_path/'session', standard_loader=ET.parse, - require_motion_evidence=True, camera_extrinsics_file=camera_file) - capture = json.loads((tmp_path/'session'/'capture_validation.json').read_text()) - assert capture['passed'] - assert result['capture_plan']['policy'] == 'adaptive_2_to_3' - assert {tuple(task['training_cycles']) for task in result['capture_plan']['tasks'].values()} == {(0, 1), (0, 1, 2)} - assert any(row.get('decision') == 'add_training' for row in records) - assert all(row['cycle'] != 2 for row in records - if row.get('kind') == 'joint_sample' and row['task_name'] == profile.motion.tasks[-1].key) - from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper - mapper = load_unified_mapper(artifact.staged_release.calibration_json) - assert max(abs(v) for v in mapper.map_positions(profile.command.baseline_values)) < 1e-10 - assert len(mapper.map_positions([0]*20)) == 20 - assert result['schema_version'] == 3 - assert len(fit.output_mappings) == len(fit.command_mappings) == 20 - for name, expected in offsets.items(): - assert fit.zero_offsets_rad[name] == pytest.approx(expected, abs=.004), name - for name in profile.zero.cad_zero_assumptions: - assert fit.zero_method_by_joint[name] == 'assumed_source_cad_zero' - original_joints = {j.get('name'): j for j in ET.parse(source).findall('joint')} - corrected = ET.parse(artifact.path) - assert not corrected.findall('.//mimic') - for joint in corrected.findall('joint'): - old = original_joints[joint.get('name')] - assert joint.find('origin').get('xyz') == old.find('origin').get('xyz') - assert joint.find('axis').attrib == old.find('axis').attrib - assert all(joint.find('limit').get(k) == old.find('limit').get(k) for k in ('effort','velocity')) - if joint.get('name') in profile.zero.cad_zero_assumptions: - assert joint.find('origin').attrib == old.find('origin').attrib - rebuilt = rebuild_urdf_from_json(artifact.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() == artifact.path.read_bytes() - assert hashlib.sha256(source.read_bytes()).hexdigest() == source_hash - accepted = json.loads(artifact.staged_release.validation_json) - assert accepted['urdf_rebuilt_from_json'] - assert accepted['final_file_image_holdout_verified'] - assert accepted['release_basis'] == 'steady_command' - assert set(accepted['final_file_image_holdout']) == {'command'} - assert not accepted['feedback_mapping_required'] - report = json.loads((tmp_path/'session'/'calibration_report.json').read_text()) - assert report['quality']['motion_observation'] == profile.acquisition.motion_observation - assert report['quality']['position_feedback_required'] is True - assert all('feedback_to_rad' not in row for row in report['joints'].values()) - assert all(row['feedback_bounds_rad'] is None for row in report['motion_range_evidence'].values()) - - -def test_segmented_offline_acceptance_and_resume_restart(tmp_path, o30_capture): - from linkerhand_calibration.runtime.engine import CalibrationEngine, ACQUISITION_POLICY_VERSION - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - from linkerhand_calibration.core.domain.motion_path import segment_metadata, record_scan_identity - from linkerhand_calibration.runtime.artifacts.replay import validate_capture_units - from linkerhand_calibration.runtime.joint_resume import JointResume - profile, _, rows, _ = o30_capture - engine = CalibrationEngine(profile) - header = dict(kind='session_start',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) - # The synthetic FK fixture is explicitly pose-only; this test exercises - # quality/resume, leaving live image-provenance replay to its own tests. - header_without_motion_claim = dict(header, pose_tracking_policy_version='fixture') - complete = [dict(kind='scan_unit_complete',task_name=u.task_key,cycle=u.cycle, - direction=u.direction,attempt=1,passed=True,**segment_metadata(u.segment_key)) - for u in engine.scan_units()] - validate_capture_units(profile,[*rows,*complete]) - from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit - spans = {} - for unit in engine.scan_units(): - if unit.segment_key and unit.cycle == 0: - assert evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans=spans).passed - middle_spans = {key for key in spans if ':middle_mcp_roll:' in key} - assert len(middle_spans) == 4 # Upper/lower travel in both directions remains independent. - missing = [r for r in complete if not (r['task_name']=='fingers_mcp_roll_front' - and r['cycle']==3 and r.get('segment_key')=='middle_lower_decreasing')] - with pytest.raises(ValueError,match='capture_incomplete'): - validate_capture_units(profile,[*rows,*missing]) - # Use a pre-motion-policy header only for staging these pose-only fixtures; - # production rejects it before reaching this boundary. - from unittest.mock import patch - resume=JointResume(profile) - with patch.object(engine,'resume_compatible',return_value=True): - resume.stage(engine,[header_without_motion_claim,*rows,*missing], - {record_scan_identity(r) for r in missing}) - group=next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - assert not set(group.joints) & resume.references.keys() - assert not any(key[0]==group.key for key in resume.units) - assert len(resume.units)==16*8 - # Legacy feedback-certified profiles still reject an unsupported endpoint. - profile = replace(profile, acquisition=replace(profile.acquisition, motion_observation="feedback"), - measurement=replace(profile.measurement, release_basis="feedback_and_command")) - engine = CalibrationEngine(profile) - resume = JointResume(profile) - # Individual sweeps can pass while the independent endpoint lies outside - # every training sweep. Such a task must restart with a fresh zero. - changed = [dict(row, feedback_u8=3.+247.*row['feedback_u8']/255.) - if row.get('joint')=='thumb_cmc_roll' and row.get('sample_phase')=='sweep' - and row.get('cycle', 99)<3 else row for row in rows] - with patch.object(engine,'resume_compatible',return_value=True): - resume.stage(engine,[header_without_motion_claim,*changed,*complete], - {record_scan_identity(r) for r in complete}) - assert not any(key[0]=='thumb_cmc_roll_front' for key in resume.units) - assert 'thumb_cmc_roll' not in resume.references - assert len(resume.units)==15*8 - assert not engine.resume_compatible(dict(header,capture_schedule_version='unified_schedule_v3_separate_mapping')) diff --git a/src/linkerhand_calibration/test/test_o30_launch.py b/src/linkerhand_calibration/test/test_o30_launch.py deleted file mode 100644 index 1e04694..0000000 --- a/src/linkerhand_calibration/test/test_o30_launch.py +++ /dev/null @@ -1,158 +0,0 @@ -"""Construct protected O30 launch actions using synthetic cameras, without ROS processes.""" - -from pathlib import Path -import hashlib -import yaml -import pytest - -from launch import LaunchContext -from launch.actions import DeclareLaunchArgument -from launch.utilities import perform_substitutions -from launch_ros.actions import Node -from launch_ros.utilities import evaluate_parameters, to_parameters_list -from ros2launch.api.api import parse_launch_arguments - -from linkerhand_calibration.product import load_product_config, _camera_info_fingerprint -from linkerhand_calibration.runtime.runner_support import launch_command -from test_unified_launch import launch_module -from o30_capture_fixture import PACKAGE - - -def synthetic_product(tmp_path): - product=yaml.safe_load((PACKAGE/'config/o30_right_product.yaml').read_text()) - product['profile_config']=str(PACKAGE/'config/profiles/o30_right_18.yaml') - for key in ('source_urdf','calibration_config','tag_config'): - product['artifacts'][key]=str(PACKAGE/product['artifacts'][key].split('package://linkerhand_calibration/')[1]) - extrinsics=dict(schema_version=1,reference_view='front',cameras={},front_from_view={},quality=dict( - passed=True,reprojection_rms_px=.1,maximum_rotation_repeatability_deg=.01, - maximum_translation_repeatability_m=.0001,front_side_captures=15,front_top_captures=15)) - for n,(view,config) in enumerate(product['cameras'].items()): - info=tmp_path/f'{view}.yaml' - info.write_text(yaml.safe_dump(dict(image_width=1280,image_height=960, - camera_matrix=dict(data=[1200,0,640,0,1200,480,0,0,1]), - distortion_coefficients=dict(data=[0.]*5),rectification_matrix=dict(data=[1,0,0,0,1,0,0,0,1]), - projection_matrix=dict(data=[1200,0,640,0,0,1200,480,0,0,0,1,0])))) - config.update(serial_number=f'SYNTHETIC_{view}',camera_info=str(info)) - extrinsics['cameras'][view]=dict(serial_number=config['serial_number'],width=1280,height=960, - intrinsics_sha256=_camera_info_fingerprint(info)) - extrinsics['front_from_view'][view]=dict(translation_xyz_m=[.1*n,0,0],quaternion_xyzw=[0,0,0,1]) - path=tmp_path/'synthetic_extrinsics.yaml';path.write_text(yaml.safe_dump(extrinsics)) - product['artifacts'].update(camera_extrinsics=str(path),camera_extrinsics_sha256=hashlib.sha256(path.read_bytes()).hexdigest()) - path=tmp_path/'product.yaml';path.write_text(yaml.safe_dump(product)) - return load_product_config(path,workspace=PACKAGE.parents[1],check_can=False) - - -def test_o30_real_launch_and_right_hand_settings(tmp_path,monkeypatch): - monkeypatch.setenv('ROS_LOG_DIR',str(tmp_path/'logs')) - config=synthetic_product(tmp_path) - context=LaunchContext() - context.launch_configurations.update(dict(parse_launch_arguments(launch_command(config,tmp_path/'session', - record_bag=False,commands_enabled=False)[4:]))) - module=launch_module() - for action in module.generate_launch_description().entities: - if isinstance(action,DeclareLaunchArgument):action.execute(context) - actions=module._launch_stack(context) - nodes=[a for a in actions if isinstance(a,Node)] - sdk=next(a for a in nodes if a.node_package=='linker_hand_o30_ros2_sdk') - params={p.name:p.value for p in to_parameters_list(context,'linker_hand_o30_sdk','', - evaluate_parameters(context,sdk._Node__parameters))} - assert params['hand_type']=='right' - assert params['comm_type']=='libcanbus' - assert params['canfd_device']==0 and params['state_rate']==30. - assert not params['auto_init_pose'] and not params['is_touch'] - assert not (tmp_path/'session/raw_samples.jsonl').exists() - - -@pytest.mark.parametrize('policy', ['taskwise_2_plus_1', 'fixed', 'staged_2_to_3', 'occlusion_aware_witness_v1']) -def test_session_profile_survives_real_launch_parameter_chain(tmp_path, monkeypatch, policy): - from dataclasses import replace - from linkerhand_calibration.product import ProductCalibrationContract - from linkerhand_calibration.core.domain.capture_plan import CapturePlan, configure_training - from linkerhand_calibration.profiles.preparation import configure_preparation - from linkerhand_calibration.runtime.profile_contract import resolve_runtime_profile - from linkerhand_calibration.runtime.ros.parameters import parameter_defaults - - monkeypatch.setenv('ROS_LOG_DIR', str(tmp_path/'logs')) - config = synthetic_product(tmp_path) - original = config.calibration_contract.typed_profile - profile = configure_training(original, - 'staged_2_to_3' if policy == 'occlusion_aware_witness_v1' else policy) - if policy == 'occlusion_aware_witness_v1': - profile = configure_preparation(profile, policy) - config = replace(config, calibration_contract=ProductCalibrationContract(declarative=profile)) - context = LaunchContext() - context.launch_configurations.update(dict(parse_launch_arguments(launch_command( - config, tmp_path/'session', record_bag=False, commands_enabled=False)[4:]))) - module = launch_module() - for action in module.generate_launch_description().entities: - if isinstance(action, DeclareLaunchArgument): - action.execute(context) - owner, = [action for action in module._launch_stack(context) - if isinstance(action, Node) and action.node_package == 'linkerhand_calibration' - and action.node_executable == 'three_camera_calibration_node'] - effective = parameter_defaults(original) - effective.update({p.name: p.value for p in to_parameters_list(context, 'o30_calibration', '', - evaluate_parameters(context, owner._Node__parameters))}) - restored = resolve_runtime_profile(original, effective.__getitem__) - assert CapturePlan.from_profile(restored).as_dict() == CapturePlan.from_profile(profile).as_dict() - assert restored.motion.preparation_witnesses == profile.motion.preparation_witnesses - assert not (tmp_path/'session/raw_samples.jsonl').exists() - if policy == 'occlusion_aware_witness_v1': - assert len(restored.motion.preparation_witnesses) == 4 - # Reproduce the real omitted-parameter failure, then explicit removal/tampering. - for name, replacement in [('preparation_witnesses_json', ''), - ('preparation_witnesses_json', '{}'), ('training_policy', 'fixed')]: - with pytest.raises(ValueError, match='runtime_capture_plan_mismatch'): - resolve_runtime_profile(original, {**effective, name: replacement}.__getitem__) - - -def test_runtime_profile_preserves_legacy_yaml_and_rejects_malformed_overrides(): - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.profile_contract import resolve_runtime_profile - from linkerhand_calibration.runtime.ros.parameters import parameter_defaults - from linkerhand_calibration.core.domain.capture_plan import CapturePlan - profile = load_bundled_hand_profile('o30_right_18') - defaults = parameter_defaults(profile) - assert CapturePlan.from_profile(resolve_runtime_profile(profile, defaults.__getitem__)) == CapturePlan.from_profile(profile) - for encoded in ['[1]', '{', '{"ring_mcp_pitch": {}}']: - with pytest.raises(ValueError, match='invalid preparation_witnesses_json'): - resolve_runtime_profile(profile, {**defaults, 'preparation_witnesses_json': encoded}.__getitem__) - - -def test_parent_model_preparation_freezes_branch_without_replacing_joint_zero(tmp_path,monkeypatch): - from types import SimpleNamespace - from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from linkerhand_calibration.runtime.branch_initialization import BranchInitialization - from linkerhand_calibration.runtime import coordinator as module - from linkerhand_calibration.profiles import load_bundled_hand_profile - profile=load_bundled_hand_profile('o30_right_18') - # Legacy explicit parent motion remains supported even though the current - # product now declares a stationary parent_reference instead. - from linkerhand_calibration.core.domain.profile import ModelPreparation - target=profile.motion.joint_zero_references['thumb_ip'].command - approach=list(target);approach[6]=255 - preparation=ModelPreparation('thumb_mcp',target,(tuple(approach),)) - motion=MotionCommand('joint_zero',preparation.command,200,'thumb_ip_front', - zero_joints=('thumb_mcp',),reference_only=True,reference_command=preparation.command, - reference_approach=preparation.approach_commands) - initialization=BranchInitialization(profile) - from dataclasses import replace - initialization.begin(replace(motion,phase='zero_approach',zero_joints=(),reference_joints=('thumb_mcp',)), - session_epoch=1,motion_version=1) - assert initialization.reference_spec==(preparation.command,preparation.approach_commands) - assert [(r.joint,r.parent_role,r.child_role) for r in initialization._relations('front')]==[ - ('thumb_mcp','front_base','thumb_mcp')] - original_zero=object(); frozen=[] - host=SimpleNamespace(profile=profile,branch_initialization=SimpleNamespace(ready=lambda _:True), - execution=SimpleNamespace(zero_references={'thumb_mcp':original_zero}), - _steady_rows=[],_freeze_reference_branches=lambda revisions:frozen.append(revisions) or True) - revisions={('front','thumb_mcp'):2,('front','front_base'):1} - monkeypatch.setattr(module,'reference_branch_revisions',lambda *args,**kwargs:revisions) - for stamp in range(9): - host._steady_rows.append(dict(kind='joint_zero_sample',joint='thumb_mcp',view='front',image_stamp_ns=stamp)) - assert not CalibrationCoordinator._confirm_model_reference(host,motion) - host._steady_rows.append(dict(kind='joint_zero_sample',joint='thumb_mcp',view='front',image_stamp_ns=9)) - assert CalibrationCoordinator._confirm_model_reference(host,motion) - assert frozen==[revisions] - assert host.execution.zero_references['thumb_mcp'] is original_zero diff --git a/src/linkerhand_calibration/test/test_o30_profile.py b/src/linkerhand_calibration/test/test_o30_profile.py deleted file mode 100644 index 8ba7b9d..0000000 --- a/src/linkerhand_calibration/test/test_o30_profile.py +++ /dev/null @@ -1,359 +0,0 @@ -"""O30 declarations, SDK boundary and synchronized motion execution.""" - -from dataclasses import replace -from pathlib import Path -from types import SimpleNamespace -import hashlib -import json -import math - -import numpy as np -import pytest -import yaml - -from linkerhand_calibration.profiles import load_bundled_hand_profile, dump_hand_profile, load_hand_profile -from linkerhand_calibration.profiles.validator import validate_executable_profile, require_release_geometry -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.runtime.steady import steady_targets -from linkerhand_calibration.runtime.visual_motion import VisualMotionObserver -from linkerhand_calibration.runtime.trajectory import build_calibration_motion_command -from linkerhand_calibration.core.domain.motion_path import task_segment, record_scan_identity -from linkerhand_calibration.core.domain.sampling import command_nodes -from o30_capture_fixture import SOURCE, PACKAGE - - -@pytest.fixture -def profile(): - return load_bundled_hand_profile('o30_right_18', scope='full') - - -def test_profile_source_bindings_and_product_hashes(profile, tmp_path): - assert load_bundled_hand_profile('o30_right_18').scope.default_scope == 'full' - report = validate_executable_profile(profile, SOURCE) - require_release_geometry(report) - assert profile.key.profile_id == 'O30/right/o30_right_18/v1' - assert len(profile.zero.active_joints) == 20 - assert not profile.zero.passive_joints - assert not profile.zero.mimic_source_by_joint - assert not profile.measurement.transferred_motion_sources - assert len(profile.zero.direct_zero_joints) == 15 - assert profile.vision.tag_ids == frozenset(range(18)) - assert all(t.size_m == .016 for v in profile.vision.views for t in v.tags) - assert report['tag_links']['thumb_mcp'] == 'thumb_proximal' - assert profile.command.baseline_u8 == (0,0,255,196,127,52) + (0,)*14 - assert profile.acquisition.fixed_reference_mode == 'stationary_image' - assert profile.command.joint_directions == (1,1,-1,-1,-1,-1) + (1,)*14 - assert len(profile.motion.tasks) == 17 - ids = {tag.role: tag.tag_id for view in profile.vision.views for tag in view.tags} - expected = { - 'thumb_cmc_roll': (0, 'front', 0, 1), - 'thumb_cmc_yaw': (1, 'top', 16, 17), - 'thumb_mcp': (6, 'front', 0, 1), - 'thumb_ip': (15, 'front', 1, 2), - } - for n, finger in enumerate(('pinky', 'ring', 'middle', 'index')): - expected.update({ - finger+'_mcp_roll': (5-n, 'front', 0, 12+n), - finger+'_mcp_pitch': (10-n, 'side', 3, 4+2*n), - finger+'_pip': (14-n, 'side', 3, 4+2*n), - finger+'_dip': (19-n, 'side', 4+2*n, 5+2*n), - }) - for joint, (channel, view, parent, child) in expected.items(): - measurement = profile.measurement.measurements[joint] - assert profile.command.command_index_by_joint[joint] == channel - assert (measurement.view, ids[measurement.parent_role], ids[measurement.child_role]) == (view,parent,child) - for finger in ('pinky', 'ring', 'middle', 'index'): - task = next(t for t in profile.motion.tasks if t.key == finger+'_dip_side') - assert task.parent_reference.pose_joint == finger+'_mcp_pitch' - assert task.parent_reference.geometry_joint == finger+'_pip' - assert profile.command.names == ('thumb_roll','thumb_yaw','index_yaw','middle_yaw','ring_yaw','little_yaw', - 'thumb_root1','index_root1','middle_root1','ring_root1','little_root1', - 'index_root2','middle_root2','ring_root2','little_root2', - 'thumb_tip','index_tip','middle_tip','ring_tip','little_tip') - path = tmp_path/'roundtrip.yaml'; path.write_text(dump_hand_profile(profile)) - assert load_hand_profile(path) == profile - product = yaml.safe_load((PACKAGE/'config/o30_right_product.yaml').read_text()) - for key in ['source_urdf', 'calibration_config', 'tag_config']: - file = PACKAGE/product['artifacts'][key].split('package://linkerhand_calibration/')[1] - assert hashlib.sha256(file.read_bytes()).hexdigest() == product['artifacts'][key+'_sha256'] - assert hashlib.sha256((PACKAGE/'config/profiles/o30_right_18.yaml').read_bytes()).hexdigest() == product['profile_config_sha256'] - extrinsics = PACKAGE.parents[1]/product['artifacts']['camera_extrinsics'] - assert hashlib.sha256(extrinsics.read_bytes()).hexdigest() == product['artifacts']['camera_extrinsics_sha256'] - - -def test_tag_sizes_agree_across_profile_detector_and_calibration(profile): - expected = {tag.tag_id: tag.size_m for view in profile.vision.views for tag in view.tags} - detectors = yaml.safe_load((PACKAGE/'config/o30_right_18_tags.yaml').read_text()) - detected = {} - for node in detectors.values(): - params = node['ros__parameters'] - assert params['size'] == .016 - tags = params['tag'] - assert len(tags['ids']) == len(tags['sizes']) - detected.update(zip(tags['ids'], tags['sizes'])) - assert detected == expected - calibration = yaml.safe_load((PACKAGE/'config/o30_three_camera_calibration.yaml').read_text()) - params = calibration['o30_calibration']['ros__parameters'] - assert params['tag_size_m'] == .016 - assert len(params['tag_size_override_ids']) == len(params['tag_size_overrides_m']) - assert dict(zip(params['tag_size_override_ids'], params['tag_size_overrides_m'])) == expected - - -def test_group_zero_approaches_preserve_baseline_and_use_observable_travel(profile): - task = next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - for name in task.joints: - spec = profile.motion.joint_zero_references[name] - assert spec.command == profile.command.baseline_values - channel = profile.command.command_index_by_joint[name] - assert spec.evidence_scope == 'path' - assert [tuple(pose[2:6]) for pose in spec.approach_commands] == [ - (0,80,0,0), (255,255,255,255), (0,80,0,0)] - values = [pose[channel] for pose in spec.approach_commands] - assert max(values)-min(values) == (175 if name == 'middle_mcp_roll' else 255) - - -def test_group_schedule_native_coverage_nodes_and_peak_velocity(profile): - task = next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - units = [u for u in CalibrationEngine(profile).scan_units() if u.task_key == task.key] - assert len(units) == 12 - for cycle in (0, 1, 3): - selected = [u for u in units if u.cycle == cycle] - assert [u.segment_key for u in selected] == [s.key for s in task.segments] - middle = [(dict(s.start_commands)[3], dict(s.end_commands)[3]) for s in task.segments] - assert middle == [(80,255),(255,80),(80,0),(0,80)] - assert {255.,196.,127.,52.,80.} <= set(command_nodes(profile, task)) - for unit in units: - segment = task_segment(task, unit.segment_key) - targets = [segment.commands_at(v, quantize=True) for v in steady_targets(profile, unit)] - for channel in segment.moving_channels: - lo, hi = sorted((dict(segment.start_commands)[channel], dict(segment.end_commands)[channel])) - expected = {v for v in command_nodes(profile, task, holdout=unit.cycle==3) if lo<=v<=hi} - assert expected <= {t[channel] for t in targets} - segment = task.segments[0] - start = build_calibration_motion_command(task, segment.start, profile=profile, segment_key=segment.key) - end = build_calibration_motion_command(task, segment.end, profile=profile, segment_key=segment.key) - execution = MotionExecution(profile, MotionCommand('sweep', tuple(end), 200, - measured_channels=segment.moving_channels), - initial_command=start, initial_feedback=start, now=0., identity='linked', - visual_observer=VisualMotionObserver(profile)) - previous = execution.sample(0) - for t in np.linspace(.01,21,2100): - actual = execution.sample(t) - progress = actual[5]/255 - assert actual[3] == pytest.approx(80+175*progress, abs=1.) - assert actual[2] == actual[4] == actual[5] - # Integer bytes carry at most one unit of quantization error. - assert max(abs(a-b) for a,b in zip(actual,previous)) <= 20*.01+1. - assert math.pi*255/(2*execution.duration) <= 20.000001 - previous = actual - - -@pytest.mark.parametrize(('scope', 'expected_units'), [('full', 108), ('without_finger_roll', 96)]) -def test_complete_motion_driver_clearances_own_channels_and_retry(scope, expected_units): - profile = load_bundled_hand_profile('o30_right_18', scope=scope) - from linkerhand_calibration.core.domain.capture_plan import configure_training, TASKWISE_TWO_ROUND - profile = configure_training(profile, TASKWISE_TWO_ROUND) - driver = SessionExecution(profile) - driver.session.device_ready();driver.session.start() - command = profile.command.baseline_values - rows, motions, stamp, omitted = [], [], 1, False - for _ in range(6000): - phase = driver.session.phase - if phase == Phase.FIT:break - if phase == Phase.REFERENCE_LOCKING: - driver.session.reference_locked();continue - if phase == Phase.EVALUATE: - driver.evaluate(rows);continue - motion = driver.motion(command) - assert motion is not None - motions.append(motion) - before = command - command = motion.target - if motion.phase == 'joint_zero' and not motion.reference_only: - driver.zero_references.update({name: SimpleNamespace(joint=name, task_key=motion.task_key, - channel=profile.command.command_index_by_joint[name], - samples=(SimpleNamespace(command=motion.target, feedback=motion.target),)) - for name in motion.zero_joints}) - if motion.phase in {'sweep','steady'}: - unit = driver.action.scan_unit - task = next(t for t in profile.motion.tasks if t.key == unit.task_key) - segment = task_segment(task, unit.segment_key) if unit.segment_key else None - names = segment.joints if segment else task.joints - for name in names: - channel = profile.command.command_index_by_joint[name] - start = before[channel] - end = dict(segment.end_commands)[channel] if segment else unit.end - # One lower-middle segment fails; valid upper-middle rows cannot conceal it. - if unit.segment_key == 'middle_lower_decreasing' and unit.cycle == 0 and motion.attempt == 1: - omitted = True;continue - for value in (np.linspace(start,command[channel],129) if motion.phase == 'sweep' else [command[channel]]*3): - stamp += 1 - rows.append(dict(joint=name,view=task.view,task_name=task.key,cycle=unit.cycle, - direction=unit.direction,segment_key=unit.segment_key,attempt=motion.attempt, - image_stamp_ns=stamp,feedback_u8=value,command_u8=value,sample_phase=motion.phase, - steady_index=motion.steady_index,steady_target=command[channel])) - driver.motion_complete() - assert driver.session.phase == Phase.FIT - assert len(driver.completed_units) == expected_units - task_order = list(dict.fromkeys(m.task_key for m in motions if m.phase == 'sweep')) - assert task_order[:4] == ['thumb_cmc_roll_front', 'thumb_mcp_front', 'thumb_ip_front', 'thumb_cmc_yaw_top'] - assert task_order[4:] == [ - f'{finger}_{joint}_side' for finger in ('pinky', 'ring', 'middle', 'index') - for joint in ('mcp_pitch', 'pip', 'dip')] + (['fingers_mcp_roll_front'] if scope == 'full' else []) - if scope == 'full': - roll_zeros = [m for m in motions if m.task_key == 'fingers_mcp_roll_front' and m.phase == 'joint_zero'] - assert roll_zeros and all(m.target[6:] == (0,) * 14 for m in roll_zeros) - for motion in motions: - if motion.task_key == 'thumb_cmc_yaw_top': - assert motion.target[:1] + motion.target[2:] == ( - profile.command.baseline_values[:1] + profile.command.baseline_values[2:]) - exits = [m for m in motions if m.phase == 'task_exit' and m.task_key == 'thumb_cmc_roll_front'] - assert len(exits) == 2 - assert exits[0].target[0] == 0 and exits[0].target[2] == 0 - assert exits[1].target[2] == 255 - mcp_exits = [m for m in motions if m.phase == 'task_exit' and m.task_key == 'thumb_mcp_front'] - assert [(m.target[6], m.target[1]) for m in mcp_exits] == [(0, 80), (0, 0)] - parent_checks = [m for m in motions if m.reference_reuse] - expected_checks = [('thumb_ip', 'thumb_cmc_roll'), - *((finger+'_dip', finger+'_mcp_pitch') for finger in ('pinky','ring','middle','index'))] - assert len(parent_checks) == len(expected_checks) - for motion, (joint, source) in zip(parent_checks, expected_checks): - assert motion.zero_joints == (source,) - assert motion.target == profile.motion.joint_zero_references[joint].command - assert all(m.target[6] == 0 for m in motions if m.task_key == 'thumb_ip_front') - fingers = ['pinky','ring','middle','index'] - for n,finger in enumerate(fingers): - held = [profile.command.command_index_by_joint[f+'_'+s] for f in fingers[:n] - for s in ('mcp_pitch','pip','dip')] - for motion in motions: - if motion.task_key and motion.task_key.startswith(finger+'_') and motion.task_key.endswith('_side'): - assert all(motion.target[i] == 255 for i in held), motion - assert {m.segment_key for m in motions if m.attempt==2} == ( - {'middle_lower_decreasing'} if scope == 'full' else set()) - - -def test_sdk_native_names_device_identity_and_topics(profile): - from linkerhand_calibration.runtime.adapters.ros_binding import bind_ros_sdk, SdkBindingPorts - from linkerhand_calibration.runtime.adapters.o30_ros import launch_parameters - from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics - callback, now, sent = [], [0.], [] - adapter = bind_ros_sdk(profile, SdkBindingPorts(lambda: True,lambda: now[0],callback.append), - publish=sent.append,set_speed=lambda i,v:sent.append((i,v))) - assert not adapter.health().connected - info = dict(hand_type='right',side='RIGHT',model='O30',uid='TEST_ONLY',joint_names=list(profile.command.names),online=True) - callback[0](json.dumps(info));assert adapter.health().connected - callback[0](json.dumps(dict(info,joint_faults={'thumb_tip':['执行器过流']}))) - assert adapter.health().active_faults == ('thumb_tip:执行器过流',) - assert not adapter.health().safe - callback[0](json.dumps(dict(info,joint_faults={'thumb_tip':['灵巧手层判定堵转']}))) - assert adapter.health().safe # Firmware endpoint telemetry is retained in diagnostic. - callback[0](json.dumps(info)) - names=list(reversed(profile.command.names)); values=list(reversed(range(20))) - assert adapter.parse_feedback(names,values) == tuple(range(20)) - assert adapter.parse_feedback([],values) is None - assert adapter.parse_feedback(names[:-1]+[names[0]],values) is None - assert adapter.parse_feedback(names,[float('nan')]*20) is None - callback[0](json.dumps(dict(info,side='LEFT')));assert not adapter.health().connected - callback[0](json.dumps(dict(info,uid='CHANGED')));assert not adapter.health().connected - callback[0](json.dumps(info));now[0]=4.;assert not adapter.health().connected - topics=sdk_topics(profile) - assert topics.setting == '/cb_right_hand_setting_cmd' - assert topics.health == '/cb_right_hand_info' - params=launch_parameters() - assert params['comm_type']=='libcanbus' and params['canfd_device']==0 - assert not params['auto_init_pose'] and not params['is_touch'] - assert params['state_rate']==30. - - -def test_capture_uses_each_feedback_and_middle_only_observations(profile): - from linkerhand_calibration.runtime.capture import ObservationCapture, CaptureFrame - from linkerhand_calibration.runtime.reference_lock import ReferenceLock - from test_observation_capture import projected_square - lock = ReferenceLock({'front':0,'side':3,'top':16}) - lock.begin_session() - lock.start_locking() - capture = ObservationCapture(profile,reference_lock=lock, - extrinsics=SimpleNamespace(transform=lambda _:np.eye(4))) - task=next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - all_roles=['front_base','pinky_mcp_roll','ring_mcp_roll','middle_mcp_roll','index_mcp_roll'] - matrix=np.array([[1200,0,640],[0,1200,480],[0,0,1.]]) - corners={role:projected_square(.016,(.024*i, .02, .45)) for i,role in enumerate(all_roles)} - vector=list(profile.command.baseline_values) - vector[2:6]=[146.,171.,146.,146.] - feedback=list(vector);feedback[2:6]=[143.,169.,140.,141.] - motion=MotionCommand('sweep',tuple(vector),200,task.key,5,0,'increasing', - segment_key='linked_increasing',observed_joints=task.joints,measured_channels=(2,3,4,5)) - for stamp in range(1,15): - rows,_=capture.consume(CaptureFrame('front',stamp*40000000,matrix,corners,tuple(feedback),tuple(vector)),motion) - samples={r['joint']:r for r in rows if r['kind']=='joint_sample'} - assert set(samples)==set(task.joints) - for name,row in samples.items(): - channel=profile.command.command_index_by_joint[name] - assert row['motor_index']==channel - assert row['feedback_u8']==feedback[channel] - assert row['command_u8']==vector[channel] - assert row['segment_key']=='linked_increasing' - lower=replace(motion,segment_key='middle_lower_decreasing',direction='decreasing', - observed_joints=('middle_mcp_roll',),measured_channels=(3,),command_index=3) - visible={r:c for r,c in corners.items() if r in {'front_base','middle_mcp_roll'}} - rows,_=capture.consume(CaptureFrame('front',600000000,matrix,visible,tuple(feedback),tuple(vector)),lower) - assert {r['joint'] for r in rows if r['kind']=='joint_sample'}=={'middle_mcp_roll'} - rows,_=capture.consume(CaptureFrame('front',640000000,matrix,visible,tuple(feedback),tuple(vector)),motion) - assert {r['joint'] for r in rows if r['kind']=='joint_sample'}=={'middle_mcp_roll'} - - -def test_model_scope_replaces_only_prepared_roles_and_schedules_parent_dependencies(profile): - from linkerhand_calibration.runtime.capture import ObservationCapture - from linkerhand_calibration.runtime.reference_lock import ReferenceLock - from linkerhand_calibration.runtime.branch_initialization import MotionBranchModel - from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation - capture=ObservationCapture(profile,reference_lock=ReferenceLock({'front':0,'side':3,'top':16}), - extrinsics=SimpleNamespace(transform=lambda _:np.eye(4))) - def model(task,joint,parent,child,identity): - return MotionBranchModel(task,'front',(SimpleNamespace(relation=MotionRelation(joint,parent,child)),),((child,identity),)) - roll=model('thumb_cmc_roll_front','thumb_cmc_roll','front_base','thumb_mcp','roll-id') - mcp=model('thumb_mcp_front','thumb_mcp','front_base','thumb_mcp','mcp-id') - capture.install_motion_model(roll) - with pytest.raises(ValueError,match='replacement_forbidden'): - capture.install_motion_model(mcp) - preparation=MotionCommand('zero_approach',profile.command.baseline_values,200, - 'thumb_mcp_front',reference_joints=('thumb_mcp',)) - capture.begin_motion(preparation) - assert not capture._motion_models - capture.install_motion_model(mcp) - capture.begin_motion(preparation) # Other waypoints of the same preparation cannot erase it. - assert list(capture._motion_models.values())==[mcp] - ip_preparation=replace(preparation,task_key='thumb_ip_front') - capture.begin_motion(ip_preparation) - parent=replace(mcp,task_name='thumb_ip_front',evidence_ids=(('thumb_mcp','parent-at-yaw-zero'),)) - child=model('thumb_ip_front','thumb_ip','thumb_mcp','thumb_ip','ip-id') - capture.install_motion_model(parent);capture.install_motion_model(child) - motion=MotionCommand('sweep',profile.command.baseline_values,200,'thumb_ip_front',15,0,'increasing') - assert capture._motion_model_schedule('front',motion)==(parent,child) - # Separately frozen four-finger models do not impose unrelated visibility. - group = next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - for name in group.joints: - capture.install_motion_model(model('fingers_mcp_roll_front',name,'front_base',name,name+'-id')) - lower=replace(motion,task_key='fingers_mcp_roll_front',segment_key='middle_lower_increasing', - observed_joints=('middle_mcp_roll',)) - assert len(capture._motion_model_schedule('front',lower))==1 - assert capture._motion_model_schedule('front',lower)[0].evidence_ids[0][0]=='middle_mcp_roll' - - -def test_old_or_misattributed_group_records_rejected(profile): - from linkerhand_calibration.runtime.acquisition import accepted_joint_records - task=next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - row=dict(joint='middle_mcp_roll',task_name=task.key,cycle=0,direction='increasing', - motor_index=3,feedback_u8=100.,command_u8=100.,image_stamp_ns=1,view='front', - relative_quaternion_xyzw=[0,0,0,1],command_vector_u8=[0.]*20) - with pytest.raises(ValueError,match='unknown motion segment'): - accepted_joint_records(profile,[row]) - row.update(segment_key='middle_lower_increasing',motor_index=5) - with pytest.raises(ValueError,match='motor or direction'): - accepted_joint_records(profile,[row]) - row.update(motor_index=3,command_vector_u8=[0,0,90,50]+[0]*16) - with pytest.raises(ValueError,match='multi-channel path'): - accepted_joint_records(profile,[row]) diff --git a/src/linkerhand_calibration/test/test_o30_side_observer.py b/src/linkerhand_calibration/test/test_o30_side_observer.py deleted file mode 100644 index 60ab895..0000000 --- a/src/linkerhand_calibration/test/test_o30_side_observer.py +++ /dev/null @@ -1,69 +0,0 @@ -"""The recorded side failure reuses the front solver with a proximal Tag.""" -from dataclasses import asdict, replace -import gzip -import json -from pathlib import Path - -from linkerhand_calibration.core.geometry.tag_pose.ippe import solve_square_tag_ippe -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation, RoleCandidates -from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageRoleObservation -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion -from linkerhand_calibration.runtime.diagnostic_analysis import image_frames -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 - - -def test_same_recorded_mcp_arc_resolves_with_middle_link_tag(): - path = Path(__file__).parent/'fixtures/o30_side_mcp_observers.json.gz' - data = json.loads(gzip.decompress(path.read_bytes())) - rows = sorted(data['original_records'], key=lambda r: r['image_stamp_ns']) - assert len(rows) == 129 - for row in rows: - assert source_frame_sha256(row) == data['original_frame_hashes'][str(row['image_stamp_ns'])] - original = image_frames(rows, (MotionRelation('pinky_mcp_pitch', 'side_base', 'pinky_dip'),)) - failed = resolve_image_motion(original, (MotionRelation('pinky_mcp_pitch', 'side_base', 'pinky_dip'),)) - assert not failed.resolved and failed.reason == 'image_motion_families_not_distinguishable' - changed = [] - for frame in original: - tag = data['secondary_corners'][str(frame.evidence.stamp_ns)] - assert tag['tag_id'] == 4 - corners = tuple(map(tuple, tag['corners_xy'])) - candidates = tuple(solve_square_tag_ippe(corners, tag_size_m=tag['tag_size_m'], - camera_matrix=frame.camera_matrix)) - changed.append(replace(frame, evidence=replace(frame.evidence, roles=(frame.evidence.roles[0], - RoleCandidates('pinky_pip', candidates))), observations=(frame.observations[0], - ImageRoleObservation('pinky_pip', tag['tag_size_m'], corners)))) - result = resolve_image_motion(changed, (MotionRelation('pinky_mcp_pitch', 'side_base', 'pinky_pip'),)) - assert result.resolved, result - assert len(result.hypotheses) == 2 - 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) - winner = next(h for h in result.hypotheses if h.branches == result.model.branches) - assert winner.validation_quality[0].rms_px < .37 - assert max(p for _, p in winner.comparison_adjusted_p_values) < .01 - - # The real preparation owner must select the same recorded observations, - # including held-channel guards and the stationary side reference. - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from o30_capture_fixture import SOURCE - profile = load_bundled_hand_profile('o30_right_18') - owner = BranchInitialization(profile, source_urdf=SOURCE) - spec = profile.motion.joint_zero_references['pinky_mcp_pitch'] - epoch, version = rows[0]['session_epoch'], rows[0]['motion_version'] - motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, - reference_joints=('pinky_mcp_pitch',)) - owner.begin(motion, session_epoch=epoch, motion_version=version) - for row, frame in zip(rows, changed): - tag = data['secondary_corners'][str(frame.evidence.stamp_ns)] - item = {**tag, 'tag_role': 'pinky_pip', 'selected_pose': None, - 'reprojection_valid_candidates': [asdict(p) for p in frame.evidence.roles[1].candidates], - 'candidate_diagnostics': {'branch_frozen': False, 'observation_stamp_ns': frame.evidence.stamp_ns}} - owner.observe({**row, 'tags': {**row['tags'], 'pinky_pip': item}}) - request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('pinky_mcp_pitch',)), - session_epoch=epoch, motion_version=version+1) - assert not request.invalid_reason - assert len(request.image_frames) == 129 - assert request.relations == (MotionRelation('pinky_mcp_pitch', 'side_base', 'pinky_pip'),) - assert solve_initialization(request).resolved diff --git a/src/linkerhand_calibration/test/test_o6_right_profile.py b/src/linkerhand_calibration/test/test_o6_right_profile.py deleted file mode 100644 index 5976231..0000000 --- a/src/linkerhand_calibration/test/test_o6_right_profile.py +++ /dev/null @@ -1,377 +0,0 @@ -from __future__ import annotations - -from pathlib import Path -import xml.etree.ElementTree as ET - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.calibrated_joint_state_bridge import CalibratedCommandMapper -from linkerhand_calibration.core import validate_profile -from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import UrdfKinematicModel -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.artifacts import ( - validate_o6_runtime_payload, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.fitting import _zero_profile -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.profile import ( - ACTIVE_JOINTS, - BASELINE_SPEED_U8, - COMMAND_NAMES, - FORMAL_SPEED_U8, - GEOMETRIC_ZERO_JOINTS, - KEY, - MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M, - MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, - MAXIMUM_HYSTERESIS_DEG, - MIMIC_SOURCE_BY_JOINT, - PASSIVE_JOINTS, - PREFLIGHT_SPEED_U8, - TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, - TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, - build_typed_profile, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.runner import render_o6_progress_zh - - -PACKAGE = Path(__file__).resolve().parents[1] -SOURCE = PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf" -LEFT_SOURCE = PACKAGE / "urdf/o6_left/linkerhand_o6_left.urdf" -KINEMATIC_MODEL = UrdfKinematicModel(SOURCE) - - -def _joint_elements(path: Path) -> dict[str, ET.Element]: - return { - str(joint.get("name")): joint - for joint in ET.parse(path).getroot().findall("joint") - if joint.get("type") == "revolute" - } - - -def test_o6_profile_declares_reviewed_six_channel_contract() -> None: - profile = build_typed_profile() - validate_profile(profile) - assert profile.key == KEY - assert profile.namespace == "/o6_calibration" - assert profile.command.names == COMMAND_NAMES - assert profile.command.baseline_u8 == (255,) * 6 - assert profile.command.feedback_name_aliases == {} - assert profile.motion.speed_parameters["baseline_u8"] == BASELINE_SPEED_U8 == 80 - assert profile.motion.speed_parameters["preflight_u8"] == PREFLIGHT_SPEED_U8 == 60 - assert profile.motion.speed_parameters["formal_u8"] == FORMAL_SPEED_U8 == 40 - assert MAXIMUM_HYSTERESIS_DEG == 3.5 - assert MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX == 1.5 - assert MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M == 0.020 - assert GEOMETRIC_ZERO_JOINTS == frozenset(profile.zero.direct_zero_joints) - assert not profile.zero.endpoint_anchor_by_joint - assert profile.vision.extrinsics_quality_limits[ - "reprojection_rms_px" - ] == 1.5 - assert _zero_profile().directed_base_axis_joints == frozenset({ - "rh_thumb_cmc_yaw", "rh_pinky_mcp_pitch" - }) - assert all( - task.preflight_speed_u8 == PREFLIGHT_SPEED_U8 - and task.formal_speed_u8 == FORMAL_SPEED_U8 - for task in profile.motion.tasks - ) - assert profile.vision.tag_ids == frozenset(range(8)) - assert [task.key for task in profile.motion.tasks] == [ - "thumb_yaw_top", "thumb_pitch_ip_front", "pinky_pitch_dip_side" - ] - assert profile.zero.coupling_model_by_joint == { - name: "linear_mimic" for name in PASSIVE_JOINTS - } - - -def test_o6_profile_matches_source_urdf_topology() -> None: - root = ET.parse(SOURCE).getroot() - joints = { - str(joint.get("name")): joint - for joint in root.findall("joint") - if joint.get("type") == "revolute" - } - assert set(joints) == set(ACTIVE_JOINTS + PASSIVE_JOINTS) - for target, source in MIMIC_SOURCE_BY_JOINT.items(): - assert joints[target].find("mimic").get("joint") == source - assert all( - joints[target].find("limit").get("upper") - == joints[source].find("limit").get("upper") - for target, source in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items() - ) - assert all( - joints[target].find("limit").get("upper") - == joints[source].find("limit").get("upper") - for target, source in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items() - ) - - -def test_o6_progress_uses_the_shared_operator_layout() -> None: - text = render_o6_progress_zh({ - "state": "RUNNING", - "serial_number": "O6_TEST", - "step_index": 12, - "step_count": 61, - "step_fraction": 0.5, - "task_name": "thumb_yaw_top", - "phase": "sweep", - "cycle": 2, - "direction": "decreasing", - "attempt": 1, - "target_u8": 0, - "current_command_u8": 127.0, - "actual_u8": 128.0, - "valid_frames": 45, - "total_frames": 48, - "tag_detection_rate": 0.96, - "joint_frame_rate": 0.9375, - "recognized_tag_ids": [6, 7], - "unrecognized_tag_ids": [], - "feedback_hz": 29.8, - }) - assert "[O6_TEST] 标定中" in text - assert "拇指 CMC yaw（上面机位，ID6→ID7）" in text - assert "阶段：正式扫描（第 3/4 轮）" in text - assert "方向：递减" in text - assert "已识别 ID6/ID7" in text - - -def test_o6_baseline_failure_identifies_the_slowest_channel() -> None: - text = render_o6_progress_zh({ - "state": "PAUSED", - "reason": "mechanical_stall:baseline:all", - "serial_number": "O6_TEST", - "step_index": 0, - "step_count": 61, - "step_fraction": 0.25, - "phase": "baseline", - "cycle": None, - "direction": None, - "attempt": 1, - "target_u8": 255, - "speed_u8": BASELINE_SPEED_U8, - "command_trajectory_full_range_seconds": 6.0, - "current_command_u8": 249.381, - "actual_u8": 233.3, - "valid_frames": 0, - "total_frames": 0, - "feedback_hz": 58.9, - "command_names": list(COMMAND_NAMES), - "latest_state_u8": [255, 125, 255, 255, 255, 255], - "maximum_error_channel": "thumb_cmc_yaw", - "maximum_error_u8": 130.0, - }) - assert "六路反馈：" in text - assert "运动：速度档 80；全行程 6.0 秒余弦轨迹" in text - assert "thumb_cmc_yaw=125.0" in text - assert "最大偏差：thumb_cmc_yaw=130.0" in text - assert "原因：目标电机连续两秒没有向目标推进" in text - - -def _valid_payload() -> dict: - profile = build_typed_profile() - curves = list(np.linspace(1.0, 0.0, 256)) - joints = {} - for name in ACTIVE_JOINTS + PASSIVE_JOINTS: - source = MIMIC_SOURCE_BY_JOINT.get(name, name) - motor = profile.command.command_index_by_joint[source] - passive = name in PASSIVE_JOINTS - item = { - "urdf_joint": name, - "sdk_channel": COMMAND_NAMES[motor], - "motor_index": motor, - "passive": passive, - "calibration_status": profile.joint_coverage[name], - "zero_command_u8": 255, - "zero_angles": {"policy": "test"}, - "angle_rad": curves, - "decreasing_rad": curves, - "increasing_rad": curves, - } - if passive: - item.update({ - "source_joint": source, - "mimic_offset_rad": 0.0, - "coupling_model": "quadratic_runtime", - "coupling_coefficients": [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], - "urdf_mimic_enabled": True, - "urdf_mimic_policy": "endpoint_linear_fallback", - "mimic_multiplier": 1.0, - }) - donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name) - if donor: - item["transferred_from_joint"] = donor - joints[name] = item - return { - "schema_version": 6, - "profile_id": KEY.profile_id, - "layout_id": KEY.layout, - "model": "O6", - "side": "right", - "serial_number": "O6_TEST", - "calibration_scope": "partial", - "publication_pointer": "latest_partial_passed", - "angle_unit": "rad", - "command_range": [0, 255], - "curve_input_domain": "feedback_u8", - "runtime_curve_policy": "direction_aware", - "command_names": list(COMMAND_NAMES), - "baseline_command_u8": [255] * 6, - "protected_inputs": { - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - }, - "joints": joints, - "quality": {"passed": True, "scope": "partial"}, - } - - -def test_o6_schema_v6_and_bridge_use_native_yaw_channel() -> None: - payload = _valid_payload() - validate_o6_runtime_payload(payload) - mapper = CalibratedCommandMapper(payload, expected_side="right") - assert mapper.feedback_name_aliases == {} - assert mapper.command_names == COMMAND_NAMES - assert len(mapper.map_positions([255] * 6, COMMAND_NAMES)) == 11 - mismatched = {**payload, "model": "L6"} - with pytest.raises(ValueError, match="identity or command contract"): - CalibratedCommandMapper(mismatched) - - -def _curve_records(travel: float) -> list[dict]: - rows = [] - for cycle in range(4): - for direction, commands in ( - ("decreasing", range(255, -1, -4)), - ("increasing", range(0, 256, 4)), - ): - values = list(commands) - endpoint = 0 if direction == "decreasing" else 255 - if values[-1] != endpoint: - values.append(endpoint) - for feedback in values: - angle = travel * (255.0 - feedback) / 255.0 - rows.append({ - "cycle": cycle, - "direction": direction, - "command_u8": feedback, - "feedback_u8": float(feedback), - "relative_quaternion_xyzw": Rotation.from_rotvec( - [angle, 0.0, 0.0] - ).as_quat().tolist(), - }) - return rows - - - - -def _pose(xyz: list[float], rpy: list[float]) -> np.ndarray: - return transform_matrix(xyz, Rotation.from_euler("xyz", rpy).as_quat()) - - -def _geometric_records() -> dict[str, list[dict]]: - model = KINEMATIC_MODEL - travels = { - "rh_thumb_cmc_pitch": 0.5, - "rh_thumb_cmc_yaw": 1.2, - "rh_pinky_mcp_pitch": 1.2, - } - offsets = { - "rh_thumb_cmc_pitch": -0.02, - "rh_thumb_cmc_yaw": 0.03, - "rh_pinky_mcp_pitch": 0.0, - } - views = { - "rh_thumb_cmc_pitch": "front", - "rh_thumb_cmc_yaw": "top", - "rh_thumb_ip": "front", - "rh_pinky_mcp_pitch": "side", - "rh_pinky_dip": "side", - } - motors = { - "rh_thumb_cmc_pitch": 0, - "rh_thumb_cmc_yaw": 1, - "rh_thumb_ip": 0, - "rh_pinky_mcp_pitch": 5, - "rh_pinky_dip": 5, - } - passive_sources = { - "rh_thumb_ip": "rh_thumb_cmc_pitch", - "rh_pinky_dip": "rh_pinky_mcp_pitch", - } - base = _pose([0.11, -0.04, 0.72], [0.11, -0.08, 0.17]) - base_mounts = { - "front": _pose([0.0, 0.008, 0.055], [0.2, -0.1, 0.3]), - "side": _pose([-0.01, 0.002, 0.045], [-0.1, 0.2, -0.2]), - "top": _pose([0.012, -0.006, 0.05], [0.1, 0.3, 0.15]), - } - link_mounts = { - "rh_thumb_cmc_yaw": _pose([0.012, 0.019, 0.006], [0.2, 0.1, -0.1]), - "rh_thumb_cmc_pitch": _pose([0.009, -0.028, 0.011], [-0.2, 0.1, 0.25]), - "rh_thumb_ip": _pose([0.006, -0.025, 0.018], [0.15, -0.1, 0.2]), - "rh_pinky_mcp_pitch": _pose([0.006, 0.004, 0.021], [-0.1, 0.2, 0.1]), - "rh_pinky_dip": _pose([0.004, 0.003, 0.019], [0.12, 0.08, -0.2]), - } - result = {name: [] for name in views} - for name, view in views.items(): - for cycle in range(4): - for direction, commands in ( - ("decreasing", range(255, -1, -8)), - ("increasing", range(0, 256, 8)), - ): - values = list(commands) - endpoint = 0 if direction == "decreasing" else 255 - if values[-1] != endpoint: - values.append(endpoint) - for feedback in values: - state = [255.0] * 6 - state[motors[name]] = float(feedback) - active = { - joint: travel * (255.0 - state[motor]) / 255.0 - for joint, travel, motor in ( - ("rh_thumb_cmc_pitch", 0.5, 0), - ("rh_thumb_cmc_yaw", 1.2, 1), - ("rh_pinky_mcp_pitch", 1.2, 5), - ) - } - angles = { - **active, - "rh_thumb_ip": active["rh_thumb_cmc_pitch"] * 1.86, - "rh_pinky_dip": active["rh_pinky_mcp_pitch"] * 0.89, - } - if name in passive_sources: - parent_name = passive_sources[name] - parent = base @ model.link_transform( - parent_name, - zero_offsets=offsets, - joint_angles=angles, - independent_mimic_angles=True, - ) @ link_mounts[parent_name] - else: - parent = base @ base_mounts[view] - child = base @ model.link_transform( - name, - zero_offsets=offsets, - joint_angles=angles, - independent_mimic_angles=True, - ) @ link_mounts[name] - relative = np.linalg.inv(parent) @ child - result[name].append({ - "cycle": cycle, - "direction": direction, - "command_u8": feedback, - "feedback_u8": float(feedback), - "relative_quaternion_xyzw": Rotation.from_matrix( - relative[:3, :3] - ).as_quat().tolist(), - "relative_translation_xyz_m": relative[:3, 3].tolist(), - "parent_pose_common": matrix_payload(parent), - "child_pose_common": matrix_payload(child), - "view_normal_common_xyz": [0.577350269] * 3, - "camera_center_common_xyz_m": [0.0, 0.0, -0.5], - "state_u8": state, - }) - return result diff --git a/src/linkerhand_calibration/test/test_o6_tag_installation.py b/src/linkerhand_calibration/test/test_o6_tag_installation.py deleted file mode 100644 index d489917..0000000 --- a/src/linkerhand_calibration/test/test_o6_tag_installation.py +++ /dev/null @@ -1,25 +0,0 @@ -"""The top thumb Tag measures yaw but is physically on the distal link.""" - -from pathlib import Path - -import numpy as np -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.profiles.observations import compile_tag_links - - -def test_top_thumb_tag_shares_the_distal_link_and_follows_flexion(): - package=Path(__file__).resolve().parents[1] - config=load_product_config(package/"config/o6_right_product.yaml",check_can=False) - profile=config.calibration_contract.typed_profile - model=UrdfKinematicModel(config.source_urdf) - links=compile_tag_links(profile,model) - assert links["thumb_yaw"]==links["thumb_ip"]=="rh_thumb_distal" - joint=model.parent_joint_by_child[links["thumb_yaw"]] - before=model.link_transform(joint,zero_offsets={},joint_angles={},independent_mimic_angles=True) - angles={"rh_thumb_cmc_yaw":0.,"rh_thumb_cmc_pitch":.35,"rh_thumb_ip":.45} - after=model.link_transform(joint,zero_offsets={},joint_angles=angles,independent_mimic_angles=True) - assert np.linalg.norm(after[:3,3]-before[:3,3])>.01 - assert Rotation.from_matrix(before[:3,:3].T@after[:3,:3]).magnitude()>.3 diff --git a/src/linkerhand_calibration/test/test_observation_capture.py b/src/linkerhand_calibration/test/test_observation_capture.py deleted file mode 100644 index 6afb19e..0000000 --- a/src/linkerhand_calibration/test/test_observation_capture.py +++ /dev/null @@ -1,133 +0,0 @@ -"""Independent pinhole projections exercise the real PnP/capture boundary.""" - -from dataclasses import replace -from types import SimpleNamespace - -import numpy as np -from scipy.spatial.transform import Rotation -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture -from linkerhand_calibration.runtime.reference_lock import ReferenceLock -from linkerhand_calibration.runtime.motion_execution import MotionCommand - - -def projected_square(size, translation, rpy=(.1, -.15, .03)): - # IPPE square order: top-left, top-right, bottom-right, bottom-left. - s = size/2 - points = np.array([[-s, s, 0], [s, s, 0], [s, -s, 0], [-s, -s, 0]]) - xyz = points @ Rotation.from_euler("xyz", rpy).as_matrix().T + translation - return (xyz[:, :2]/xyz[:, 2:])*1200 + [640, 480] - - -def make_capture(): - profile = load_bundled_hand_profile("l6_right_8") - front = profile.vision.views[0] - tags = tuple(replace(tag, size_m=.024 if tag.role == "thumb_pitch" else .016) for tag in front.tags) - profile = replace(profile, vision=replace(profile.vision, - views=(replace(front, tags=tags),)+profile.vision.views[1:])) - lock = ReferenceLock({v.name: next(t.tag_id for t in v.tags if t.fixed_reference) for v in profile.vision.views}) - capture = ObservationCapture(profile, reference_lock=lock, - extrinsics=SimpleNamespace(transform=lambda _: np.eye(4))) - return profile, lock, capture - - -def frame(profile, stamp, *, omit=(), shift=0, feedback=True): - roles = {"front_base": (.0, .0, .45), "thumb_pitch": (.05, .03, .45), "thumb_dip": (.08, .05, .45)} - sizes = {tag.role: tag.size_m for tag in profile.vision.views[0].tags} - corners = {role: projected_square(sizes[role], xyz)+[shift, 0] for role, xyz in roles.items() if role not in omit} - values = tuple(map(float, profile.command.baseline_values)) - return CaptureFrame("front", stamp*40_000_000, np.array([[1200, 0, 640], [0, 1200, 480], [0, 0, 1]]), - corners, values if feedback else None, values, command_directions=("increasing",)*len(values)) - - -def test_only_ten_unique_post_start_valid_frames_lock_and_mounts_keep_per_tag_size(): - profile, lock, capture = make_capture() - for index in range(20): - capture.consume(frame(profile, index+1)) - assert not lock.references # Preview is not a reference. - lock.begin_session() - capture.reset() - lock.start_locking() - for _ in range(20): - capture.consume(frame(profile, 100), collect_installations=True) - assert not lock.references - # Five independent images confirm a branch; ten accepted images lock it. - for index in range(13): - capture.consume(frame(profile, 101+index), collect_installations=True) - fixed, moving = capture.fingerprint_poses() - assert "front" in fixed - np.testing.assert_allclose(fixed["front"]["translation_xyz_m"], [0, 0, .45], atol=1e-7) - assert "front:thumb_pitch" in moving - assert len(capture.installations["front:thumb_pitch"]) == 10 - - -def test_short_visual_loss_discards_frames_cached_base_is_not_fake_image_data(): - profile, lock, capture = make_capture() - lock.begin_session() - lock.start_locking() - for i in range(14): - capture.consume(frame(profile, i+1), collect_installations=True) - motion = MotionCommand("sweep", tuple(profile.command.baseline_values), 1, - "thumb_pitch_dip_front", 0, 0, "decreasing") - rows, moved = capture.consume(frame(profile, 20, omit=("front_base",)), motion) - assert moved is None and len(rows) == 2 - assert rows[0]["locked_reference_roles"] == ["front_base"] - assert "image_relative_xy_px" not in rows[0] - assert capture.consume(frame(profile, 21, feedback=False), motion)[0] == () - assert capture.consume(frame(profile, 22, omit=("thumb_pitch",)), motion)[0] == () - assert len(capture.consume(frame(profile, 23), motion)[0]) == 2 - assert len(capture.installations["front:thumb_pitch"]) == 10 # Frozen during motion. - - -@pytest.mark.parametrize("drift", [6, 200]) -def test_fixed_reference_displacement_requires_ten_confirming_images(drift): - profile, lock, capture = make_capture() - lock.begin_session() - lock.start_locking() - for i in range(14): - capture.consume(frame(profile, i+1)) - for i in range(9): - assert capture.consume(frame(profile, 20+i, shift=drift))[1] is None - assert capture.consume(frame(profile, 29, shift=drift))[1] is not None - - -def test_joint_zero_only_requires_its_own_parent_child_tags(): - from linkerhand_calibration.core.domain.reference import build_joint_zero_reference - profile, lock, capture = make_capture() - lock.begin_session() - lock.start_locking() - for i in range(14): - capture.consume(frame(profile, i+1)) - name = "rh_thumb_cmc_pitch" - spec = profile.motion.joint_zero_references[name] - motion = MotionCommand("joint_zero", spec.command, 1, task_key=spec.task_key, - command_index=0, cycle=0, zero_joints=(name,)) - rows = [] - for i in range(10): - samples, movement = capture.consume(frame(profile, 20+i, omit=("thumb_dip",)), motion) - assert movement is None - rows.extend(samples) - assert len(rows) == 10 and all(row["kind"] == "joint_zero_sample" for row in rows) - reference = build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=2) - assert reference.joint == name and len(reference.samples) == 10 - - -def test_articulated_capture_discards_partial_moving_tags_and_records_cached_base(): - profile, lock, capture = make_capture() - profile = replace(profile, measurement=replace(profile.measurement, - candidate_selection_tasks=frozenset({"thumb_pitch_dip_front"}))) - capture.profile = profile - lock.begin_session() - lock.start_locking() - for i in range(14): - capture.consume(frame(profile, i+1)) - motion = MotionCommand("sweep", profile.command.baseline_values, 1, - "thumb_pitch_dip_front", 0, 0, "decreasing") - assert capture.consume(frame(profile, 20, omit=("thumb_dip",)), motion)[0] == () - samples, _ = capture.consume(frame(profile, 21, omit=("front_base",)), motion) - candidate = next(r for r in samples if r["kind"] == "pnp_candidate_frame") - assert candidate["roles"]["front_base"]["locked_reference"] is True - assert candidate["roles"]["front_base"]["corners_xy"] is None - assert candidate["roles"]["front_base"]["selected"] diff --git a/src/linkerhand_calibration/test/test_observation_feedback_dependencies.py b/src/linkerhand_calibration/test/test_observation_feedback_dependencies.py deleted file mode 100644 index 8059f08..0000000 --- a/src/linkerhand_calibration/test/test_observation_feedback_dependencies.py +++ /dev/null @@ -1,74 +0,0 @@ -"""Tag mounting topology, rather than hand identity, bounds geometry holds.""" -from pathlib import Path - -import pytest -import yaml - -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.observations import compile_tag_feedback_channels -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization -from test_branch_initialization import arrival - - -PACKAGE = Path(__file__).resolve().parents[1] - - -def product_model(model): - product = yaml.safe_load((PACKAGE/f'config/{model}_right_product.yaml').read_text()) - source = PACKAGE/product['artifacts']['source_urdf'].split('package://linkerhand_calibration/')[1] - return load_bundled_hand_profile(product['tag_layout']), UrdfKinematicModel(source) - - -@pytest.mark.parametrize('model', ['g20', 'l6', 'o6', 'o12', 'o30']) -def test_all_products_resolve_tag_dependencies_including_passive_sources(model): - profile, source = product_model(model) - dependencies = compile_tag_feedback_channels(profile, source) - for view in profile.vision.views: - for tag in view.tags: - assert tag.role in dependencies - assert dependencies[tag.role] <= set(range(profile.command.command_count)) - if not tag.fixed_reference: - assert dependencies[tag.role] - - -def test_shared_proximal_tag_depends_on_all_upstream_axes_but_not_ip(): - profile, source = product_model('o30') - dependencies = compile_tag_feedback_channels(profile, source) - assert dependencies['thumb_mcp'] == {0, 1, 6} - assert dependencies['thumb_ip'] == {0, 1, 6, 15} - assert dependencies['front_base'] == set() - - -@pytest.mark.parametrize('channel, invalid', [(15, False), (5, False), (6, True), (1, True)]) -def test_only_feedback_that_can_move_observed_tag_invalidates_preparation(channel, invalid): - profile, source = product_model('o30') - approach, zero, rows = arrival(profile) - owner = BranchInitialization(profile, image_geometry=False, source_urdf=source.source) - owner.begin(approach, session_epoch=1, motion_version=7) - for index, row in enumerate(rows): - if index >= len(rows)//2: - row['feedback_vector'][channel] += 2. - owner.observe(row) - owner.begin(zero, session_epoch=1, motion_version=8) - request = owner.request('front', zero, session_epoch=1, motion_version=8) - assert (request.invalid_reason == 'motion_held_feedback_changed') is invalid - if not invalid: - assert not request.invalid_reason - assert request.frames - # Out-of-chain feedback is still retained verbatim as diagnostic evidence. - assert any(row['feedback_vector'][channel] != row['command_vector'][channel] - for row in owner._source_records['front'].values()) - - -def test_unrelated_command_change_still_rejects_wrong_avoidance_pose(): - profile, source = product_model('o30') - approach, zero, rows = arrival(profile) - owner = BranchInitialization(profile, image_geometry=False, source_urdf=source.source) - owner.begin(approach, session_epoch=1, motion_version=7) - for row in rows: - row['command_vector'][5] += 1. - owner.observe(row) - owner.begin(zero, session_epoch=1, motion_version=8) - request = owner.request('front', zero, session_epoch=1, motion_version=8) - assert not request.frames diff --git a/src/linkerhand_calibration/test/test_observation_strategy.py b/src/linkerhand_calibration/test/test_observation_strategy.py deleted file mode 100644 index 0f23adf..0000000 --- a/src/linkerhand_calibration/test/test_observation_strategy.py +++ /dev/null @@ -1,171 +0,0 @@ -"""Measured and explicitly reused motions share one profile contract.""" - -from dataclasses import replace -import hashlib -from pathlib import Path -import xml.etree.ElementTree as ET - -import pytest -import yaml - -from linkerhand_calibration.core.domain.profile import ProfileKey, validate_profile -from linkerhand_calibration.profiles import dump_hand_profile, load_bundled_hand_profile, load_hand_profile -from linkerhand_calibration.profiles.validator import validate_executable_profile - - -PACKAGE = Path(__file__).resolve().parents[1] -PROFILES = ( - ("o6_right_8", "o6_right/linkerhand_o6_right.urdf", 6, ()), - ("l6_right_8", "l6_right/linkerhand_l6v3.1_right.urdf", 6, ()), - ("o12_right_16", "o12_right/linkerhand_o12_t3_right-0703.urdf", 3, ()), - ("g20_right_19", "g20_right/linkerhand_g20_right.urdf", 0, ()), -) - - -@pytest.mark.parametrize("layout,urdf,transfer_count,geometry_gaps", PROFILES) -def test_common_strategy_retains_measured_coverage_and_explicit_gaps(layout, urdf, transfer_count, geometry_gaps): - profile = load_bundled_hand_profile(layout) - report = validate_executable_profile(profile, PACKAGE / "urdf" / urdf) - coverage = report["independent_measurement_coverage"] - transfers = profile.measurement.transferred_motion_sources - assert len(transfers) == transfer_count - assert coverage["unmeasured_joints"] == sorted(transfers) - assert coverage["unresolved_motion_joints"] == [] - assert coverage["transferred_motion_sources"] == transfers - assert coverage["missing_baseline_geometry"] == list(geometry_gaps) - observed = {name for task in profile.motion.tasks for name in task.joints} - assert set(profile.motion.joint_zero_references) == observed - assert not set(transfers) & observed - assert set(transfers.values()) <= observed - assert not set(profile.zero.cad_zero_assumptions) & set(profile.zero.known_baseline_geometry) - product = yaml.safe_load((PACKAGE / "config" / f"{profile.key.model.lower()}_right_product.yaml").read_text()) - assert product["profile_config_sha256"] == hashlib.sha256( - (PACKAGE / "config" / "profiles" / f"{layout}.yaml").read_bytes()).hexdigest() - from linkerhand_calibration.profiles.validator import require_release_geometry - require_release_geometry(report) - assert profile.acquisition.command_capture_mode == "separate" - - -def test_old_geometry_transfer_does_not_authorize_copying_motion(): - profile = load_bundled_hand_profile("o6_right_8") - profile = replace(profile, measurement=replace(profile.measurement, transferred_motion_sources={})) - coverage = validate_executable_profile(profile, - PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf")["independent_measurement_coverage"] - assert len(coverage["unresolved_motion_joints"]) == 6 - assert coverage["transferred_motion_sources"] == {} - assert set(coverage["missing_baseline_geometry"]) == set(profile.zero.transferred_zero_sources) - from linkerhand_calibration.profiles.validator import require_release_geometry - with pytest.raises(ValueError, match="release_geometry_incomplete"): - require_release_geometry({"independent_measurement_coverage": coverage}) - - -@pytest.mark.parametrize("target,source", [ - ("missing_joint", "rh_pinky_mcp_pitch"), - ("rh_index_mcp_pitch", "missing_joint"), - ("rh_index_mcp_pitch", "rh_middle_mcp_pitch"), - ("rh_pinky_mcp_pitch", "rh_pinky_mcp_pitch"), - ("rh_thumb_cmc_pitch", "rh_pinky_mcp_pitch"), - ("rh_index_mcp_pitch", "rh_pinky_dip"), - ("rh_index_dip", "rh_thumb_ip"), -]) -def test_invalid_donors_types_and_chains_are_rejected(target, source): - profile = load_bundled_hand_profile("o6_right_8") - profile = replace(profile, measurement=replace(profile.measurement, - transferred_motion_sources={**profile.measurement.transferred_motion_sources, target: source})) - with pytest.raises(ValueError, match="motion"): - validate_profile(profile) - - -@pytest.mark.parametrize("field,value", [ - ("baseline_u8", 254), ("lower_bounds", 1), ("upper_bounds", 254), - ("sdk_to_joint_direction", 1), -]) -def test_recipient_domain_baseline_and_sign_must_match_donor(field, value): - profile = load_bundled_hand_profile("o6_right_8") - original = getattr(profile.command, field) - if not original: - original = profile.command.minimum_values if field == "lower_bounds" else profile.command.maximum_values - changed = list(original) - changed[2] = value - profile = replace(profile, command=replace(profile.command, **{field: tuple(changed)})) - with pytest.raises(ValueError, match="motion transfer requires equal SDK"): - validate_profile(profile) - - -def test_native_recipient_range_cannot_silently_rescale_donor_curve(): - profile = load_bundled_hand_profile("o12_right_16") - upper = list(profile.command.upper_bounds) - upper[10] -= 0.01 - profile = replace(profile, command=replace(profile.command, upper_bounds=tuple(upper))) - with pytest.raises(ValueError, match="motion transfer requires equal SDK"): - validate_profile(profile) - - -@pytest.mark.parametrize("field,delta", [ - ("feedback_lower_bounds", -0.01), ("feedback_upper_bounds", 0.01), -]) -def test_recipient_feedback_domain_must_match_donor_even_when_command_domain_matches(field, delta): - profile = load_bundled_hand_profile("o12_right_16") - original = (profile.command.minimum_feedback_values if field == "feedback_lower_bounds" - else profile.command.maximum_feedback_values) - changed = list(original) - changed[10] += delta - command = replace(profile.command, **{field: tuple(changed)}) - assert command.minimum_values[10] == command.minimum_values[11] - assert command.maximum_values[10] == command.maximum_values[11] - with pytest.raises(ValueError, match="motion transfer requires equal SDK"): - validate_profile(replace(profile, command=command)) - - -def test_recipient_cad_mimic_sign_is_checked(tmp_path): - profile = load_bundled_hand_profile("o6_right_8") - tree = ET.parse(PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf") - mimic = tree.getroot().find("joint[@name='rh_index_dip']/mimic") - mimic.set("multiplier", "-1") - source = tmp_path / "reversed_mimic.urdf" - tree.write(source) - with pytest.raises(ValueError, match="equal CAD output signs"): - validate_executable_profile(profile, source) - - -@pytest.mark.parametrize("joint,reason", [ - ("missing", "accepted"), ("rh_pinky_dip", " "), - ("rh_thumb_cmc_pitch", "accepted"), ("rh_index_mcp_pitch", "accepted"), -]) -def test_cad_assumptions_cannot_hide_invalid_or_conflicting_zero_sources(joint, reason): - profile = load_bundled_hand_profile("o6_right_8") - profile = replace(profile, zero=replace(profile.zero, - cad_zero_assumptions={**profile.zero.cad_zero_assumptions, joint: reason})) - with pytest.raises(ValueError, match="CAD zero assumption"): - validate_profile(profile) - - -def test_virtual_model_can_reorder_channels_views_and_tasks(tmp_path): - profile = load_bundled_hand_profile("o6_right_8") - order = (5, 3, 1, 4, 0, 2) - index = {old: new for new, old in enumerate(order)} - reorder = lambda row: tuple(row[i] for i in order) - views = {"front": "inspection_a", "side": "inspection_b", "top": "inspection_c"} - command = replace(profile.command, names=reorder(profile.command.names), - baseline_u8=reorder(profile.command.baseline_u8), - sdk_to_joint_direction=reorder(profile.command.sdk_to_joint_direction), - command_index_by_joint={name: index[channel] for name, channel in profile.command.command_index_by_joint.items()}) - motion = replace(profile.motion, - tasks=tuple(replace(task, view=views[task.view], command_index=index[task.command_index]) - for task in reversed(profile.motion.tasks)), - joint_zero_references={name: replace(reference, command=reorder(reference.command), - approach_commands=tuple(reorder(row) for row in reference.approach_commands)) - for name, reference in profile.motion.joint_zero_references.items()}) - vision = replace(profile.vision, views=tuple(replace(view, name=views[view.name]) - for view in reversed(profile.vision.views)), extrinsic_reference_view=views[profile.vision.extrinsic_reference_view]) - measurement = replace(profile.measurement, measurements={name: replace(spec, view=views[spec.view]) - for name, spec in profile.measurement.measurements.items()}) - virtual = replace(profile, key=ProfileKey("virtual", "right", "mixed_strategy"), - command=command, motion=motion, vision=vision, measurement=measurement) - path = tmp_path / "virtual.yaml" - path.write_text(dump_hand_profile(virtual)) - virtual = load_hand_profile(path) - report = validate_executable_profile(virtual, PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf") - assert not report["independent_measurement_coverage"]["unresolved_motion_joints"] - assert virtual.command.command_index_by_joint["rh_index_mcp_pitch"] == 5 - assert virtual.command.command_index_by_joint["rh_pinky_mcp_pitch"] == 0 diff --git a/src/linkerhand_calibration/test/test_observation_timing.py b/src/linkerhand_calibration/test/test_observation_timing.py deleted file mode 100644 index 25aed78..0000000 --- a/src/linkerhand_calibration/test/test_observation_timing.py +++ /dev/null @@ -1,134 +0,0 @@ -"""Report image loss separately from visibility and synchronization failures.""" - -from copy import deepcopy - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.observation_timing import summarize_observation_timing - - -@pytest.fixture -def capture(): - 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") - tags = next(view.tags for view in profile.vision.views if view.name == "front") - rows = [] - for index in range(5): - stamp = 1_000_000_000 + index * 100_000_000 - rows.append({"kind": "diagnostic_observation_frame", "sample_phase": "sweep", - "task_name": unit.task_key, "cycle": unit.cycle, "direction": unit.direction, - "attempt": 1, "view": "front", "image_stamp_ns": stamp, - "command_unit": profile.command.unit, - "command_vector": list(profile.command.baseline_values), - "feedback_vector": list(profile.command.baseline_values), - "tags": {tag.role: {"tag_role": tag.role, "tag_id": tag.tag_id, - "corners_xy": [[10, 10], [50, 10], [50, 50], [10, 50]], - "candidate_diagnostics": {"observation_stamp_ns": stamp}} - for tag in tags if not tag.fixed_reference}}) - return profile, unit, rows - - -def ip_metrics(capture, rows=None): - profile, unit, original = capture - result = summarize_observation_timing(profile, unit, 1, original if rows is None else rows) - return result[f"joint:rh_thumb_ip:front:{unit.direction}"] - - -def test_current_moving_tags_are_visible_without_fixed_tag_or_selected_pose(capture): - before = deepcopy(capture[2]) - metrics = ip_metrics(capture) - assert metrics == {"raw_frames": 5, "visible_frames": 5, "synchronized_frames": 5, - "missing_command_frames": 0, "missing_feedback_frames": 0, - "maximum_image_gap_seconds": 0.1, "maximum_synchronized_gap_seconds": 0.1} - assert capture[2] == before - assert "passed" not in metrics - - -def test_command_and_feedback_gaps_do_not_disappear_into_image_counts(capture): - rows = capture[2] - rows[1]["command_vector"] = None - rows[2]["feedback_vector"] = None - rows[3]["command_vector"] = [] - rows[3]["feedback_vector"] = [float("nan")] * len(rows[3]["feedback_vector"]) - metrics = ip_metrics(capture) - assert metrics["raw_frames"] == metrics["visible_frames"] == 5 - assert metrics["synchronized_frames"] == 2 - assert metrics["missing_command_frames"] == metrics["missing_feedback_frames"] == 2 - assert metrics["maximum_image_gap_seconds"] == 0.1 - assert metrics["maximum_synchronized_gap_seconds"] == 0.4 - - -@pytest.mark.parametrize("role", ["thumb_pitch", "thumb_ip"]) -@pytest.mark.parametrize("fault", ["missing", "stale", "id", "role", "corners", "not_finite"]) -def test_both_moving_tags_need_current_correctly_bound_corners(capture, role, fault): - rows = capture[2] - for row in rows[1:4]: - item = row["tags"][role] - if fault == "missing": - del row["tags"][role] - elif fault == "stale": - item["candidate_diagnostics"]["observation_stamp_ns"] -= 1 - elif fault == "id": - item["tag_id"] = -1 - elif fault == "role": - item["tag_role"] = "unrelated" - elif fault == "corners": - item["corners_xy"] = None - else: - item["corners_xy"][0][0] = float("inf") - metrics = ip_metrics(capture) - assert metrics["raw_frames"] == 5 - assert metrics["visible_frames"] == metrics["synchronized_frames"] == 2 - assert metrics["missing_command_frames"] == metrics["missing_feedback_frames"] == 0 - assert metrics["maximum_synchronized_gap_seconds"] == 0.4 - - -def test_duplicate_images_are_counted_once_and_not_merged(capture): - profile, unit, rows = capture - original = deepcopy(rows) - assert ip_metrics(capture, rows + deepcopy(rows)) == ip_metrics(capture, original) - rows[2]["command_vector"] = None - duplicates = deepcopy(rows) - duplicates[2]["command_vector"] = list(profile.command.baseline_values) - duplicates[2]["feedback_vector"] = None - metrics = ip_metrics(capture, rows + duplicates) - assert metrics["raw_frames"] == 5 - assert metrics["synchronized_frames"] == 4 - assert metrics["missing_command_frames"] == 1 - assert metrics["maximum_synchronized_gap_seconds"] == 0.2 - - -@pytest.mark.parametrize("field,value", [ - ("kind", "joint_sample"), ("sample_phase", "joint_zero"), - ("task_name", "thumb_yaw_top"), ("cycle", 1), ("direction", "increasing"), - ("attempt", 2), ("view", "side"), ("image_stamp_ns", None), - ("image_stamp_ns", 0), ("image_stamp_ns", True), -]) -def test_other_scopes_and_invalid_image_identities_do_not_supply_frames(capture, field, value): - for row in capture[2]: - row[field] = value - metrics = ip_metrics(capture) - assert metrics["raw_frames"] == metrics["visible_frames"] == metrics["synchronized_frames"] == 0 - assert metrics["maximum_image_gap_seconds"] is None - assert metrics["maximum_synchronized_gap_seconds"] is None - - -@pytest.mark.parametrize("count", [0, 1]) -def test_fewer_than_two_images_cannot_establish_a_gap(capture, count): - metrics = ip_metrics(capture, capture[2][:count]) - assert metrics["raw_frames"] == count - assert metrics["maximum_image_gap_seconds"] is None - assert metrics["maximum_synchronized_gap_seconds"] is None - - -def test_incorrect_native_unit_is_not_a_synchronized_vector(capture): - for row in capture[2]: - row["command_unit"] = "rad" - metrics = ip_metrics(capture) - assert metrics["raw_frames"] == metrics["visible_frames"] == 5 - assert metrics["synchronized_frames"] == 0 - assert metrics["missing_command_frames"] == metrics["missing_feedback_frames"] == 5 - assert metrics["maximum_synchronized_gap_seconds"] is None diff --git a/src/linkerhand_calibration/test/test_observer_policy.py b/src/linkerhand_calibration/test/test_observer_policy.py deleted file mode 100644 index ace3b05..0000000 --- a/src/linkerhand_calibration/test/test_observer_policy.py +++ /dev/null @@ -1,71 +0,0 @@ -"""Select observer strategies by dependencies without reinterpreting old passes.""" -from dataclasses import replace - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import STAGED, TASKWISE_TWO_ROUND, configure_training -from linkerhand_calibration.profiles import load_bundled_hand_profile, load_hand_profile -from linkerhand_calibration.profiles.export import write_hand_profile -from linkerhand_calibration.profiles.observer_policy import ( - configure_observer_policy, rigid_observer_dependencies, RIGID_OBSERVER, LEGACY_OBSERVER, -) -from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_contract -from linkerhand_calibration.profiles.validator import validate_executable_profile -from o30_capture_fixture import SOURCE - - -@pytest.fixture(params=[STAGED, TASKWISE_TWO_ROUND]) -def staged(request): - return configure_training(load_bundled_hand_profile('o30_right_18'), request.param) - - -def test_new_preparation_covers_every_independent_parent_hinge(staged, tmp_path): - old = configure_preparation(staged, 'occlusion_aware_witness_v1') - new = configure_preparation(staged) - dependencies = rigid_observer_dependencies(new) - assert dependencies == {'thumb_ip': 'thumb_mcp', **{ - f'{finger}_dip': f'{finger}_pip' for finger in ('pinky', 'ring', 'middle', 'index')}} - assert all(s.observer_pose_policy == LEGACY_OBSERVER for s in old.measurement.measurements.values()) - assert {j for j, s in new.measurement.measurements.items() - if s.observer_pose_policy == RIGID_OBSERVER} == dependencies.keys() - assert old.motion == new.motion and old.quality == new.quality - assert preparation_contract(old) != preparation_contract(new) - validate_executable_profile(new, SOURCE) - path = tmp_path/'new.yaml' - write_hand_profile(new, path) - assert load_hand_profile(path) == new - assert configure_observer_policy(new) == new - - -def test_passed_measurements_keep_their_original_strategy(staged): - preserved = set(staged.measurement.measurements) - {'index_dip'} - new = configure_observer_policy(staged, preserved_joints=preserved) - for joint in preserved: - assert new.measurement.measurements[joint] == staged.measurement.measurements[joint] - assert new.measurement.measurements['index_dip'].observer_pose_policy == RIGID_OBSERVER - assert new.motion == staged.motion - with pytest.raises(ValueError, match='unknown_preserved_joint'): - configure_observer_policy(staged, preserved_joints={'nonexistent'}) - - -def test_dependency_compilation_uses_roles_not_finger_names(staged): - # A valid-looking name cannot enable a disconnected observation. - specs = dict(staged.measurement.measurements) - specs['index_dip'] = replace(specs['index_dip'], parent_role='middle_pip') - changed = replace(staged, measurement=replace(staged.measurement, measurements=specs)) - assert 'index_dip' not in rigid_observer_dependencies(changed) - assert configure_observer_policy(changed).measurement.measurements['index_dip'].observer_pose_policy == LEGACY_OBSERVER - - -def test_unsupported_legacy_modes_are_not_silently_changed(): - old = load_bundled_hand_profile('o30_right_18') - assert rigid_observer_dependencies(old) == {} - assert configure_observer_policy(old) == old - - -def test_forced_strategy_without_independent_hinge_is_rejected(staged): - specs = dict(staged.measurement.measurements) - specs['index_mcp_pitch'] = replace(specs['index_mcp_pitch'], observer_pose_policy=RIGID_OBSERVER) - changed = replace(staged, measurement=replace(staged.measurement, measurements=specs)) - with pytest.raises(ValueError, match='rigid_pair_requires_independent_parent_hinge'): - validate_executable_profile(changed, SOURCE) diff --git a/src/linkerhand_calibration/test/test_observer_pose.py b/src/linkerhand_calibration/test/test_observer_pose.py deleted file mode 100644 index 07ad19d..0000000 --- a/src/linkerhand_calibration/test/test_observer_pose.py +++ /dev/null @@ -1,136 +0,0 @@ -"""Independent observer images may constrain a family, never waive ambiguity.""" - -from dataclasses import replace -import math - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe -from linkerhand_calibration.core.geometry.tag_pose.observer_pose import fit_observer_pose -from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - - -@pytest.fixture(scope='module') -def observer_images(): - matrix = np.array([[1600., 0, 640.], [0, 1600., 480.], [0, 0, 1.]]) - parents, candidates, corners = [], [], [] - relative = Rotation.identity() - translation = np.array([.045, .02, 0.]) - for theta in np.linspace(-35, 35, 48): - rotation = Rotation.from_euler('xy', [theta, theta*.4], degrees=True) - origin = np.array([.015, -.02, .5]) - parents.append(SquareTagPose(tuple(rotation.as_quat()), tuple(origin), 0.)) - points = rotation.apply(relative.apply(square_object_points(.016))+translation)+origin - projected = points@matrix.T - pixels = projected[:, :2]/projected[:, 2:] - corners.append(pixels) - candidates.append(tuple(solve_square_tag_ippe(pixels, tag_size_m=.016, camera_matrix=matrix))) - return parents, candidates, corners, [matrix]*48, .016, tuple(range(1, 49)) - - -def test_all_legal_candidates_use_fixed_split_and_propagate_parent_uncertainty(observer_images): - inherited = (math.radians(.05), .00005) - result = fit_observer_pose(*observer_images, inherited=inherited) - assert np.allclose(result.translation_xyz_m, [.045, .02, 0.], atol=1e-8) - assert Rotation.from_quat(result.quaternion_xyzw).magnitude() < 1e-6 - assert result.rotation_uncertainty_rad >= inherited[0] - assert result.translation_uncertainty_m >= inherited[1] - assert set(result.training_stamps).isdisjoint(result.validation_stamps) - assert result.training_stamps == tuple(range(1, 49, 2)) - - -def test_rigid_pair_recovers_truth_from_biased_parent_initialization(observer_images): - from linkerhand_calibration.core.geometry.tag_pose.rigid_tag_pair import fit_rigid_tag_pair - parents, candidates, corners, matrices, size, stamps = observer_images - parent_pixels, biased = [], [] - for parent, matrix in zip(parents, matrices): - rotation = Rotation.from_quat(parent.quaternion_xyzw) - points = rotation.apply(square_object_points(size)) + parent.translation_xyz_m - projected = points @ matrix.T - parent_pixels.append(projected[:, :2] / projected[:, 2:]) - biased.append(replace(parent, - quaternion_xyzw=tuple((Rotation.from_euler('x', 2., degrees=True) * rotation).as_quat()), - translation_xyz_m=tuple(np.asarray(parent.translation_xyz_m) + [.005, -.003, .002]))) - result = fit_rigid_tag_pair(biased, candidates, corners, matrices, size, stamps, - parent_corners=parent_pixels, parent_size=size) - assert np.allclose(result.translation_xyz_m, [.045, .02, 0.], atol=1e-6) - assert Rotation.from_quat(result.quaternion_xyzw).magnitude() < 1e-4 - assert 0 < result.rotation_uncertainty_rad < math.radians(1) - assert 0 < result.translation_uncertainty_m < .001 - - -def test_complete_parent_and_child_candidates_recover_known_installation(observer_images): - from linkerhand_calibration.core.geometry.tag_pose.rigid_tag_pair import fit_rigid_tag_pair - parents, candidates, corners, matrices, size, stamps = observer_images - parent_pixels, parent_candidates = [], [] - for parent, matrix in zip(parents, matrices): - points = Rotation.from_quat(parent.quaternion_xyzw).apply(square_object_points(size))+parent.translation_xyz_m - projected = points@matrix.T - pixels = projected[:,:2]/projected[:,2:] - parent_pixels.append(pixels) - parent_candidates.append(tuple(solve_square_tag_ippe(pixels, tag_size_m=size, camera_matrix=matrix))) - result = fit_rigid_tag_pair(*observer_images, parent_corners=parent_pixels, - parent_size=size, parent_candidates=parent_candidates) - assert np.linalg.norm(np.asarray(result.translation_xyz_m)-[.045,.02,0.]) < 1e-6 - assert Rotation.from_quat(result.quaternion_xyzw).magnitude() < 1e-4 - assert set(result.training_stamps).isdisjoint(result.validation_stamps) - - -@pytest.mark.parametrize('inherited', [(math.radians(2.274), .0005), (0., .001)]) -def test_exhausted_parent_budget_is_rejected_before_fitting(observer_images, monkeypatch, inherited): - monkeypatch.setattr('linkerhand_calibration.core.geometry.tag_pose.observer_pose.least_squares', - lambda *a, **kw: pytest.fail('fitting cannot recover spent parent precision')) - with pytest.raises(ValueError, match='parent_precision_budget_exhausted'): - fit_observer_pose(*observer_images, inherited=inherited) - - -@pytest.mark.parametrize('fault', ['missing', 'moving', 'installation', 'unobservable', 'uncertain_parent']) -def test_invalid_observer_evidence_cannot_authorize_a_pose(observer_images, fault): - parents, candidates, corners, matrices, size, stamps = observer_images - inherited = (0., 0.) - if fault == 'missing': - parents, candidates, corners, matrices, stamps = [v[:10] for v in (parents, candidates, corners, matrices, stamps)] - elif fault in {'moving', 'installation'}: - corners = [np.array(pixels, copy=True) for pixels in corners] - for i in (range(1, len(corners), 2) if fault == 'moving' else range(len(corners)//2, len(corners))): - corners[i] += [10., -10.] - elif fault == 'unobservable': - parents = [parents[0]]*len(parents) - else: - inherited = (math.radians(2), .01) - with pytest.raises(ValueError, match='observer_'): - fit_observer_pose(parents, candidates, corners, matrices, size, stamps, inherited=inherited) - - -@pytest.mark.integration -def test_measured_observer_pose_resolves_only_a_separated_hinge_family(observer_images): - from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion - from test_motion_image_diagnostics import frames, RELATIONS - motion = frames(alternatives=True) - unresolved = resolve_image_motion(motion, RELATIONS) - assert not unresolved.resolved and unresolved.reason == 'image_motion_families_not_distinguishable' - measured = fit_observer_pose(*observer_images) - stationary = tuple(replace(motion[0], evidence=replace(motion[0].evidence, - stamp_ns=10_000_000_000+i*30_000_000)) for i in range(10)) - bridge = SharedTagPoseBridge(RELATIONS[0], measured.quaternion_xyzw, measured.translation_xyz_m, - stationary, rotation_uncertainty_rad=measured.rotation_uncertainty_rad, - translation_uncertainty_m=measured.translation_uncertainty_m) - result = resolve_image_motion(motion, RELATIONS, pose_bridges=(bridge,)) - assert result.resolved, result - assert all(c.status == 'consistent' for h in result.hypotheses if h.branches == result.model.branches - for c in h.pose_bridge_checks) - - -def test_observer_roles_do_not_change_the_current_task_or_require_visibility(): - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from linkerhand_calibration.runtime.observer_reference import observer_roles - profile = load_bundled_hand_profile('o30_right_18') - spec = profile.motion.joint_zero_references['middle_pip'] - motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, reference_joints=('middle_pip',)) - assert 'middle_dip' in observer_roles(profile, motion) - assert observer_roles(profile, replace(motion, phase='sweep')) == set() - assert motion.reference_joints == ('middle_pip',) diff --git a/src/linkerhand_calibration/test/test_observer_preparation.py b/src/linkerhand_calibration/test/test_observer_preparation.py deleted file mode 100644 index 16d950e..0000000 --- a/src/linkerhand_calibration/test/test_observer_preparation.py +++ /dev/null @@ -1,352 +0,0 @@ -"""Bounded observation acquires new information without changing accepted joints.""" -from copy import deepcopy -from dataclasses import asdict, replace -from types import SimpleNamespace as NS - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.domain.capture_plan import STAGED, configure_training -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe -from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.profiles import load_bundled_hand_profile, load_hand_profile -from linkerhand_calibration.profiles.export import write_hand_profile -from linkerhand_calibration.profiles.observer_preparation import configure_observer_preparations, observer_actions -from linkerhand_calibration.profiles.validator import validate_executable_profile -from linkerhand_calibration.runtime.observer_preparation import ObserverPreparation, ObserverProbeEvidence, normal_coverage, POLICY -from linkerhand_calibration.runtime.observer_preparation_evidence import validate_probe_evidence, build_probe_bridge -from linkerhand_calibration.runtime.observer_reference import ObserverBridgeInput -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 -from linkerhand_calibration.runtime.joint_resume import _index_motion_provenance -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope='module') -def active_profile(): - profile = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) - return configure_observer_preparations(profile, UrdfKinematicModel(SOURCE), - preserved_joints=set(profile.measurement.measurements)-{'index_dip'}) - - -def test_compile_bounds_preserves_all_existing_measurements(active_profile, tmp_path): - original = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) - assert set(active_profile.motion.observer_preparations) == {'index_dip'} - plan = observer_actions(active_profile, 'index_dip')[0] - assert plan.joint == 'index_mcp_roll' - assert plan.command == original.motion.joint_zero_references['index_dip'].command - assert abs(plan.approach_commands[1][2]-plan.command[2]) == 63 - for name in set(original.measurement.measurements)-{'index_dip'}: - assert original.measurement.measurements[name] == active_profile.measurement.measurements[name] - assert original.motion.tasks == active_profile.motion.tasks - assert original.quality == active_profile.quality - validate_executable_profile(active_profile, SOURCE) - path = tmp_path/'active.yaml';write_hand_profile(active_profile,path) - assert load_hand_profile(path) == active_profile - - -@pytest.fixture(scope='module') -def synthetic_probe(active_profile): - profile = active_profile - plan = observer_actions(profile, 'index_dip')[0] - matrix = np.array([[3700.,0.,1024.],[0.,3700.,768.],[0.,0.,1.]]) - spec = profile.measurement.measurements['index_dip'] - tags = {t.role:t for v in profile.vision.views if v.name==spec.view for t in v.tags} - relative = Rotation.from_euler('xyz',[.02,.04,-.03]) - translation = np.array([.001,-.024,.008]) - decision = dict(kind='observer_preparation_decision',policy=POLICY,joint='index_dip', - task_name='index_dip_side',session_epoch=1,stamp_ns=1,action='excite',donor=plan.joint, - path=[list(p) for p in (*plan.approach_commands,plan.command)]) - identity = source_frame_sha256(decision) - rows,segments=[],[] - stamp=1_000_000_000 - def image(value): - rotation=Rotation.from_euler('xyz',[.2+(255-value)/63*.6,.12,.08]) - origin=np.array([.01,-.02,.89]) - output={} - for role,r,t in [(spec.parent_role,rotation,origin), - (spec.child_role,rotation*relative,origin+rotation.apply(translation))]: - p=tags[role] - camera=r.apply(square_object_points(p.size_m))+t - projected=camera@matrix.T - pixels=projected[:,:2]/projected[:,2:] - poses=solve_square_tag_ippe(pixels,tag_size_m=p.size_m,camera_matrix=matrix) - output[role]=dict(tag_id=p.tag_id,tag_size_m=p.size_m,corners_xy=pixels.tolist(), - selected_pose=asdict(SquareTagPose(tuple(r.as_quat()),tuple(t),0.)), - reprojection_valid_candidates=[asdict(p) for p in poses], - candidate_diagnostics=dict(observation_stamp_ns=stamp,branch_frozen=role==spec.parent_role)) - return output - for index,target in enumerate((*plan.approach_commands,plan.command)): - start=plan.command[2] if index==0 else plan.approach_commands[index-1][2] - values=[target[2]]*12 if index==0 else [*np.round(np.linspace(start,target[2],49)),*[target[2]]*12] - for value in values: - stamp+=33_333_333 - command=list(plan.command);command[2]=float(value) - rows.append(dict(kind='image_observation_frame',sample_phase='observer_probe',view='side', - image_stamp_ns=stamp,session_epoch=1,motion_version=index+1,reference_path_index=index, - task_name='index_dip_side',observer_joint='index_dip',observer_preparation_id=identity, - command_vector=command,feedback_vector=command,camera_matrix=matrix.tolist(),tags=image(value))) - segments.append(dict(kind='observer_preparation_segment_complete',observer_preparation_id=identity, - path_index=index,session_epoch=1,motion_version=index+1,stamp_ns=stamp+1_000_000)) - stamp+=2_000_000 - endpoints=[] - for i in range(10): - stamp+=33_333_333 - endpoints.append(dict(kind='motion_branch_observation',view='side',image_stamp_ns=stamp, - task_name='index_dip_side',session_epoch=1,motion_version=4,zero_joints=['index_dip'], - sample_phase='zero_approach',command_vector=list(plan.command),feedback_vector=list(plan.command), - camera_matrix=matrix.tolist(),tags=image(plan.command[2]))) - geometry=NS(relation=NS(child_role=spec.parent_role)) - model=NS(camera_matrix=tuple(map(tuple,matrix)),geometry=(geometry,)) - first_motion=next(r for r in rows if r['reference_path_index']==1) - started=dict(kind='observer_preparation_excursion_started',observer_preparation_id=identity, - session_epoch=1,motion_version=2,stamp_ns=first_motion['image_stamp_ns']-1_000_000) - source=ObserverBridgeInput('index_dip',model,dict(image_model_sha256='a'*64,evidence_ids={spec.parent_role:'parent'}), - (),tuple(endpoints),(16,),ObserverProbeEvidence(decision,tuple(rows),tuple(segments),started)) - return source,relative,translation - - -def test_tilt_diversity_not_image_plane_rotation(active_profile, synthetic_probe): - source,_,_=synthetic_probe - rows=source.preparation.rows - assert not normal_coverage(rows,'index_pip')['needs_excitation'] - flat=[] - template=rows[0] - matrix=np.asarray(template['camera_matrix']) - points=square_object_points(template['tags']['index_pip']['tag_size_m']) - for i,angle in enumerate(np.linspace(0,100,40)): - rotation=Rotation.from_euler('z',angle,degrees=True)*Rotation.from_euler('x',.05) - xyz=rotation.apply(points)+[.01,.01,.89] - projected=xyz@matrix.T - row=deepcopy(template);row['image_stamp_ns']=i+1 - row['tags']['index_pip']['corners_xy']=(projected[:,:2]/projected[:,2:]).tolist();flat.append(row) - coverage=normal_coverage(flat,'index_pip') - assert coverage['needs_excitation'] and coverage['normal_span_deg']<10 - - -def test_independent_probe_recovers_true_installation(active_profile, synthetic_probe): - source,rotation,translation=synthetic_probe - bridge,record=build_probe_bridge(active_profile,source) - assert np.linalg.norm(np.asarray(bridge.translation_xyz_m)-translation)<1e-6 - assert (Rotation.from_quat(bridge.quaternion_xyzw).inv()*rotation).magnitude()<1e-5 - assert bridge.rotation_uncertainty_rad=24 - assert controller.cancel_unstarted('index_dip',epoch=2,stamp_ns=source.segments[-1]['stamp_ns']+1) is None - motion=MotionCommand('observer_preflight',(),200,task_key='index_dip_side',reference_joints=('index_dip',)) - extension=controller.decide(motion,epoch=2,stamp_ns=source.segments[-1]['stamp_ns']+1) - assert extension['action_index']==1 and extension['action_limit']==2 - assert extension['previous_preparation_id']==source_frame_sha256(source.decision) - assert extension['path'][1][2]==0 - restored=ObserverPreparation(NS(profile=active_profile)); restored.restore([*records,extension]) - assert len(restored.history['index_dip'])==1 - assert restored.decisions['index_dip']==extension - reused=deepcopy(extension); reused['stamp_ns']+=1 - with pytest.raises(ValueError,match='budget_reused'): - restored.observe(reused) - skipped=ObserverPreparation(NS(profile=active_profile)) - with pytest.raises(ValueError,match='previous_action_missing'): - skipped.observe(extension) - with pytest.raises(ValueError,match='incomplete_checkpoint'): - controller.decide(motion,epoch=2,stamp_ns=source.segments[-1]['stamp_ns']+2) - reference=read_joint_zero_references(active_profile,[data['reference']])['index_mcp_pitch'] - channels=compile_tag_feedback_channels(active_profile,UrdfKinematicModel(SOURCE)) - task=next(t for t in active_profile.motion.tasks if t.key=='index_dip_side') - held=channels[active_profile.measurement.measurements['index_mcp_pitch'].child_role] - with pytest.raises(ValueError,match='held_feedback_changed'): - check_parent_pose(active_profile,task,reference,data['current'],held) - check_parent_pose(active_profile,task,reference,data['current'],held, - verification=REVISIT_POLICY,feedback_limits=revisit_feedback_limits(active_profile)) - assert reference.as_record()==data['reference'] or json.loads(json.dumps(reference.as_record()))==data['reference'] - - -def test_single_trajectory_action_declares_budget_and_cannot_escalate(active_profile, synthetic_probe): - from linkerhand_calibration.core.domain.profile import ObserverPreparationPlan - from linkerhand_calibration.profiles.observer_policy import TRAJECTORY_OBSERVER - from linkerhand_calibration.runtime.observer_preparation import TRAJECTORY_POLICY - source, _, _ = synthetic_probe - joint = 'index_dip' - specs = dict(active_profile.measurement.measurements) - specs[joint] = replace(specs[joint], observer_pose_policy=TRAJECTORY_OBSERVER) - profile = replace(active_profile, measurement=replace(active_profile.measurement, measurements=specs), - motion=replace(active_profile.motion, observer_preparations={joint: - ObserverPreparationPlan((observer_actions(active_profile, joint)[0],))})) - controller = ObserverPreparation(NS(profile=profile)) - motion = MotionCommand('observer_preflight', (), 200, task_key='index_dip_side', reference_joints=(joint,)) - # No sources requires excitation; dependency registry is empty. - controller.owner.parent_references = NS(models={}) - controller.owner.observer_records = {} - decision = controller.decide(motion, epoch=1, stamp_ns=1) - assert decision['policy'] == TRAJECTORY_POLICY - assert decision['action_limit'] == 1 and decision['action_index'] == 0 - first = source.preparation - identity = source_frame_sha256(decision) - rows = tuple(dict(r, observer_preparation_id=identity) for r in first.rows) - segments = tuple(dict(s, observer_preparation_id=identity) for s in first.segments) - started = dict(first.started, observer_preparation_id=identity) - evidence = ObserverProbeEvidence(decision, rows, segments, started) - validate_probe_evidence(profile, joint, evidence) - restored = ObserverPreparation(NS(profile=profile)) - restored.restore([decision, started, *rows, *segments]) - assert restored.decide(motion, epoch=2, stamp_ns=segments[-1]['stamp_ns']+1, - reason='rigid_pair_candidates_unresolved') is None - with pytest.raises(ValueError, match='action_budget_changed'): - validate_probe_evidence(profile, joint, replace(evidence, - decision=dict(decision, action_limit=2))) - - -def test_two_completed_actions_exhaust_budget_across_restart(active_profile, synthetic_probe): - source,_,_=synthetic_probe - first=source.preparation - records=[first.decision,first.started,*first.rows,*first.segments] - controller=ObserverPreparation(NS(profile=active_profile));controller.restore(records) - motion=MotionCommand('observer_preflight',(),200,task_key='index_dip_side',reference_joints=('index_dip',)) - shift=first.segments[-1]['stamp_ns']+1_000_000_000 - decision=controller.decide(motion,epoch=1,stamp_ns=shift,reason='rigid_pair_candidates_unresolved') - identity=source_frame_sha256(decision) - rows=deepcopy(first.rows) - for row in rows: - row.update(observer_preparation_id=identity,image_stamp_ns=row['image_stamp_ns']+shift, - motion_version=row['motion_version']+3) - for field in ('command_vector','feedback_vector'): - row[field]=list(row[field]) - row[field][2]=255-(255-row[field][2])*255/63 - for item in row['tags'].values(): - item['candidate_diagnostics']['observation_stamp_ns']=row['image_stamp_ns'] - segments=tuple(dict(s,observer_preparation_id=identity,stamp_ns=s['stamp_ns']+shift, - motion_version=s['motion_version']+3) for s in first.segments) - started=dict(first.started,observer_preparation_id=identity,stamp_ns=first.started['stamp_ns']+shift, - motion_version=first.started['motion_version']+3) - second=ObserverProbeEvidence(decision,rows,segments,started) - validate_probe_evidence(active_profile,'index_dip',second) - restored=ObserverPreparation(NS(profile=active_profile)) - restored.restore([*records,decision,started,*rows,*segments]) - assert restored.source('index_dip')==second - assert restored.decide(motion,epoch=2,stamp_ns=segments[-1]['stamp_ns']+1, - reason='rigid_pair_candidates_unresolved') is None - assert len(restored.history['index_dip'])==1 diff --git a/src/linkerhand_calibration/test/test_observer_reference.py b/src/linkerhand_calibration/test/test_observer_reference.py deleted file mode 100644 index ff3f04e..0000000 --- a/src/linkerhand_calibration/test/test_observer_reference.py +++ /dev/null @@ -1,215 +0,0 @@ -"""Raw observer pixels traverse the runtime bridge admission boundary.""" - -from copy import deepcopy -from dataclasses import asdict, replace -from types import SimpleNamespace as NS - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion -from linkerhand_calibration.runtime.observer_reference import ObserverBridgeInput, build_observer_bridge -from linkerhand_calibration.runtime.branch_initialization import BranchInitializationRequest, initialization_artifacts, solve_initialization -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 -from test_motion_image_diagnostics import frames, RELATIONS, MATRIX - - -@pytest.fixture(scope='module') -def observer_source(): - source = frames() - # A tilted upstream axis provides genuinely different camera directions; - # rolling a frontal plane in place must remain ambiguous. - turn, origin = Rotation.from_euler('y', 25, degrees=True), np.array([0., 0., .8]) - transformed = [] - for frame in source: - roles, observations = [], [] - for role, observation in zip(frame.evidence.roles, frame.observations): - pose = role.candidates[0] - rotation = turn*Rotation.from_quat(pose.quaternion_xyzw) - if role.role == 'moving': - rotation = rotation*Rotation.from_euler('x', 20, degrees=True) - translation = origin+turn.apply(np.asarray(pose.translation_xyz_m)-origin) - pose = replace(pose, quaternion_xyzw=tuple(rotation.as_quat()), translation_xyz_m=tuple(translation)) - projected = (rotation.apply(square_object_points(.016))+translation)@np.asarray(MATRIX).T - roles.append(replace(role, candidates=(pose,))) - observations.append(replace(observation, corners_xy=tuple(map(tuple, projected[:, :2]/projected[:, 2:])))) - transformed.append(replace(frame, evidence=replace(frame.evidence, roles=tuple(roles)), observations=tuple(observations))) - source = tuple(transformed) - profile = NS(command=NS(unit='u8', command_index_by_joint={'hinge': 0, 'distal': 1}), - acquisition=NS(fixed_reference_minimum_frames=10), - motion=NS(joint_zero_references={'distal': NS(command=(255., 0.))}), - measurement=NS(measurements={'distal': NS(view='view', parent_role='moving', child_role='terminal', - observer_pose_policy='frozen_parent_v1')}), - vision=NS(views=(NS(name='view', tags=(NS(role='terminal', size_m=.016),)),))) - rows = [] - for i, frame in enumerate(source): - stamp = frame.evidence.stamp_ns - tags = {} - for role, observation in zip(frame.evidence.roles, frame.observations): - tags[role.role] = dict(tag_role=role.role, tag_size_m=.016, corners_xy=observation.corners_xy, - selected_pose=asdict(role.candidates[0]), reprojection_valid_candidates=[asdict(p) for p in role.candidates], - candidate_diagnostics=dict(observation_stamp_ns=stamp, branch_frozen=role.frozen)) - parent = frame.evidence.roles[1].candidates[0] - rotation = Rotation.from_quat(parent.quaternion_xyzw) - points = rotation.apply(Rotation.from_euler('x', 20, degrees=True).apply(square_object_points(.016)) - + [.03, .01, -.005]) + parent.translation_xyz_m - projected = points@np.asarray(MATRIX).T - pixels = projected[:, :2]/projected[:, 2:] - candidates = solve_square_tag_ippe(pixels, tag_size_m=.016, camera_matrix=MATRIX) - tags['terminal'] = dict(tag_role='terminal', tag_size_m=.016, corners_xy=pixels.tolist(), - selected_pose=None, reprojection_valid_candidates=[asdict(p) for p in candidates], - candidate_diagnostics=dict(observation_stamp_ns=stamp, branch_frozen=False)) - rows.append(dict(kind='motion_branch_observation', image_stamp_ns=stamp, view='view', task_name='upstream', - zero_joints=['hinge'], session_epoch=1, motion_version=1, camera_matrix=MATRIX, tags=tags, - input_is_rectified=True, sample_phase='zero_approach', - command_vector=[i*255/(len(source)-1), 0.], feedback_vector=[i*255/(len(source)-1), 0.])) - request = BranchInitializationRequest('upstream', 'view', ('hinge',), 1, 1, 1, - tuple(f.evidence for f in source), RELATIONS, (), ('moving',), - tuple((str(r['image_stamp_ns']), source_frame_sha256(r)) for r in rows), - image_frames=source, image_geometry=True) - result = resolve_image_motion(source, RELATIONS) - assert result.resolved - _, report = initialization_artifacts(request, result) - endpoints = [] - for i in range(10): - row = deepcopy(rows[-1]) - row.update(task_name='downstream', zero_joints=['distal'], motion_version=2, - image_stamp_ns=row['image_stamp_ns']+(i+1)*30_000_000) - for item in row['tags'].values(): - item['candidate_diagnostics']['observation_stamp_ns'] = row['image_stamp_ns'] - row['tags']['moving']['candidate_diagnostics']['branch_frozen'] = True - endpoints.append(row) - return profile, ObserverBridgeInput('distal', result.model, report, tuple(rows), tuple(endpoints), (1,)), request - - -def test_raw_bridge_keeps_original_corners_candidates_and_fixed_image_split(observer_source): - profile, source, _ = observer_source - original = source_frame_sha256(source.rows[0]) - bridge, record = build_observer_bridge(profile, source) - assert np.allclose(bridge.translation_xyz_m, [.03, .01, -.005], atol=1e-6) - assert (Rotation.from_quat(bridge.quaternion_xyzw).inv()*Rotation.from_euler('x', 20, degrees=True)).magnitude() < 1e-5 - assert record['source_frame_hashes'][str(source.rows[0]['image_stamp_ns'])] == original - assert source_frame_sha256(source.rows[0]) == original - assert set(record['pose']['training_stamps']).isdisjoint(record['pose']['validation_stamps']) - - -@pytest.mark.parametrize('fault', ['feedback', 'command', 'size', 'missing']) -def test_bad_observer_evidence_is_not_a_new_reference(observer_source, fault): - profile, source, request = observer_source - rows = deepcopy(source.rows) - if fault == 'feedback': - rows[-1]['feedback_vector'][1] = 10 - elif fault == 'command': - rows[-1]['command_vector'][1] = 10 - elif fault == 'size': - rows[-1]['tags']['terminal']['tag_size_m'] *= 2 - else: - del rows[-1]['tags']['terminal'] - invalid = replace(source, rows=tuple(rows)) - with pytest.raises((ValueError, KeyError)): - build_observer_bridge(profile, invalid) - if fault == 'missing': - # Missing optional pixels do not change this upstream hinge's evidence. - result = solve_initialization(replace(request, observer_sources=(invalid,), observer_profile=profile)) - assert result.resolved and result.observer_rejections and not result.observer_pose_bridges - - -@pytest.fixture(scope='module') -def observer_audit(observer_source): - from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation - from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, MotionRelation, RoleCandidates - from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - profile, source, _ = observer_source - parent = source.rows[-1]['tags']['moving']['selected_pose'] - parent_rotation = Rotation.from_quat(parent['quaternion_xyzw']) - parent_point = np.asarray(parent['translation_xyz_m']) - relation = MotionRelation('distal', 'moving', 'terminal') - rows, images = [], [] - for index, theta in enumerate(np.linspace(np.deg2rad(35), 0., 49)): - stamp = 5_000_000_000+index*50_000_000+(500_000_000 if index == 48 else 0) - arc = Rotation.from_rotvec([0., theta, 0.]) - poses = (('moving', parent_rotation, parent_point), - ('terminal', parent_rotation*arc*Rotation.from_euler('x', 20, degrees=True), - parent_point+parent_rotation.apply(arc.apply([.03, .01, -.005])))) - roles, observations, tags = [], [], {} - for role, rotation, point in poses: - projected = (rotation.apply(square_object_points(.016))+point)@np.asarray(MATRIX).T - corners = tuple(map(tuple, projected[:, :2]/projected[:, 2:])) - pose = SquareTagPose(tuple(rotation.as_quat()), tuple(point), .1) - roles.append(RoleCandidates(role, (pose,), role == 'moving')) - observations.append(ImageRoleObservation(role, .016, corners)) - tags[role] = dict(tag_role=role, tag_size_m=.016, corners_xy=corners, selected_pose=asdict(pose), - reprojection_valid_candidates=[asdict(pose)], - candidate_diagnostics=dict(observation_stamp_ns=stamp, branch_frozen=role == 'moving')) - images.append(ImageMotionFrame(MotionEvidenceFrame(stamp, tuple(roles)), MATRIX, tuple(observations))) - rows.append(dict(kind='motion_branch_observation', image_stamp_ns=stamp, view='view', task_name='downstream', - zero_joints=['distal'], session_epoch=1, motion_version=2, camera_matrix=MATRIX, tags=tags, - input_is_rectified=True, sample_phase='zero_approach', command_vector=[255., float(theta)], - feedback_vector=[255., float(theta)])) - endpoints = [] - for index in range(10): - row = deepcopy(rows[-1]) - row['image_stamp_ns'] = rows[-2]['image_stamp_ns']+(index+1)*30_000_000 - for tag in row['tags'].values(): - tag['candidate_diagnostics']['observation_stamp_ns'] = row['image_stamp_ns'] - endpoints.append(row) - source = replace(source, endpoint_rows=tuple(endpoints)) - request = BranchInitializationRequest('downstream', 'view', ('distal',), 1, 2, 1, - tuple(f.evidence for f in images), (relation,), tuple(source.parent_report['evidence_ids'].items()), - ('moving', 'terminal'), tuple((str(r['image_stamp_ns']), source_frame_sha256(r)) for r in rows), - image_frames=tuple(images), image_geometry=True, observer_sources=(source,), observer_profile=profile) - result = solve_initialization(request) - assert result.resolved and result.observer_pose_bridges, result.reason - _, report = initialization_artifacts(request, result) - evidence = {report['evidence_ids'][role]: payload for report in (source.parent_report, report) - for role, payload in report['evidence_payloads'].items()} - records = (*source.rows, source.parent_report, *rows[:-1], *endpoints, rows[-1], report) - return profile, records, evidence - - -def test_observer_pixels_and_frozen_model_survive_journal_readback(observer_audit): - from linkerhand_calibration.runtime.observer_reference import validate_observer_references - from linkerhand_calibration.runtime.motion_provenance import _initializations - profile, records, evidence = observer_audit - assert _initializations(records) == evidence - validate_observer_references(profile, records, evidence) - - -def test_resume_retains_bound_observer_images_without_inventing_missing_pixels(observer_source): - from linkerhand_calibration.runtime.observer_reference import restore_observer_records, observer_inputs - profile, source, _ = observer_source - profile = NS(**vars(profile), vision_motion=False) - owner = NS(profile=profile, zero_joints=('distal',), observer_records={}, - tag_feedback_channels={'moving': {0}, 'terminal': {0, 1}}, - parent_references=NS(models={(2, 'view', 'hinge'): (NS(image_model=source.parent_model), source.parent_report)})) - from linkerhand_calibration.runtime.observer_preparation import ObserverPreparation - owner.observer_preparation = ObserverPreparation(owner) - assert not observer_inputs(owner, 'view', source.endpoint_rows, set(), 2) - rows = deepcopy(source.rows) - del rows[0]['tags']['terminal'] - restore_observer_records(owner, (*rows, *source.endpoint_rows)) - restored, = observer_inputs(owner, 'view', source.endpoint_rows, set(), 2) - assert restored.rows == rows[1:] - assert len(owner.observer_records[(1, 'upstream', 'view')]) == len(rows) - assert source_frame_sha256(restored.rows[0]) == source_frame_sha256(rows[1]) - - -@pytest.mark.parametrize('fault,reason', [('pixels', 'source_pixels_changed'), - ('sampling', 'source_sampling_changed'), ('installation', 'parent_installation_changed')]) -def test_observer_audit_rejects_changed_sources_without_refitting_whole_hand(observer_audit, fault, reason): - from linkerhand_calibration.runtime.observer_reference import validate_observer_references - profile, records, evidence = observer_audit - records, evidence = deepcopy(records), deepcopy(evidence) - payload = next(p for p in evidence.values() if p.get('observer_pose_bridges')) - bridge = payload['observer_pose_bridges'][0] - if fault == 'pixels': - row = next(r for r in records if str(r.get('image_stamp_ns')) in bridge['source_frame_hashes']) - row['tags']['terminal']['corners_xy'][0][0] += .1 - elif fault == 'sampling': - bridge['source_frame_hashes'].pop(next(iter(bridge['source_frame_hashes']))) - else: - bridge['parent_model_sha256'] = '0'*64 - with pytest.raises(ValueError, match=reason): - validate_observer_references(profile, records, evidence) diff --git a/src/linkerhand_calibration/test/test_offline_motion_initialization.py b/src/linkerhand_calibration/test/test_offline_motion_initialization.py new file mode 100644 index 0000000..fa5201a --- /dev/null +++ b/src/linkerhand_calibration/test/test_offline_motion_initialization.py @@ -0,0 +1,89 @@ +"""Recorded branch swaps and endpoint history must not distort offline seeds.""" +import gzip +import json +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.geometry.tag_pose.urdf_command_images import UrdfCommandImages +from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import ( + RoleCandidates, connect_candidate_families, +) +from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose +from linkerhand_calibration.core.fitting.raw_joint_draft import fit_draft +from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint + + +@pytest.fixture +def recorded(): + with gzip.open(Path(__file__).parent/'fixtures/o30_recorded_motion_initialization.json.gz', 'rt') as stream: + rows = json.load(stream)['rows'] + bundle = object.__new__(UrdfCommandImages) + bundle.profile = load_bundled_hand_profile('o30_right_18') + bundle.rows = rows + bundle.commands = np.asarray([row['command_vector'] for row in rows]) + bundle.channels = {name: channel_for_joint(bundle.profile, name) + for name in bundle.profile.measurement.measurements} + bundle.tags = {tag.role: (view.name, tag) for view in bundle.profile.vision.views for tag in view.tags} + bundle.extrinsics = SimpleNamespace(transform=lambda view: np.eye(4)) + bundle._pnps = {} + return bundle + + +def test_recorded_yaw_candidate_rank_swap_does_not_become_motion(recorded): + indices = recorded.motion_indices('thumb_cmc_yaw', 0, 0) + indices = [i for i in indices if recorded.commands[i, 1] in (32, 64, 96, 128)] + naive = np.asarray([recorded._pose(i, 'thumb_cmc_yaw', 0) for i in indices]) + def steps(poses): + rotations = Rotation.from_matrix(poses[:, :3, :3]) + return np.degrees((rotations[1:]*rotations[:-1].inv()).magnitude()) + assert max(steps(naive)) > 40 + families = [] + for branch in (0, 1): + selected, poses = recorded.motion_poses(indices, 'thumb_cmc_yaw', branch) + assert selected == indices + assert max(steps(poses)) < 22 + families.append(poses) + assert not np.allclose(*families) + + +def test_start_belongs_to_scan_without_overwriting_hysteresis(recorded): + before = recorded._directions(recorded.rows).copy() + indices = recorded.motion_indices('thumb_cmc_yaw', 0, 0) + assert recorded.commands[indices[0], 1] == 0 + assert before[indices[0], 1] == 1 # Incoming return move was decreasing. + assert recorded.commands[indices[-1], 1] == 255 + assert np.array_equal(before, recorded._directions(recorded.rows)) + for finger in ('index', 'ring', 'pinky'): + indices = recorded.motion_indices(finger+'_mcp_roll', 0, 0) + assert {recorded.rows[i]['segment_key'] for i in indices} == {'linked_increasing'} + indices = recorded.motion_indices('middle_mcp_roll', 0, 0) + assert {recorded.rows[i]['segment_key'] for i in indices} == {'linked_increasing', 'middle_lower_increasing'} + assert min(recorded.commands[indices, 3]) == 0 + assert max(recorded.commands[indices, 3]) == 255 + + +def test_missing_candidate_is_not_copied_and_strict_gate_remains(): + def pose(angle): + return SquareTagPose(tuple(Rotation.from_euler('x', angle, degrees=True).as_quat()), (0., 0., 1.), .1) + first, second, moved = pose(-15), pose(15), pose(16) + families, reason = connect_candidate_families([ + RoleCandidates('tag', (first, second)), RoleCandidates('tag', (moved,))]) + assert not reason + assert families[0] == [first, None] and families[1] == [second, moved] + families, reason = connect_candidate_families([ + RoleCandidates('tag', (first,)), RoleCandidates('tag', (pose(170),))]) + assert families is None and reason == 'motion_candidate_family_discontinuous' + + +def test_underdetermined_zero_is_rejected_even_when_thin_svd_has_no_null_rows(): + bundle = SimpleNamespace(tags={'tag': object()}, scale=np.ones(2), zero_columns={'joint': 0}, + fit=lambda _: SimpleNamespace(success=True, x=np.zeros(2), fun=np.ones(8), + nfev=1, initialization_evaluations=0), output_mappings=lambda _: {}, + zero_offsets=lambda x: {'joint': x[0]}, jacobian=lambda _: np.array([[1., 1.]])) + with pytest.raises(ValueError, match='draft_estimated_zero_unobservable:joint'): + fit_draft(bundle) diff --git a/src/linkerhand_calibration/test/test_offline_replay.py b/src/linkerhand_calibration/test/test_offline_replay.py deleted file mode 100644 index 6bff284..0000000 --- a/src/linkerhand_calibration/test/test_offline_replay.py +++ /dev/null @@ -1,86 +0,0 @@ -import pytest - -from linkerhand_calibration.compat.legacy import ( - uses_coupled_full_hand_zero_solver, -) -from linkerhand_calibration.offline_replay import ( - _latest_attempt_records, - _latest_palm_axis_records, - _output_suffix, -) - - -def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict: - return { - "kind": "sample", - "joint": joint, - "cycle": cycle, - "direction": direction, - "attempt": attempt, - } - - -def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None: - rows = [ - {"kind": "session_start"}, - _sample("pinky_pip", 0, "decreasing", 1), - _sample("pinky_pip", 0, "decreasing", 3), - _sample("pinky_pip", 0, "increasing", 1), - _sample("pinky_pip", 1, "decreasing", 2), - _sample("thumb_cmc_yaw", 0, "decreasing", 1), - ] - - selected = _latest_attempt_records(rows) - - assert [ - record["attempt"] for record in selected["pinky_pip"] - ] == [3, 1, 2] - assert [ - record["attempt"] for record in selected["thumb_cmc_yaw"] - ] == [1] - - -def test_latest_palm_axis_attempt_is_selected_per_cycle_and_direction() -> None: - def sample(cycle: int, direction: str, attempt: int) -> dict: - return { - "kind": "palm_axis_sample", - "source_joint": "pinky_mcp_pitch_front_axis", - "cycle": cycle, - "direction": direction, - "attempt": attempt, - } - - selected = _latest_palm_axis_records( - [ - sample(0, "decreasing", 1), - sample(0, "decreasing", 2), - sample(0, "increasing", 1), - ] - ) - - assert [ - record["attempt"] - for record in selected["pinky_mcp_pitch_front_axis"] - ] == [2, 1] - - -def test_output_suffix_is_safe_and_explicit() -> None: - assert _output_suffix(None) == "" - assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2" - for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65): - with pytest.raises(ValueError, match="output tag"): - _output_suffix(invalid) - - -def test_v1_session_solver_selection_is_confined_to_compatibility() -> None: - legacy = { - "sample_schema_version": 1, - "capabilities": ["palm_axis_side_channel_v2"], - } - current = { - "sample_schema_version": 1, - "capabilities": ["palm_axis_relative_motion_v3"], - } - - assert uses_coupled_full_hand_zero_solver(legacy) - assert not uses_coupled_full_hand_zero_solver(current) diff --git a/src/linkerhand_calibration/test/test_optical_verification.py b/src/linkerhand_calibration/test/test_optical_verification.py deleted file mode 100644 index 081332d..0000000 --- a/src/linkerhand_calibration/test/test_optical_verification.py +++ /dev/null @@ -1,69 +0,0 @@ -import copy -from dataclasses import replace - -import numpy as np -import pytest - -from optical_fixture import optical_dataset -from linkerhand_calibration.core.geometry.extrinsics import CameraExtrinsics, CameraCalibrationIdentity -from linkerhand_calibration.runtime.optical_verification import verify_optical_observations - - -@pytest.fixture -def optical(): - cameras = {v: CameraCalibrationIdentity(v, 1600, 1200, 'b'*64) for v in ('a','b','c')} - transforms = {v: np.eye(4) for v in cameras} - transforms['b'][0,3] = .03 - transforms['c'][1,3] = .03 - extrinsics = CameraExtrinsics(cameras, 'a', transforms, {}) - matrices = {v: np.array([[1400.,0,800.],[0,1400.,600.],[0,0,1.]]) for v in cameras} - return extrinsics, matrices, optical_dataset(extrinsics, matrices, 'a'*64) - - -def test_measured_optical_check_freezes_camera_parameters(optical): - extrinsics, matrices, dataset = optical - report = verify_optical_observations(dataset, extrinsics, matrices, extrinsics_sha256='a'*64) - assert report['passed'] and report['reprojection_p95_px'] < 1e-5 - assert report['intrinsics_updated'] is report['extrinsics_updated'] is False - - -@pytest.mark.parametrize('error', ['intrinsics', 'extrinsics', 'coverage', 'serial', 'synchronization']) -def test_configuration_identity_cannot_replace_optical_observations(optical, error): - extrinsics, matrices, dataset = optical - if error == 'intrinsics': - matrices['b'][0,0] *= 1.1 - for capture in dataset['captures']: - capture['views']['b']['camera_matrix'] = matrices['b'].tolist() - elif error == 'extrinsics': - extrinsics.reference_from_view['b'][0,3] += .03 - elif error == 'coverage': - dataset['captures'] = dataset['captures'][:2] - elif error == 'serial': - dataset['captures'][0]['views']['b']['serial_number'] = 'wrong' - else: - dataset['captures'][0]['views']['b']['image_stamp_ns'] += 60_000_000 - with pytest.raises(ValueError, match='optical_'): - verify_optical_observations(dataset, extrinsics, matrices, extrinsics_sha256='a'*64) - - -def test_explicit_camera_reuse_checks_prior_quality_without_claiming_new_measurement(tmp_path): - import hashlib - from pathlib import Path - import yaml - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.optical_verification import accept_existing_camera_calibration - profile = load_bundled_hand_profile('o30_right_18') - source = Path(__file__).resolve().parents[3]/'config/o30_three_camera_extrinsics_20260922_101452.yaml' - path = tmp_path/'cameras.yaml' - path.write_bytes(source.read_bytes()) - digest = hashlib.sha256(path.read_bytes()).hexdigest() - report = accept_existing_camera_calibration(profile, path, digest) - assert report['accepted'] is True and report['passed'] is False - assert report['current_optical_verification'] == 'not_performed' - with pytest.raises(ValueError, match='existing_camera_calibration_changed'): - accept_existing_camera_calibration(profile, path, '0'*64) - payload = yaml.safe_load(path.read_text()) - payload['quality']['reprojection_rms_px'] = 100. - path.write_text(yaml.safe_dump(payload)) - with pytest.raises(ValueError): - accept_existing_camera_calibration(profile, path, hashlib.sha256(path.read_bytes()).hexdigest()) diff --git a/src/linkerhand_calibration/test/test_package_rename.py b/src/linkerhand_calibration/test/test_package_rename.py deleted file mode 100644 index 658cded..0000000 --- a/src/linkerhand_calibration/test/test_package_rename.py +++ /dev/null @@ -1,56 +0,0 @@ -import importlib -from pathlib import Path -import re -import sys - -import pytest - -from linkerhand_calibration.compat import resolve_renamed_package_path - - -def test_legacy_python_package_forwards_only_public_config_api() -> None: - with pytest.warns(DeprecationWarning, match="linkerhand_calibration"): - if "g20_thumb_apriltag_calibration" in sys.modules: - legacy = importlib.reload( - sys.modules["g20_thumb_apriltag_calibration"] - ) - else: - legacy = importlib.import_module( - "g20_thumb_apriltag_calibration" - ) - - current = importlib.import_module("linkerhand_calibration.product") - assert legacy.ProductConfig is current.ProductConfig - assert legacy.load_product_config is current.load_product_config - - -def test_new_and_legacy_executable_names_share_one_implementation() -> None: - setup_text = (Path(__file__).resolve().parents[1] / "setup.py").read_text( - encoding="utf-8" - ) - for executable in ("calibrate_hand", "calibrate_g20_right"): - assert re.search( - rf'"{executable} = "\s*' - r'"linkerhand_calibration\.runtime\.runner:main"', - setup_text, - ) - assert ( - "linkerhand_calibration.runtime.ros.entrypoint:main" - in setup_text - ) - - -def test_package_resource_uri_resolves_to_the_workspace_copy() -> None: - repository = Path(__file__).resolve().parents[3] - expected = ( - repository - / "src/linkerhand_calibration/urdf/g20_right/linkerhand_g20_right.urdf" - ) - - resolved = resolve_renamed_package_path( - "package://linkerhand_calibration/urdf/g20_right/" - "linkerhand_g20_right.urdf", - repository, - ) - - assert resolved == expected diff --git a/src/linkerhand_calibration/test/test_parallel_image_replay.py b/src/linkerhand_calibration/test/test_parallel_image_replay.py deleted file mode 100644 index e5fe57b..0000000 --- a/src/linkerhand_calibration/test/test_parallel_image_replay.py +++ /dev/null @@ -1,49 +0,0 @@ -"""Parallel preparation must run the same image and sample integrity checks.""" - -from copy import deepcopy - -import pytest - -from linkerhand_calibration.runtime import image_replay -from test_image_motion_provenance import recorded_image - - -def test_spawned_replay_accepts_the_same_images_and_preserves_records(): - frame, row = recorded_image() - original = deepcopy((frame, row)) - groups = [(frame, [row])] * 256 - image_replay.validate_image_groups(groups) - image_replay.validate_image_groups(groups, workers=2) - assert (frame, row) == original - - -@pytest.mark.parametrize("target", ["frame", "sample"]) -def test_spawned_replay_propagates_corruption_after_valid_batches(target): - frame, row = recorded_image() - altered_frame, altered_row = deepcopy((frame, row)) - if target == "frame": - altered_frame["roles"]["moving"]["frozen_image_model"]["geometry"][0]["pivot_parent_xyz_m"] = (.2, .2, .2) - else: - altered_row["relative_translation_xyz_m"][0] += .01 - groups = [(frame, [row])] * 255 + [(altered_frame, [altered_row])] - with pytest.raises(ValueError) as serial: - image_replay.validate_image_groups(groups) - with pytest.raises(ValueError) as parallel: - image_replay.validate_image_groups(groups, workers=2) - assert str(parallel.value) == str(serial.value) - - -@pytest.mark.parametrize("small,daemon", [(True, False), (False, True)]) -def test_small_inputs_and_daemon_finalizers_do_not_spawn(monkeypatch, small, daemon): - from types import SimpleNamespace - - monkeypatch.setattr(image_replay.multiprocessing, "current_process", lambda: SimpleNamespace(daemon=daemon)) - monkeypatch.setattr(image_replay, "ProcessPoolExecutor", lambda **kwargs: pytest.fail("unexpected process pool")) - frame, row = recorded_image() - image_replay.validate_image_groups([(frame, [row])] * (1 if small else 256), workers=4) - - -@pytest.mark.parametrize("cpus,expected", [(None, 1), (1, 1), (2, 1), (4, 2), (24, 4)]) -def test_checkpoint_workers_leave_host_capacity(monkeypatch, cpus, expected): - monkeypatch.setattr(image_replay.os, "cpu_count", lambda: cpus) - assert image_replay.checkpoint_replay_workers() == expected diff --git a/src/linkerhand_calibration/test/test_parent_reference_provenance.py b/src/linkerhand_calibration/test/test_parent_reference_provenance.py deleted file mode 100644 index d17131d..0000000 --- a/src/linkerhand_calibration/test/test_parent_reference_provenance.py +++ /dev/null @@ -1,169 +0,0 @@ -"""Read back stationary parent checks from actual projected corner evidence.""" - -from copy import deepcopy -from dataclasses import asdict, replace -import json - -import pytest - -from linkerhand_calibration.core.domain.reference import build_joint_zero_reference -from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_motion_model_from_dict -from linkerhand_calibration.core.geometry.tag_pose.cad_image_model import TransferredImageMotionModel -from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry -from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageHingeGeometry -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation -from linkerhand_calibration.core.geometry.tag_pose.source_hinge import SourceHinge -from linkerhand_calibration.runtime.parent_reference import ParentReferenceRegistry -from linkerhand_calibration.runtime.parent_reference_evidence import ( - validate_parent_references, validate_source_hinges, same_capture_epoch, -) -from linkerhand_calibration.runtime.branch_initialization import MotionBranchModel -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 -from test_image_motion_provenance import recorded_image -from test_parent_reference_reuse import setup - - -def pixels(profile, stamp, task_key, command): - # Known 3D truth is projected by the shared independent image fixture. - # Rename only the declared roles/joint; geometry and measured pixels stay. - frame, row = recorded_image(stamp=stamp) - mapping = {"root": "front_base", "moving": "thumb_mcp", "hinge": "thumb_cmc_roll", "task": task_key} - def renamed(value): - if isinstance(value, str): return mapping.get(value, value) - if isinstance(value, dict): return {mapping.get(k, k): renamed(v) for k, v in value.items()} - if isinstance(value, (tuple, list)): return [renamed(v) for v in value] - return value - frame, row = renamed(frame), renamed(row) - image = frame["roles"]["thumb_mcp"]["frozen_image_model"] - digest = image_model_sha256(image) - for item in (frame["roles"]["thumb_mcp"], row["pnp_observation_evidence"]["child"]): - item["image_model_sha256"] = digest - item["candidate_diagnostics"].update(motion_evidence_verified=True, motion_evidence_id="a"*64) - row.update(kind="joint_zero_sample", sample_phase="joint_zero", command_vector_u8=list(command), - state_u8=list(command), command_direction_by_index=["decreasing"]*20) - return frame, row, image - - -def journal(setup): - profile, task, channels = setup - name = task.parent_reference.pose_joint - spec = profile.motion.joint_zero_references[name] - target = profile.motion.joint_zero_references["thumb_ip"].command - records, before, after = [], [], [] - for stamp in range(4000000000, 4000000010): - frame, row, image = pixels(profile, stamp, spec.task_key, spec.command) - records.extend((frame, row)); before.append(row) - reference = build_joint_zero_reference(profile, name, before, session_epoch=1, motion_version=3) - records.append(reference.as_record()) - for stamp in range(5000000000, 5000000010): - frame, row, _ = pixels(profile, stamp, task.key, target) - records.extend((frame, row)); after.append(row) - model = image_motion_model_from_dict(image) - pose = MotionBranchModel(spec.task_key, "front", (), (("thumb_mcp", "a"*64),), model) - report = dict(session_epoch=1, image_model_sha256=image_model_sha256(image)) - owner = ParentReferenceRegistry(profile, channels) - owner.remember(pose, report) - # A separate accepted geometry source is required even for activation. - owner.models[(1, "front", "thumb_mcp")] = (pose, report) - check = owner.confirm(task, 1, 10, reference, after) - records.append(check) - parent = ImageHingeGeometry(MotionRelation("thumb_mcp", "front_base", "thumb_mcp"), - (0., 0., 1.), (0., 0., 0., 1.), (0., 0., 0.), (.01, 0., 0.)) - source = SourceHinge(parent, "b"*64, "c"*64, .001, .0001) - child = ImageHingeGeometry(MotionRelation("thumb_ip", "thumb_mcp", "thumb_ip"), - (0., 0., 1.), (0., 0., 0., 1.), (.03, 0., 0.), (.015, .002, 0.)) - transferred = TransferredImageMotionModel((child,), model.camera_matrix, - (("thumb_mcp", .016), ("thumb_ip", .016)), (6000000000, 6000000001), - (6000000000,), (6000000001,), (("thumb_mcp", 0), ("thumb_ip", 0)), - constraints=(ParallelAxisGeometry("thumb_mcp", "thumb_ip", 1, .04),), - source_urdf_sha256="d"*64, source_hinges=(source,)) - evidence = {"a"*64: dict(frozen_image_model=image, image_model_sha256=report["image_model_sha256"], - view="front", tag_role="thumb_mcp", session_epoch=1, source_image_stamps=[1, 2]), - "e"*64: dict(frozen_image_model=asdict(transferred), image_model_sha256=image_model_sha256(asdict(transferred)), - view="front", tag_role="thumb_ip", session_epoch=1, motion_version=12, - task_name=task.key, parent_reference_sha256=source_frame_sha256(check), - source_image_stamps=list(transferred.source_stamps_ns))} - return profile, records, evidence - - -def test_parent_check_survives_file_roundtrip_with_original_zero_and_current_pixels(setup): - profile, records, evidence = journal(setup) - records, evidence = json.loads(json.dumps((records, evidence))) - validate_parent_references(profile, records, evidence) - - -@pytest.mark.parametrize("change", ["missing_check", "held_channels", "sample", "missing_pixels", "model", "old_zero", "feedback_policy"]) -def test_parent_check_cannot_be_relabelled_or_lose_original_pixels(setup, change): - profile, records, evidence = journal(setup) - records, evidence = deepcopy((records, evidence)) - check = next(row for row in records if row["kind"] == "parent_reference_verified") - if change == "missing_check": records.remove(check) - elif change == "held_channels": check["held_channels"] = [] - elif change == "sample": - row = next(row for row in records if row["kind"] == "joint_zero_sample" and row["image_stamp_ns"] >= 5000000000) - row["state_u8"][1] = 80 - elif change == "missing_pixels": - records[:] = [r for r in records if not (r["kind"] == "pnp_candidate_frame" and r["image_stamp_ns"] >= 5000000000)] - elif change == "model": check["pose_evidence_id"] = "f"*64 - elif change == "feedback_policy": check['feedback_limits']['between_visit_difference'] = 11 - else: - records[:] = [r for r in records if not (r["kind"] == "pnp_candidate_frame" and r["image_stamp_ns"] < 5000000000)] - # Rehash the changed check: validation must inspect meaning and pixels, - # rather than rely on a metadata hash mismatch alone. - evidence["e"*64]["parent_reference_sha256"] = source_frame_sha256(check) - with pytest.raises(ValueError): validate_parent_references(profile, records, evidence) - - -def test_callback_epoch_changes_need_ordered_internal_recovery_evidence(): - recovery = dict(kind="joint_zero_recovery", action="retry_zero_images", - previous_session_epoch=1, session_epoch=2, stamp_ns=50) - assert same_capture_epoch([recovery], 1, 2, 10, 100) - assert not same_capture_epoch([], 1, 2, 10, 100) - assert not same_capture_epoch([recovery], 1, 3, 10, 100) - assert not same_capture_epoch([recovery], 1, 2, 60, 100) - assert not same_capture_epoch([{**recovery, "action": "pause"}], 1, 2, 10, 100) - - -@pytest.mark.parametrize("use", ["pose", "hinge"]) -@pytest.mark.parametrize("change", [None, "missing", "identity", "model", "epoch", "task", "late", "undeclared"]) -def test_restored_parent_sources_require_exact_journalled_model_identity(setup, use, change): - profile, records, evidence = journal(setup) - if use == "pose": - source_id = "a"*64 - original = evidence[source_id] - validate = lambda: validate_parent_references(profile, records, evidence) - else: - payload = evidence["e"*64] - transferred = image_motion_model_from_dict(payload["frozen_image_model"]) - source = transferred.source_hinges[0] - original_model = replace(image_motion_model_from_dict(evidence["a"*64]["frozen_image_model"]), - geometry=(source.geometry,)) - digest = image_model_sha256(asdict(original_model)) - transferred = replace(transferred, source_hinges=(replace(source, model_sha256=digest),)) - payload.update(frozen_image_model=asdict(transferred), - image_model_sha256=image_model_sha256(asdict(transferred))) - source_id = source.evidence_id - original = evidence[source_id] = dict(frozen_image_model=asdict(original_model), - image_model_sha256=digest, view="front", tag_role="thumb_mcp", - source_image_stamps=[1, 2], geometry_uncertainty=[("thumb_mcp", .001, .0001)]) - validate = lambda: validate_source_hinges(evidence, records) - original.update(session_epoch=2, task_name="passed_source_task") - restored = dict(kind="passed_pose_models_restored", resume_mode="passed", - unchanged_installation_declared=True, session_epoch=1, stamp_ns=4500000000, - models=[dict(source_epoch=2, task_name=original["task_name"], - image_model_sha256=original["image_model_sha256"], - evidence_ids={original["tag_role"]: source_id})]) - item = restored["models"][0] - if change == "identity": item["evidence_ids"][original["tag_role"]] = "f"*64 - elif change == "model": item["image_model_sha256"] = "f"*64 - elif change == "epoch": item["source_epoch"] = 3 - elif change == "task": item["task_name"] = "different_task" - elif change == "late": restored["stamp_ns"] = 7000000000 - elif change == "undeclared": restored["unchanged_installation_declared"] = False - if change != "missing": records.append(restored) - if change is None: - validate() - else: - with pytest.raises(ValueError, match="(?:pose_source|source_hinge)_unbound"): - validate() diff --git a/src/linkerhand_calibration/test/test_parent_reference_reuse.py b/src/linkerhand_calibration/test/test_parent_reference_reuse.py deleted file mode 100644 index 35c261d..0000000 --- a/src/linkerhand_calibration/test/test_parent_reference_reuse.py +++ /dev/null @@ -1,262 +0,0 @@ -"""A held parent is checked in place; independent zeros and sources stay frozen.""" - -from copy import deepcopy -from dataclasses import asdict, replace -from types import SimpleNamespace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.validator import validate_executable_profile -from linkerhand_calibration.profiles.observations import compile_tag_feedback_channels -from linkerhand_calibration.core.domain.reference import JointZeroPose, JointZeroSample, JointZeroReference -from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionModel -from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageHingeGeometry -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, MotionBranchModel -from linkerhand_calibration.runtime.parent_reference import ParentReferenceRegistry, check_parent_pose -from linkerhand_calibration.runtime.capture import ObservationCapture -from linkerhand_calibration.runtime.reference_lock import ReferenceLock -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.zero_recovery import ZeroRecovery -from o30_capture_fixture import SOURCE - - -@pytest.fixture -def setup(): - profile = load_bundled_hand_profile("o30_right_18", scope="without_finger_roll") - task = next(t for t in profile.motion.tasks if t.key == "thumb_ip_front") - channels = compile_tag_feedback_channels(profile, UrdfKinematicModel(SOURCE)) - return profile, task, channels - - -def reference_and_rows(profile, task): - spec = profile.motion.joint_zero_references[task.parent_reference.pose_joint] - pose = JointZeroPose((0., 0., 0., 1.), (.02, .03, .7)) - command = spec.command - feedback = list(command) - feedback[0] = 10. # A repeatable command/feedback offset is not a pose fault. - samples = tuple(JointZeroSample(f"front:{100+i}", "front", 100+i, (0., 0., 0., 1.), - (0., 0., 0.), command, tuple(feedback), ("decreasing",)*20, pose, pose) for i in range(10)) - reference = JointZeroReference(task.parent_reference.pose_joint, spec.task_key, 0., 0, 1, 2, samples) - target = profile.motion.joint_zero_references["thumb_ip"].command - feedback[2] = target[2] - rows = [dict(kind="joint_zero_sample", joint=reference.joint, task_name=task.key, - view="front", image_stamp_ns=1000+i, relative_quaternion_xyzw=(0., 0., 0., 1.), - relative_translation_xyz_m=(0., 0., 0.), command_vector_u8=target, state_u8=tuple(feedback), - command_direction_by_index=("decreasing",)*20, parent_pose_common=asdict(pose), child_pose_common=asdict(pose)) - for i in range(10)] - return reference, rows - - -@pytest.mark.parametrize("change", ["none", "yaw", "roll", "tag", "feedback", "unstable", "short", "duplicate", "stale"]) -def test_stationary_check_uses_ancestry_current_images_and_feedback_stability(setup, change): - profile, task, channels = setup - reference, rows = reference_and_rows(profile, task) - before = deepcopy(reference) - if change in {"yaw", "roll"}: - values = list(rows[-1]["command_vector_u8"]); values[1 if change == "yaw" else 0] += 1 - rows[-1]["command_vector_u8"] = tuple(values) - elif change == "tag": - rows[-1]["child_pose_common"]["quaternion_xyzw"] = tuple(Rotation.from_euler("x", 10, degrees=True).as_quat()) - elif change in {"feedback", "unstable"}: - for row in (rows if change == "feedback" else rows[-1:]): - values = list(row["state_u8"]); values[6] += 10 if change == "feedback" else 3 - row["state_u8"] = tuple(values) - elif change == "short": rows.pop() - elif change == "duplicate": rows[-1] = deepcopy(rows[0]) - elif change == "stale": rows[0]["image_stamp_ns"] = 1 - if change == "none": - check_parent_pose(profile, task, reference, rows, channels["thumb_mcp"]) - else: - with pytest.raises(ValueError, match="parent_reference"): - check_parent_pose(profile, task, reference, rows, channels["thumb_mcp"]) - assert reference == before - - -def model(joint, identity): - image = ImageMotionModel((ImageHingeGeometry(MotionRelation(joint, "front_base", "thumb_mcp"), - (0., 0., 1.), (0., 0., 0., 1.), (.01, .02, 0.), (.02, 0., 0.)),), - ((2000., 0., 800.), (0., 2000., 600.), (0., 0., 1.)), - (("front_base", .0165), ("thumb_mcp", .0165)), (1, 2), (1,), (2,), (("front_base", 0), ("thumb_mcp", 0))) - branch = MotionBranchModel(joint+"_front", "front", (), (("thumb_mcp", identity),), image) - report = dict(session_epoch=1, image_model_sha256=image_model_sha256(asdict(image)), - geometry_uncertainty=[(joint, .001, .0001)]) - return branch, report - - -def test_registry_needs_both_sources_and_fresh_check_and_preserves_zeros(setup): - profile, task, channels = setup - owner = ParentReferenceRegistry(profile, channels) - reference, rows = reference_and_rows(profile, task) - roll, roll_report = model("thumb_cmc_roll", "a"*64) - mcp, mcp_report = model("thumb_mcp", "b"*64) - owner.remember(roll, roll_report) - with pytest.raises(ValueError, match="source_missing"): owner.sources(task, 1) - owner.remember(mcp, mcp_report) - with pytest.raises(ValueError, match="source_missing"): owner.sources(task, 2) - with pytest.raises(ValueError, match="unverified"): owner.geometry(task, 1) - - refresh = owner.authorize_refreshed_pose(task, 1, 9, reference) - assert refresh["passed_zero_preserved"] is True - assert refresh["image_model_sha256"] == roll_report["image_model_sha256"] - assert owner.pose_source(task, 1) == (roll, roll_report) - - for row in rows: - row["pnp_observation_evidence"] = {"child": dict(tag_role="thumb_mcp", - image_model_sha256=roll_report["image_model_sha256"], - candidate_diagnostics={"motion_evidence_id": "a"*64})} - check = owner.confirm(task, 1, 10, reference, rows) - hinges, identity = owner.geometry(task, 1) - assert hinges[0].geometry == mcp.image_model.geometry[0] - assert hinges[0].evidence_id == "b"*64 and check["pose_evidence_id"] == "a"*64 - assert check["source_reference"] == reference.as_record() and len(identity) == 64 - owner.advance_recovery_epoch(1, 2) - assert owner.geometry(task, 2) == (hinges, identity) - assert owner.sources(task, 2)[0][1]["session_epoch"] == 1 - owner.clear() - with pytest.raises(ValueError, match="unverified"): owner.geometry(task, 1) - - -def test_parent_repeatability_uses_declared_revisit_policy_and_preserves_legacy(setup): - from linkerhand_calibration.runtime.revisit import REVISIT_POLICY, revisit_feedback_limits - profile, task, channels = setup - reference, rows = reference_and_rows(profile, task) - limits = revisit_feedback_limits(profile) - for row in rows: - values = list(row['state_u8']); values[6] += 8 - row['state_u8'] = tuple(values) - with pytest.raises(ValueError, match='held_feedback_changed'): - check_parent_pose(profile, task, reference, rows, channels['thumb_mcp']) - check_parent_pose(profile, task, reference, rows, channels['thumb_mcp'], - verification=REVISIT_POLICY, feedback_limits=limits) - for fault in ('unstable', 'outside', 'policy', 'image'): - changed = deepcopy(rows); declared = dict(limits) - if fault == 'unstable': - values=list(changed[-1]['state_u8']); values[6] -= 3 - changed[-1]['state_u8']=tuple(values) - elif fault == 'outside': - for row in changed: - values=list(row['state_u8']);values[6]+=3;row['state_u8']=tuple(values) - elif fault == 'policy': declared['between_visit_difference'] = 11 - else: changed[-1]['child_pose_common']['translation_xyz_m'] = (.02,.03,.72) - with pytest.raises(ValueError, match='parent_reference'): - check_parent_pose(profile, task, reference, changed, channels['thumb_mcp'], - verification=REVISIT_POLICY, feedback_limits=declared) - - -def test_activation_removes_incompatible_parent_and_keeps_reused_model_through_ip_approach(setup): - profile, task, _ = setup - capture = ObservationCapture(profile, reference_lock=ReferenceLock({"front": 0, "side": 3, "top": 16}), - extrinsics=SimpleNamespace(transform=lambda _: np.eye(4))) - roll, _ = model("thumb_cmc_roll", "a"*64) - mcp, _ = model("thumb_mcp", "b"*64) - capture.install_motion_model(mcp) - capture.activate_reference_model(roll) - assert list(capture._motion_models.values()) == [roll] - capture.begin_motion(MotionCommand("zero_approach", profile.command.baseline_values, 200, - task_key=task.key, reference_joints=("thumb_ip",))) - assert list(capture._motion_models.values()) == [roll] - assert roll.image_model.geometry[0].relation.joint == "thumb_cmc_roll" - - -def test_stationary_reference_cannot_trigger_a_new_solver_or_extra_motion_retry(setup): - profile, task, _ = setup - motion = MotionCommand("joint_zero", profile.command.baseline_values, 200, - task_key=task.key, zero_joints=("thumb_cmc_roll",), reference_only=True, reference_reuse=True) - owner = BranchInitialization(profile, source_urdf=SOURCE) - owner.begin(motion, session_epoch=1, motion_version=10) - assert owner.request("front", motion, session_epoch=1, motion_version=10) is None - recovery = ZeroRecovery() - assert recovery.take(motion, geometry_error="", reports={}, referenced_joints=()) is None - assert recovery.take_parent_model_refresh(motion, stale_image_anchor=False) is None - ordinary = replace(motion, reference_only=False, reference_reuse=False, zero_joints=("thumb_ip",)) - assert recovery.take(ordinary, geometry_error="ambiguous", reports={"front": { - "status": "unresolved", "reason": "image_motion_families_not_distinguishable"}}, referenced_joints=()) is None - - -def test_stale_cross_session_anchor_gets_one_model_refresh_without_replacing_zero(setup): - profile, task, _ = setup - parent = task.parent_reference.pose_joint - motion = MotionCommand("joint_zero", - profile.motion.joint_zero_references[task.joints[0]].command, 200, - task_key=task.key, cycle=0, zero_joints=(parent,), - reference_only=True, reference_reuse=True) - recovery = ZeroRecovery() - assert recovery.take_parent_model_refresh(motion, stale_image_anchor=True) == "refresh_parent_model" - assert recovery.take_parent_model_refresh(motion, stale_image_anchor=True) is None - - execution = SessionExecution(profile) - execution.session.phase = Phase.PREPARE - frozen = object() - execution.zero_references[parent] = frozen - execution._effects = [motion] - execution.refresh_parent_model(motion.target) - assert execution.zero_references[parent] is frozen - assert execution._effects[-1] is motion - refresh = execution._effects[:-1] - assert refresh and refresh[-1].phase == "joint_zero" - assert refresh[-1].zero_joints == (parent,) - assert all(item.parent_model_refresh_for == task.key for item in refresh) - assert all(item.reference_only and not item.reference_reuse for item in refresh) - moving = [item for item in refresh if item.phase == "zero_approach"] - from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint - channel = channel_for_joint(profile, parent) - assert max(item.target[channel] for item in moving) != min(item.target[channel] for item in moving) - - -def test_coordinator_waits_for_current_parent_images_and_keeps_both_joint_zeros(setup, tmp_path, monkeypatch): - from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator - from linkerhand_calibration.runtime import coordinator as module - - profile, task, channels = setup - reference, rows = reference_and_rows(profile, task) - owner = ParentReferenceRegistry(profile, channels) - roll, report = model("thumb_cmc_roll", "a"*64) - owner.remember(roll, report) - owner.remember(*model("thumb_mcp", "b"*64)) - for row in rows: - row["pnp_observation_evidence"] = {"child": dict(tag_role="thumb_mcp", - image_model_sha256=report["image_model_sha256"], - candidate_diagnostics={"motion_evidence_id": "a"*64})} - frozen, records = [], [] - zeros = {reference.joint: reference, "thumb_mcp": object()} - host = SimpleNamespace(profile=profile, - branch_initialization=SimpleNamespace(ready=lambda _: pytest.fail("stationary reference cannot wait for a new solve"), - parent_references=owner), execution=SimpleNamespace(zero_references=zeros.copy()), - _steady_rows=rows[:-1], _observation_epoch=1, _segment_number=10, - _freeze_reference_branches=lambda value: frozen.append(value) or True, - raw_path=tmp_path/"reference.jsonl", capture_index=records) - revisions = {("front", "thumb_mcp"): 2, ("front", "front_base"): 1} - monkeypatch.setattr(module, "reference_branch_revisions", lambda *args, **kwargs: revisions) - motion = MotionCommand("joint_zero", profile.command.baseline_values, 200, - task_key=task.key, zero_joints=(reference.joint,), reference_only=True, reference_reuse=True) - assert not CalibrationCoordinator._confirm_model_reference(host, motion) - assert not records and not frozen - host._steady_rows = rows - assert CalibrationCoordinator._confirm_model_reference(host, motion) - assert frozen == [revisions] and records[0]["kind"] == "parent_reference_verified" - assert host.execution.zero_references == zeros - assert owner.geometry(task, 1)[0][0].geometry.relation.joint == "thumb_mcp" - - -@pytest.mark.parametrize("change", ["future_source", "wrong_tag", "wrong_pose", "nonadjacent_geometry"]) -def test_invalid_reference_contract_rejected_before_hardware(setup, change): - profile, task, _ = setup - if change == "future_source": task = replace(task, parent_reference=replace(task.parent_reference, pose_joint="thumb_cmc_yaw")) - elif change == "wrong_tag": task = replace(task, parent_reference=replace(task.parent_reference, pose_joint="index_mcp_roll")) - elif change == "nonadjacent_geometry": task = replace(task, parent_reference=replace(task.parent_reference, geometry_joint="thumb_cmc_roll")) - else: - references = dict(profile.motion.joint_zero_references) - pose = list(references["thumb_ip"].command); pose[1] = 80 - references["thumb_ip"] = replace(references["thumb_ip"], command=tuple(pose)) - profile = replace(profile, motion=replace(profile.motion, joint_zero_references=references)) - profile = replace(profile, motion=replace(profile.motion, - tasks=tuple(task if t.key == task.key else t for t in profile.motion.tasks))) - with pytest.raises(ValueError): validate_executable_profile(profile, SOURCE) diff --git a/src/linkerhand_calibration/test/test_partial_o30_calibration.py b/src/linkerhand_calibration/test/test_partial_o30_calibration.py deleted file mode 100644 index 320f750..0000000 --- a/src/linkerhand_calibration/test/test_partial_o30_calibration.py +++ /dev/null @@ -1,161 +0,0 @@ -"""Partial release retains CAD byte values and cannot invent excluded curves.""" - -from copy import deepcopy -from dataclasses import replace -import hashlib -import json -import xml.etree.ElementTree as ET - -import pytest - -from o30_capture_fixture import synthetic_capture, SOURCE -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.core.urdf.partial_scope import profile_scope, retained_from_payload, require_retained_pose -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -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 - - -def test_partial_tasks_keep_sdk_and_avoidance_contract(tmp_path): - full = load_bundled_hand_profile('o30_right_18', scope='full') - partial = load_bundled_hand_profile('o30_right_18', scope='without_finger_roll') - assert partial.command == full.command - assert len(partial.motion.tasks) == 16 - assert [task.key for task in partial.motion.tasks] == [ - 'thumb_cmc_roll_front', 'thumb_mcp_front', 'thumb_ip_front', 'thumb_cmc_yaw_top', - *(f'{finger}_{joint}_side' for finger in ('pinky', 'ring', 'middle', 'index') - for joint in ('mcp_pitch', 'pip', 'dip')), - ] - from linkerhand_calibration.runtime.trajectory import build_calibration_motion_command - for task in partial.motion.tasks: - reference = partial.motion.joint_zero_references[task.joints[0]] - assert reference.command == tuple(build_calibration_motion_command(task, 0, profile=partial)) - assert reference.command[3:6] == (196, 127, 52) - assert reference.command[2] == (0 if task.key == 'thumb_cmc_roll_front' else 255) - for approach in reference.approach_commands: - assert all(value == reference.command[index] for index, value in enumerate(approach) - if index != task.command_index) - yaw = partial.motion.joint_zero_references['thumb_cmc_yaw'] - assert yaw.command == partial.command.baseline_values - assert yaw.approach_commands == ((0, 255, 255, 196, 127, 52) + (0,) * 14,) - assert len(partial.zero.direct_zero_joints) == 11 - assert tuple(t for t in full.motion.tasks if t.key != 'fingers_mcp_roll_front') == partial.motion.tasks - assert not partial.retained_joints & partial.urdf_authorized_fields.keys() - assert partial.artifacts.publication_pointer != full.artifacts.publication_pointer - for name in partial.retained_joints: - assert name not in partial.motion.joint_zero_references - # Scope compilation is idempotent across profile export/readback. - from linkerhand_calibration.profiles.scope import compile_capture_scope - assert compile_capture_scope(partial) == partial - from linkerhand_calibration.profiles import dump_hand_profile, load_hand_profile - path = tmp_path/'partial.yaml'; path.write_text(dump_hand_profile(partial)) - assert load_hand_profile(path) == partial - from linkerhand_calibration.runtime.engine import CalibrationEngine, ACQUISITION_POLICY_VERSION - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - engine = CalibrationEngine(partial) - header = dict(profile_id=partial.key.profile_id, - acquisition_policy_version=ACQUISITION_POLICY_VERSION, - capture_schedule_version=engine.capture_schedule_version, - pose_tracking_policy_version=POSE_TRACKING_POLICY_VERSION, - calibration_scope=profile_scope(partial)) - assert engine.resume_compatible(header) - assert not engine.resume_compatible({**header, - 'capture_schedule_version': 'unified_schedule_v7_single_pass_endpoint_images'}) - assert not CalibrationEngine(full).resume_compatible(header) - header.pop('calibration_scope') - assert not engine.resume_compatible(header) - - -@pytest.mark.parametrize('camera_aligned_reference', [False, True]) -def test_partial_o30_finalization_and_independent_readback(tmp_path, monkeypatch, camera_aligned_reference): - from linkerhand_calibration.runtime.artifacts import finalization - from test_final_image_holdout import independent_pixels - original = finalization.FrozenTagArtifactValidator - def independent_validator(*args, **kwargs): - validator = original(*args, **kwargs) - return replace(validator, require_image_holdout=True, - command_image_observations=independent_pixels(validator.command_observations)) - monkeypatch.setattr(finalization, 'FrozenTagArtifactValidator', independent_validator) - profile, source, records, truth = synthetic_capture(scope='without_finger_roll', - camera_aligned_reference=camera_aligned_reference) - digest = hashlib.sha256(source.read_bytes()).hexdigest() - hashes = {key: 'a'*64 for key in profile.artifacts.protected_input_fields} - hashes['source_urdf_sha256'] = digest - _, fit, artifact = finalize_profile_session(profile=profile, source_urdf=source, - records=records, protected_inputs=hashes, serial_number='SYNTHETIC_PARTIAL_ONLY', - session_dir=tmp_path/'session', standard_loader=lambda _: None) - payload = json.loads(artifact.staged_release.calibration_json.read_text()) - assert len(payload['joints']) == 16 - assert payload['calibration_scope'] == profile_scope(profile) - assert not profile.retained_joints & (fit.zero_offsets_rad.keys() | fit.command_mappings.keys()) - for name in profile.zero.direct_zero_joints: - assert fit.zero_offsets_rad[name] == pytest.approx(truth[name], abs=.004) - before = {j.get('name'): ET.tostring(j) for j in ET.parse(source).findall('joint')} - after = {j.get('name'): ET.tostring(j) for j in ET.parse(artifact.path).findall('joint')} - for name in profile.retained_joints: - assert before[name] == after[name] - mapper = load_unified_mapper(artifact.staged_release.calibration_json) - assert set(mapper.urdf_joint_names) == payload['joints'].keys() - assert len(mapper.map_positions(profile.command.baseline_values)) == 16 - changed = list(profile.command.baseline_values); changed[4] += 1 - with pytest.raises(ValueError, match='uncalibrated_ancestor'): - mapper.map_positions(changed) - rebuilt = rebuild_urdf_from_json(artifact.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() == artifact.path.read_bytes() - assert hashlib.sha256(source.read_bytes()).hexdigest() == digest - corrupt = deepcopy(payload) - corrupt['urdf_correction']['joint_patches']['ring_mcp_roll'] = {'origin_rpy': '0 0 0'} - with pytest.raises(ValueError, match='retained_joint_has_urdf_correction'): - retained_from_payload(corrupt) - del corrupt['calibration_scope'] - with pytest.raises(ValueError, match='joints differ'): - retained_from_payload(corrupt, UrdfKinematicModel(source)) - - -def test_retained_commands_are_checked_only_on_observed_chain(): - profile = load_bundled_hand_profile('o30_right_18', scope='without_finger_roll') - model = UrdfKinematicModel(SOURCE) - retained = profile_scope(profile)['retained_joints'] - commands = list(profile.command.baseline_values); commands[2] = 0 - # Thumb roll requires index avoidance; it does not move the thumb's ancestors. - require_retained_pose(retained, commands, model=model, link='thumb_proximal') - with pytest.raises(ValueError, match='index_mcp_roll'): - require_retained_pose(retained, commands, model=model, link='index_distal') - - -def test_auxiliary_pixels_outside_partial_scope_are_explicit_and_never_primary(): - from types import SimpleNamespace - from linkerhand_calibration.runtime.artifacts.image_evidence import ImageEvidence - from linkerhand_calibration.runtime.image_capture import IMAGE_OBSERVATION_POLICY - from linkerhand_calibration.core.domain.motion_path import record_scan_identity - profile = load_bundled_hand_profile('o30_right_18', scope='without_finger_roll') - evidence = object.__new__(ImageEvidence) - evidence.profile, evidence.source_model = profile, UrdfKinematicModel(SOURCE) - evidence.tasks = {t.key: t for t in profile.motion.tasks} - evidence.tags = {tag.role: (v.name, tag) for v in profile.vision.views for tag in v.tags} - evidence.out_of_scope_images = [] - task = next(t for t in profile.motion.tasks if 'index_mcp_pitch' in t.joints) - row = dict(kind='image_observation_frame', policy=IMAGE_OBSERVATION_POLICY, - view='side', task_name=task.key, cycle=3, direction='increasing', attempt=1, - sample_phase='steady', command_unit='u8', command_vector=list(profile.command.baseline_values), - feedback_vector=list(profile.command.baseline_values), state_image_sync_error_ns=0, - command_direction_by_index=['increasing']*20, tags={'index_dip': {}}) - extra = deepcopy(row); extra['task_name'] = 'thumb_cmc_roll_front'; extra['command_vector'][2] = 0 - evidence.raw_frames = {'side:1': row, 'side:2': extra} - evidence.completed = {record_scan_identity(r): dict(r, passed=True) for r in (row, extra)} - primary = SimpleNamespace(role='index_dip', sample_id='side:1') - # This test isolates evidence routing; the end-to-end test uses independent pixels. - evidence.primary_holdout = lambda _: (primary,) - evidence._image = lambda frame, **kwargs: primary if kwargs['sample_id']=='side:1' else None - mappings = {n: SimpleNamespace(motor_index=profile.command.command_index_by_joint[n]) - for n in profile.measurement.measurements} - assert evidence.all_view_holdout((primary,), mappings=mappings, input_kind='command', holdout_cycle=3) == (primary,) - assert evidence.out_of_scope_images == [{'sample_id': 'side:2', 'role': 'index_dip', - 'task_name': 'thumb_cmc_roll_front', 'retained_ancestors': ['index_mcp_roll'], - 'reason': 'unmeasured_ancestor_outside_declared_hold'}] - row['command_vector'][2] = 0 - with pytest.raises(ValueError, match='primary_image_has_uncalibrated_moving_ancestor'): - evidence.all_view_holdout((primary,), mappings=mappings, input_kind='command', holdout_cycle=3) diff --git a/src/linkerhand_calibration/test/test_passed_resume.py b/src/linkerhand_calibration/test/test_passed_resume.py deleted file mode 100644 index 3ae8875..0000000 --- a/src/linkerhand_calibration/test/test_passed_resume.py +++ /dev/null @@ -1,225 +0,0 @@ -"""Passed-task continuation skips old motion/replay and retains live safety.""" - -from dataclasses import replace -from types import SimpleNamespace - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.joint_resume import JointResume -from linkerhand_calibration.runtime import passed_resume -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from runtime_host_fixture import coordinator_fixture, ready - - -def passed_records(engine, task): - records = [dict(kind='scan_unit_complete', task_name=unit.task_key, cycle=unit.cycle, - direction=unit.direction, segment_key=unit.segment_key, attempt=1, passed=True) - for unit in engine.scan_units() if unit.task_key == task.key] - return [*records, dict(kind='task_input_support_checked', task_name=task.key, passed=True)] - - -def stub_references(tasks): - return {name: SimpleNamespace(joint=name, task_key=task.key, samples=(), - as_record=lambda name=name: dict(kind='joint_zero_reference', joint=name)) - for task in tasks for name in task.joints} - - -def test_passed_parent_model_keeps_original_source_in_new_observation_epoch(): - from linkerhand_calibration.runtime.parent_reference import ParentReferenceRegistry - from test_image_motion_provenance import recorded_image - - frame, _ = recorded_image() - moving = frame['roles']['moving'] - report = dict(task_name='task', view='front', session_epoch=2, - evidence_ids={'moving': 'a' * 64}, frozen_image_model=moving['frozen_image_model'], - image_model_sha256=moving['image_model_sha256']) - registry = ParentReferenceRegistry(None, {}) - passed_resume.restore_parent_models(registry, [report], 7) - task = SimpleNamespace(view='front', parent_reference=SimpleNamespace(pose_joint='hinge', geometry_joint='hinge')) - pose_source, geometry_source = registry.sources(task, 7) - assert pose_source == geometry_source - assert pose_source[1] is report and report['session_epoch'] == 2 - assert dict(pose_source[0].evidence_ids)['moving'] == 'a' * 64 - - -def test_passed_resume_does_not_restore_incomplete_task_preparation(monkeypatch): - profile = load_bundled_hand_profile('o30_right_18') - engine, resume = CalibrationEngine(profile), JointResume(profile) - passed, incomplete = profile.motion.tasks[:2] - rows = [*passed_records(engine, passed), - dict(kind='motion_branch_observation', task_name=passed.key, - view=passed.view, image_stamp_ns=1, session_epoch=1), - dict(kind='motion_branch_observation', task_name=incomplete.key, - view=incomplete.view, image_stamp_ns=2, session_epoch=1), - dict(kind='motion_branch_initialization', task_name=incomplete.key, - view=incomplete.view, status='resolved', session_epoch=1)] - monkeypatch.setattr(passed_resume, 'read_joint_zero_references', - lambda *args: stub_references([passed, incomplete])) - - passed_resume.stage_passed_tasks(resume, engine, rows) - pending = passed_resume.reference_import(resume) - - assert {row.get('task_name') for row in pending.rows if row.get('task_name')} == {passed.key} - assert not pending.model_records - - -def test_taskwise_passed_resume_does_not_restore_unpassed_visit_model(): - from linkerhand_calibration.runtime import staged_resume - - profile = load_bundled_hand_profile('o30_right_18') - passed, incomplete = profile.motion.tasks[:2] - references = stub_references([passed, incomplete]) - rows = [ - dict(kind='motion_branch_observation', task_name=passed.key, - view=passed.view, image_stamp_ns=1, session_epoch=1), - dict(kind='motion_branch_observation', task_name=incomplete.key, - view=incomplete.view, image_stamp_ns=2, session_epoch=1), - dict(kind='motion_branch_initialization', task_name=incomplete.key, - view=incomplete.view, status='resolved', session_epoch=1), - ] - accepted = next(unit for unit in CalibrationEngine(profile).scan_units() - if unit.task_key == passed.key) - resume = JointResume(profile) - resume.rows, resume.references, resume.units = tuple(rows), references, {accepted.identity} - - pending = staged_resume.reference_import(resume, reuse_passed_visits=True) - - assert set(pending.references) == set(passed.joints) - assert {row.get('task_name') for row in pending.rows if row.get('task_name')} == {passed.key} - assert not pending.model_records - - -def test_passed_resume_omits_orphaned_observer_cancellation(): - from linkerhand_calibration.runtime import staged_resume - - profile = load_bundled_hand_profile('o30_right_18') - task = profile.motion.tasks[0] - accepted = next(unit for unit in CalibrationEngine(profile).scan_units() - if unit.task_key == task.key) - orphan = dict(kind='observer_preparation_cancelled_before_excursion', - joint=task.joints[0], observer_preparation_id='a'*64, - session_epoch=1, stamp_ns=1) - resume = JointResume(profile) - resume.rows = (orphan,) - resume.references = stub_references([task]) - resume.units = {accepted.identity} - - pending = staged_resume.reference_import(resume, reuse_passed_visits=True) - - assert orphan not in pending.rows - - -@pytest.mark.parametrize('failure', [None, 'missing', 'failed', 'support', 'new_attempt']) -def test_last_group_failure_keeps_96_prior_passes_without_recalculating(monkeypatch, failure): - profile = load_bundled_hand_profile('o30_right_18') - engine, resume = CalibrationEngine(profile), JointResume(profile) - rows = [row for task in profile.motion.tasks for row in passed_records(engine, task)] - if failure == 'missing': - rows.pop(-2) - elif failure == 'failed': - rows[-2]['passed'] = False - elif failure == 'support': - rows[-1]['passed'] = False - elif failure == 'new_attempt': - rows.append({**rows[-2], 'attempt': 2}) - monkeypatch.setattr(passed_resume, 'read_joint_zero_references', lambda *args: stub_references(profile.motion.tasks)) - from linkerhand_calibration.runtime import motion_provenance, scan_quality, task_quality - for module, name in ((motion_provenance, 'validate_motion_provenance'), - (scan_quality, 'evaluate_capture_unit'), (task_quality, 'evaluate_task_input_support')): - monkeypatch.setattr(module, name, lambda *a, **kw: pytest.fail('unexpected historical validation')) - passed_resume.stage_passed_tasks(resume, engine, rows) - assert len(resume.units) == (108 if failure is None else 96) - assert len(resume.references) == (20 if failure is None else 16) - assert resume.verified == set(resume.references) - - -@pytest.mark.parametrize('interruption', [None, 'abort', 'feedback']) -@pytest.mark.parametrize('staged', [False, True]) -def test_direct_resume_imports_durably_then_skips_old_joint_motion(tmp_path, monkeypatch, interruption, staged): - from linkerhand_calibration.core.domain.capture_plan import configure_training, STAGED - from linkerhand_calibration.runtime import staged_resume - host, clock = coordinator_fixture(tmp_path, model='o30' if staged else 'o6', - profile_transform=(lambda p: configure_training(p, STAGED)) if staged else None) - try: - host.parameters = replace(host.parameters, resume_mode='passed') - task = host.profile.motion.tasks[0] - expected_units = sum(unit.task_key == task.key for unit in host.execution.session.engine.scan_units()) - rows = [dict(kind='motion_branch_observation', view=task.view, image_stamp_ns=i, - session_epoch=1, task_name=task.key) - for i in range(257)] + [dict(kind='observer_preparation_decision', - task_name=task.key, joint=task.joints[0], action='excite', - action_index=0, session_epoch=1, stamp_ns=1)] + passed_records( - host.execution.session.engine, task) - references = stub_references([task]) - monkeypatch.setattr(passed_resume, 'read_joint_zero_references', lambda *args: references) - resume = JointResume(host.profile) - if staged: - rows = [r for r in rows if r.get('kind') != 'scan_unit_complete' or r['cycle'] == 0] - resume.references, resume.rows = references, tuple(rows) - resume.units = {u.identity for u in host.execution.session.engine.scan_units() - if u.task_key == task.key and u.cycle == 0} - expected_units = len(resume.units) - pending = staged_resume.reference_import(resume, reuse_passed_visits=True) - assert resume.verified_visits == {(name, 0) for name in task.joints} - else: - passed_resume.stage_passed_tasks(resume, host.execution.session.engine, rows) - pending = passed_resume.reference_import(resume) - host._resume_checkpoint = SimpleNamespace(joints=resume, - passed_import=pending, rows=rows, frozen_training=None, - fixed_reference={'protected_hashes': dict(host.parameters.protected_inputs)}) - monkeypatch.setattr(passed_resume, 'reference_import', - lambda *a: pytest.fail('checkpoint indexing must finish before live callbacks')) - from linkerhand_calibration.runtime.resume import ResumeVerifier - monkeypatch.setattr(ResumeVerifier, 'compare', lambda *a: pytest.fail('unexpected physical recheck')) - monkeypatch.setattr(host, '_lock_joint_zeros', lambda *a: pytest.fail('old zero must not be acquired')) - ready(host, clock) - assert host.start().success - clock.now += .1 - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.execution.session.phase = Phase.RESUME_VERIFY - host._current_fingerprint = {'protected_hashes': dict(host.parameters.protected_inputs)} - host.tick() - assert host._passed_reference_import is not None - host.tick() - assert not host.execution.zero_references and host._resumed_count == 0 - if interruption: - if interruption == 'abort': - host.abort() - else: - clock.now += 2 - host.tick() - assert host.state in {'ABORTED', 'PAUSED'} - assert not host.execution.zero_references and host._resumed_count == 0 - else: - # Reference restoration is intentionally sliced across callbacks - # and journal barriers run on a worker. Give that worker real - # scheduling opportunities instead of spinning only the fake clock. - import time - for _ in range(400): - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.tick() - if host.execution.session.current_unit.task_key != task.key: - break - time.sleep(.001) - assert host._resumed_count == expected_units - assert host.execution.zero_references == references - assert not host.branch_initialization.observer_preparation.decisions - assert host.segment is None - import json - imported = [json.loads(line) for line in host.raw_path.read_text().splitlines()] - if staged: - assert sum(r.get('kind') == 'scan_unit_complete' for r in imported) == expected_units - audit = next(r for r in imported if r.get('kind') == 'passed_tasks_reused') - assert audit['historical_replay_performed'] is False - assert audit['full_replay_required_before_publication'] is True - assert audit['physical_zero_reverification_performed'] is False - binding = next(r for r in imported if r.get('kind') == 'passed_pose_models_restored') - assert binding['unchanged_installation_declared'] is True - else: - again = JointResume(host.profile) - passed_resume.stage_passed_tasks(again, host.execution.session.engine, imported) - assert len(again.units) == expected_units - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_paused_snapshot.py b/src/linkerhand_calibration/test/test_paused_snapshot.py deleted file mode 100644 index 8f343c6..0000000 --- a/src/linkerhand_calibration/test/test_paused_snapshot.py +++ /dev/null @@ -1,106 +0,0 @@ -"""A paused task must never display another scan's successful statistics.""" - -import pytest - -from linkerhand_calibration.runtime.engine import SweepQuality -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.status import render_status_zh -from runtime_host_fixture import coordinator_fixture, ready - - -@pytest.mark.parametrize('current_samples', [0, 37]) -def test_paused_front_task_reports_its_own_partial_capture(tmp_path, current_samples): - host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb') - try: - ready(host, clock) - assert host.start().success - session = host.execution.session - session._unit_index = next(i for i, unit in enumerate(session._units) - if unit.task_key == 'thumb_pitch_ip_front' and unit.direction == 'decreasing') - session.phase = Phase.PAUSED - unit = session.current_unit - host.execution.last_quality = SweepQuality(True, (), (), {'previous_yaw': { - 'valid_frames': 194, 'feedback_span': 1., 'feedback_bins': 132, 'maximum_bin_gap': 6}}) - rows = [] - for joint in ('rh_thumb_cmc_pitch', 'rh_thumb_ip'): - for index in range(current_samples): - rows.append({'kind': 'joint_sample', 'joint': joint, 'view': 'front', - 'task_name': unit.task_key, 'cycle': unit.cycle, 'direction': unit.direction, - 'attempt': 1, 'sample_phase': 'sweep', 'image_stamp_ns': index+1, - 'feedback_u8': 255-index}) - # Stale or different-direction observations cannot fill current bins. - rows += [dict(row, task_name='thumb_yaw_top') for row in rows] - rows += [dict(row, direction='increasing') for row in rows] - host._unit_rows = rows - status = host.snapshot().calibration - assert status.task.key == unit.task_key - assert status.acquisition.valid_samples == current_samples - assert status.acquisition.coverage_01 < .2 - assert status.acquisition.bins <= current_samples - assert host.execution.last_quality.metrics['previous_yaw']['valid_frames'] == 194 - finally: - host.close() - - -@pytest.mark.parametrize('phase', [Phase.PAUSED, Phase.ABORTED, Phase.FAILED]) -def test_interruption_phase_is_not_hidden_by_last_joint_zero_motion(tmp_path, phase): - host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb') - try: - ready(host, clock) - assert host.start().success - host.execution.session.phase = phase - host._motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 40., - task_key='thumb_yaw_top', command_index=1, zero_joints=('rh_thumb_cmc_yaw',)) - snapshot = host.snapshot() - assert snapshot.calibration.phase == phase.value - assert snapshot.calibration.task.label == '采集关节零位：rh_thumb_cmc_yaw' - assert '阶段：采集并冻结关节零位' not in render_status_zh(snapshot.as_dict()) - finally: - host.close() - - -@pytest.mark.parametrize('diagnostic', [True, False]) -def test_diagnostic_status_separates_raw_corner_coverage_from_accepted_pose_coverage(tmp_path, diagnostic): - from test_diagnostic_quality import raw_frames - - host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb' if diagnostic else '') - try: - ready(host, clock) - assert host.start().success - host.execution.session.phase = Phase.SWEEP - unit = host.execution.session.current_unit - host._unit_rows = raw_frames(host.profile, unit) - snapshot = host.snapshot() - rendered = render_status_zh(snapshot.as_dict()) - if diagnostic: - assert snapshot.calibration.acquisition.valid_samples == 129 - assert snapshot.calibration.acquisition.coverage_01 == 1. - assert snapshot.diagnostic_capture['accepted_pose_quality']['passed'] is False - assert snapshot.diagnostic_capture['accuracy_validated'] is False - assert '原始观测 129；覆盖 100.0%' in rendered - assert '可信姿态样本 0；覆盖 0.0%（未进行精度验收）' in rendered - else: - assert snapshot.calibration.acquisition.valid_samples == 0 - assert snapshot.diagnostic_capture is None - assert '有效样本 0' in rendered and '原始观测' not in rendered - finally: - host.close() - - -def test_paused_retry_does_not_reuse_first_attempt_samples(tmp_path): - host, clock = coordinator_fixture(tmp_path, diagnostic_capture='o6_thumb') - try: - ready(host, clock) - assert host.start().success - session = host.execution.session - unit = session.current_unit - session._retry_by_unit[(unit.task_key, unit.cycle, unit.direction)] = 1 - session.phase = Phase.PAUSED - host._unit_rows = [{'joint': 'rh_thumb_cmc_yaw', 'view': 'top', - 'task_name': unit.task_key, 'cycle': unit.cycle, 'direction': unit.direction, - 'attempt': 1, 'sample_phase': 'sweep', 'image_stamp_ns': i+1, - 'feedback_u8': i} for i in range(256)] - assert host.snapshot().calibration.acquisition.valid_samples == 0 - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_pnp.py b/src/linkerhand_calibration/test/test_pnp.py deleted file mode 100644 index e180b4b..0000000 --- a/src/linkerhand_calibration/test/test_pnp.py +++ /dev/null @@ -1,895 +0,0 @@ -from __future__ import annotations - -import cv2 -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.pnp import ( - SquareTagGroupPoseTracker, - SquareTagPose, - SquareTagPoseTracker, - rotation_distance_rad, - select_continuous_pose, - select_rigid_group_trajectory, - solve_square_tag_ippe, - square_object_points, -) - - -def _camera_matrix() -> np.ndarray: - return np.asarray( - [ - [650.0, 0.0, 640.0], - [0.0, 650.0, 360.0], - [0.0, 0.0, 1.0], - ] - ) - - -def _project( - rotation: Rotation, - translation_xyz_m: np.ndarray, - *, - tag_size_m: float = 0.01, -) -> np.ndarray: - rotation_vector, _ = cv2.Rodrigues(rotation.as_matrix()) - corners, _ = cv2.projectPoints( - square_object_points(tag_size_m), - rotation_vector, - translation_xyz_m, - _camera_matrix(), - np.zeros((4, 1)), - ) - return corners.reshape(4, 2) - - -def test_ippe_recovers_known_square_tag_pose() -> None: - expected_rotation = Rotation.from_euler( - "xyz", [10.0, -15.0, 25.0], degrees=True - ) - expected_translation = np.asarray([0.02, -0.01, 0.25]) - candidates = solve_square_tag_ippe( - _project(expected_rotation, expected_translation), - tag_size_m=0.01, - camera_matrix=_camera_matrix(), - ) - - assert len(candidates) == 2 - actual = min(candidates, key=lambda item: item.reprojection_error_px) - assert ( - rotation_distance_rad( - expected_rotation.as_quat(), - actual.quaternion_xyzw, - ) - < 1.0e-8 - ) - assert np.allclose(actual.translation_xyz_m, expected_translation) - assert actual.reprojection_error_px < 1.0e-8 - - -def test_temporal_selection_breaks_near_reprojection_tie() -> None: - previous = SquareTagPose( - quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), - translation_xyz_m=(0.0, 0.0, 0.25), - reprojection_error_px=0.2, - ) - continuous = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 2.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.001, 0.0, 0.25), - reprojection_error_px=0.11, - ) - flipped = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 55.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.0, 0.0, 0.25), - reprojection_error_px=0.1, - ) - - selected, reason = select_continuous_pose( - [flipped, continuous], - previous=previous, - maximum_reprojection_error_px=1.5, - reprojection_tie_px=0.03, - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - maximum_tag_tilt_rad=np.deg2rad(75.0), - ) - - assert reason == "" - assert selected == continuous - - -def test_stateless_selector_requires_confirmation_for_large_branch_correction() -> None: - stale_mirror = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 55.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.0, 0.0, 0.25), - reprojection_error_px=0.25, - ) - true_pose = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 2.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.001, 0.0, 0.25), - reprojection_error_px=0.05, - ) - continued_mirror = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 54.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.0, 0.0, 0.25), - reprojection_error_px=0.24, - ) - - selected, reason = select_continuous_pose( - [continued_mirror, true_pose], - previous=stale_mirror, - maximum_reprojection_error_px=1.5, - reprojection_tie_px=0.03, - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - maximum_tag_tilt_rad=np.deg2rad(75.0), - ) - - assert reason == "pose_branch_unconfirmed" - assert selected is None - - -@pytest.mark.parametrize("tie", [-0.01, 1.5, 2.0, float("nan"), float("inf")]) -def test_invalid_branch_tolerance_is_rejected_before_tracking(tie) -> None: - parameters = dict( - maximum_reprojection_error_px=1.5, reprojection_tie_px=tie, - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, maximum_tag_tilt_rad=np.deg2rad(75.0), - ) - with pytest.raises(ValueError, match="reprojection_tie_px"): - SquareTagPoseTracker(**parameters, reset_after_seconds=5.0) - with pytest.raises(ValueError, match="reprojection_tie_px"): - select_continuous_pose([], previous=None, **parameters) - - -def test_tracker_recovers_after_timestamp_gap() -> None: - tracker = SquareTagPoseTracker( - maximum_reprojection_error_px=1.5, - maximum_pose_jump_rad=np.deg2rad(1.0), - maximum_translation_jump_m=0.04, - maximum_tag_tilt_rad=np.deg2rad(75.0), - reset_after_seconds=0.5, - ) - first_rotation = Rotation.from_euler("y", 0.0, degrees=True) - # Keep both IPPE solutions near-tied so this exercises gap recovery, - # rather than correction of a branch with clearly worse image fit. - second_rotation = Rotation.from_euler("y", 2.0, degrees=True) - for index in range(5): - first, first_reason = tracker.estimate( - "t0", _project(first_rotation, np.asarray([0.0, 0.0, 0.25])), - tag_size_m=0.01, camera_matrix=_camera_matrix(), - stamp_ns=1_000_000_000 + index * 30_000_000) - rejected, rejection_reason = tracker.estimate( - "t0", - _project(second_rotation, np.asarray([0.0, 0.0, 0.25])), - tag_size_m=0.01, - camera_matrix=_camera_matrix(), - stamp_ns=1_150_000_000, - ) - ambiguous, ambiguous_reason = tracker.estimate( - "t0", - _project(second_rotation, np.asarray([0.0, 0.0, 0.25])), - tag_size_m=0.01, - camera_matrix=_camera_matrix(), - stamp_ns=1_700_000_000, - ) - - assert first is not None - assert first_reason == "" - assert rejected is None - assert rejection_reason == "pose_jump" - assert ambiguous is None - assert ambiguous_reason == "pose_branch_unconfirmed" - # A real timestamp gap permits reacquisition, but cannot itself resolve - # ambiguous image evidence. A larger projected Tag supplies a distinct - # image-fit winner; independent frames must then confirm it again. - for index in range(5): - recovered, recovered_reason = tracker.estimate( - "t0", _project(second_rotation, np.asarray([0.0, 0.0, 0.25]), tag_size_m=0.05), - tag_size_m=0.05, camera_matrix=_camera_matrix(), - stamp_ns=1_800_000_000 + index * 30_000_000) - assert recovered is not None - assert recovered_reason == "" - - -def _pose( - rotation_deg: float, - x_m: float, - reprojection_error_px: float, -) -> SquareTagPose: - return SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler( - "y", rotation_deg, degrees=True - ).as_quat() - ), - translation_xyz_m=(x_m, 0.0, 0.25), - reprojection_error_px=reprojection_error_px, - ) - - -def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None: - per_tag = SquareTagPoseTracker( - maximum_reprojection_error_px=1.5, - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - maximum_tag_tilt_rad=np.deg2rad(75.0), - reset_after_seconds=5.0, - ) - oblique_rotation = Rotation.from_euler("y", 89.0, degrees=True) - - independent, reason = per_tag.estimate( - "child", - _project(oblique_rotation, np.asarray([0.03, 0.0, 0.25])), - tag_size_m=0.01, - camera_matrix=_camera_matrix(), - stamp_ns=1_000_000_000, - ) - - assert independent is None - assert reason == "no_pose_within_reprojection_or_tilt_limit" - assert per_tag.last_candidates_by_role["child"] - diagnostics = per_tag.last_candidate_diagnostics_by_role["child"] - assert diagnostics["reprojection_candidate_count"] == 2 - assert diagnostics["independent_tilt_candidate_count"] == 0 - assert diagnostics["minimum_candidate_tilt_deg"] > 75.0 - - group = SquareTagGroupPoseTracker( - roles=("base", "child"), - adjacent_pairs=(("base", "child"),), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - ) - selected, group_reason = group.select( - { - "base": (_pose(0.0, 0.0, 0.05),), - "child": per_tag.last_candidates_by_role["child"], - }, - stamp_ns=1_000_000_000, - ) - - assert group_reason == "" - assert selected is not None - assert "child" in selected - - -def test_group_tracker_exposes_roles_without_candidates() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("base", "parent", "child"), - adjacent_pairs=(("base", "parent"), ("parent", "child")), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - ) - - selected, reason = tracker.select( - { - "base": (_pose(0.0, 0.0, 0.05),), - "parent": (), - }, - stamp_ns=1_000_000_000, - ) - - assert selected is None - assert reason == "group_missing_pose_candidates" - assert tracker.last_missing_roles == ("parent", "child") - - -def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("base", "mcp", "ip"), - adjacent_pairs=(("base", "mcp"), ("mcp", "ip")), - maximum_pose_jump_rad=np.deg2rad(360.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(1000.0), - relative_translation_scale_m=1.0, - reprojection_scale_px=0.1, - reprojection_weight=1.0, - reset_after_seconds=5.0, - coupled_rotation_pairs=( - ("base", "mcp", "mcp", "ip", 1.03), - ), - coupled_rotation_scale_rad=np.deg2rad(3.0), - maximum_coupled_rotation_residual_rad=np.deg2rad(7.5), - ) - baseline = { - "base": (_pose(0.0, 0.00, 0.05),), - "mcp": (_pose(0.0, 0.03, 0.05),), - "ip": (_pose(0.0, 0.06, 0.05),), - } - selected, reason = tracker.select( - baseline, - stamp_ns=1_000_000_000, - trajectory_command_u8=255, - trajectory_direction="decreasing", - ) - assert selected is not None - assert reason == "" - - driver = _pose(10.0, 0.03, 0.05) - measured_ip = _pose(20.3, 0.06, 0.20) - lower_reprojection_mirror = _pose(30.0, 0.06, 0.05) - selected, reason = tracker.select( - { - "base": (_pose(0.0, 0.00, 0.05),), - "mcp": (driver,), - "ip": (lower_reprojection_mirror, measured_ip), - }, - stamp_ns=1_033_000_000, - trajectory_command_u8=128, - trajectory_direction="decreasing", - ) - - assert reason == "" - assert selected is not None - assert selected["ip"] == measured_ip - - fallback, fallback_reason = tracker.select( - { - "base": (_pose(0.0, 0.00, 0.05),), - "mcp": (driver,), - "ip": (lower_reprojection_mirror,), - }, - stamp_ns=1_066_000_000, - trajectory_command_u8=128, - trajectory_direction="decreasing", - ) - assert fallback is not None - assert fallback_reason == "" - assert fallback["ip"] == lower_reprojection_mirror - - -def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("t0", "t3", "t4", "t5"), - adjacent_pairs=( - ("t0", "t3"), - ("t3", "t4"), - ("t4", "t5"), - ), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - ) - first = { - "t0": (_pose(0.0, 0.00, 0.05),), - "t3": (_pose(5.0, 0.03, 0.05),), - "t4": (_pose(15.0, 0.06, 0.05),), - "t5": (_pose(25.0, 0.09, 0.05),), - } - selected_first, first_reason = tracker.select( - first, - stamp_ns=1_000_000_000, - ) - assert first_reason == "" - assert selected_first is not None - - # The per-tag minimum-error solutions move only a few degrees and can - # therefore fool independent trackers. Together they change T4->T5 by - # 8 deg; the slightly higher-error pair preserves the physical chain. - continuous_t4 = _pose(16.0, 0.061, 0.20) - continuous_t5 = _pose(26.0, 0.091, 0.20) - independent_best_t4 = _pose(19.0, 0.061, 0.05) - independent_best_t5 = _pose(21.0, 0.091, 0.05) - second = { - "t0": (_pose(0.2, 0.00, 0.05),), - "t3": (_pose(5.2, 0.03, 0.05),), - "t4": (independent_best_t4, continuous_t4), - "t5": (independent_best_t5, continuous_t5), - } - selected, reason = tracker.select( - second, - stamp_ns=1_033_000_000, - ) - - assert reason == "" - assert selected is not None - assert selected["t4"] == continuous_t4 - assert selected["t5"] == continuous_t5 - assert tracker.branch_correction_counts == {"t4": 1, "t5": 1} - - -def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("t4", "t5"), - adjacent_pairs=(("t4", "t5"),), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - ) - true_t4 = _pose(30.0, 0.06, 0.05) - true_t5 = _pose(65.0, 0.09, 0.05) - selected, _ = tracker.select( - {"t4": (true_t4,), "t5": (true_t5,)}, - stamp_ns=1_000_000_000, - ) - assert selected is not None - - return_t4 = _pose(29.5, 0.06, 0.20) - return_t5 = _pose(64.5, 0.09, 0.20) - mirror_t4 = _pose(33.0, 0.06, 0.04) - mirror_t5 = _pose(57.0, 0.09, 0.04) - selected, reason = tracker.select( - { - "t4": (mirror_t4, return_t4), - "t5": (mirror_t5, return_t5), - }, - stamp_ns=1_033_000_000, - ) - - assert reason == "" - assert selected == {"t4": return_t4, "t5": return_t5} - - -def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("parent", "child"), - adjacent_pairs=(("parent", "child"),), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - ) - parent = _pose(0.0, 0.00, 0.05) - for stamp, command, angle in ( - (1_000_000_000, 255, 20.0), - (1_033_000_000, 64, 45.0), - (1_066_000_000, 0, 50.0), - ): - selected, reason = tracker.select( - {"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)}, - stamp_ns=stamp, - trajectory_command_u8=command, - trajectory_direction="decreasing", - ) - assert reason == "" - assert selected is not None - - # The true return contains 3 deg of real hysteresis along the learned - # y-axis. The lower-error mirror candidate is temporally smoother but - # adds a 2 deg tilt outside that physical motion axis. - true_return = _pose(42.0, 0.03, 0.20) - smoother_mirror = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat() - ), - translation_xyz_m=(0.03, 0.0, 0.25), - reprojection_error_px=0.01, - ) - selected, reason = tracker.select( - { - "parent": (parent,), - "child": (smoother_mirror, true_return), - }, - stamp_ns=1_099_000_000, - trajectory_command_u8=64, - trajectory_direction="increasing", - ) - - assert reason == "" - assert selected == {"parent": parent, "child": true_return} - - -def test_group_tracker_initializes_from_multiple_static_frames() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("parent", "child"), - adjacent_pairs=(("parent", "child"),), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - initialization_frames=8, - ) - parent = _pose(0.0, 0.00, 0.05) - true_child = _pose(20.0, 0.03, 0.10) - - for index in range(7): - false_child = _pose(5.0 * index, 0.03, 0.05) - selected, reason = tracker.select( - { - "parent": (parent,), - "child": (false_child, true_child), - }, - stamp_ns=1_000_000_000 + index * 33_000_000, - ) - assert selected is None - assert reason == f"group_initializing:{index + 1}/8" - - false_child = _pose(35.0, 0.03, 0.05) - selected, reason = tracker.select( - { - "parent": (parent,), - "child": (false_child, true_child), - }, - stamp_ns=1_231_000_000, - ) - - assert reason == "" - assert selected == {"parent": parent, "child": true_child} - assert ( - tracker.last_initialization_quality["initialization_search"] - == "static_reference" - ) - assert tracker.last_initialization_quality[ - "p95_pair_rotation_drift_rad" - ] < np.deg2rad(1.0) - - -def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("parent", "child"), - adjacent_pairs=(("parent", "child"),), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - initialization_frames=8, - ) - parent = _pose(0.0, 0.00, 0.05) - anchored_child = _pose(20.0, 0.03, 0.20) - for index in range(8): - selected, reason = tracker.select( - {"parent": (parent,), "child": (anchored_child,)}, - stamp_ns=1_000_000_000 + index * 33_000_000, - trajectory_command_u8=255, - trajectory_direction="decreasing", - ) - assert reason == "" - assert selected == {"parent": parent, "child": anchored_child} - - tracker.reset(preserve_task_reference=True) - lower_error_mirror = _pose(5.0, 0.03, 0.01) - for index in range(8): - selected, reason = tracker.select( - { - "parent": (parent,), - "child": (lower_error_mirror, anchored_child), - }, - stamp_ns=2_000_000_000 + index * 33_000_000, - trajectory_command_u8=255, - trajectory_direction="decreasing", - ) - - assert reason == "" - assert selected == {"parent": parent, "child": anchored_child} - assert tracker.last_initialization_quality["task_reference_used"] == "true" - - tracker.reset() - for index in range(8): - selected, reason = tracker.select( - { - "parent": (parent,), - "child": (lower_error_mirror, anchored_child), - }, - stamp_ns=3_000_000_000 + index * 33_000_000, - trajectory_command_u8=255, - trajectory_direction="decreasing", - ) - assert reason == "" - assert selected == {"parent": parent, "child": lower_error_mirror} - - -def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("mcp", "pip", "dip"), - adjacent_pairs=(("mcp", "pip"), ("pip", "dip")), - normal_alignment_pairs=(("mcp", "pip"), ("pip", "dip")), - normal_alignment_scale_rad=np.deg2rad(5.0), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - initialization_frames=8, - maximum_normal_alignment_rad=np.deg2rad(15.0), - ) - mcp = _pose(12.0, 0.00, 0.05) - correct_pip = _pose(14.0, 0.03, 0.10) - mirror_pip = _pose(50.0, 0.03, 0.05) - dip = _pose(15.0, 0.06, 0.05) - - for index in range(8): - selected, reason = tracker.select( - { - "mcp": (mcp,), - "pip": (mirror_pip, correct_pip), - "dip": (dip,), - }, - stamp_ns=1_000_000_000 + index * 33_000_000, - ) - - assert reason == "" - assert selected == { - "mcp": mcp, - "pip": correct_pip, - "dip": dip, - } - assert tracker.last_initialization_quality[ - "maximum_normal_alignment_rad" - ] < np.deg2rad(3.0) - - -def test_static_group_rejects_when_no_aligned_branch_exists() -> None: - tracker = SquareTagGroupPoseTracker( - roles=("base", "moving"), - adjacent_pairs=(("base", "moving"),), - normal_alignment_pairs=(("base", "moving"),), - normal_alignment_scale_rad=np.deg2rad(5.0), - maximum_normal_alignment_rad=np.deg2rad(15.0), - maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - relative_rotation_scale_rad=np.deg2rad(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=5.0, - initialization_frames=8, - ) - base = _pose(0.0, 0.00, 0.05) - mirror = _pose(38.0, 0.03, 0.05) - - for index in range(8): - selected, reason = tracker.select( - {"base": (base,), "moving": (mirror,)}, - stamp_ns=1_000_000_000 + index * 33_000_000, - ) - - assert selected is None - assert reason == "group_normal_alignment" - - -def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None: - roles = ("t3", "t4", "t5") - mount_rotations = { - "t3": Rotation.identity(), - "t4": Rotation.from_euler("z", 20.0, degrees=True), - "t5": Rotation.from_euler("z", -15.0, degrees=True), - } - mount_positions = { - "t3": np.asarray([0.0, 0.0, 0.0]), - "t4": np.asarray([0.025, 0.0, 0.0]), - "t5": np.asarray([0.05, 0.0, 0.0]), - } - false_factors = {"t3": 0.5, "t4": -0.5, "t5": 1.0} - frames = [] - true_frames = [] - for angle_deg in np.linspace(0.0, 45.0, 30): - group_rotation = Rotation.from_euler( - "y", angle_deg, degrees=True - ) - origin = np.asarray([0.0, 0.0, 0.3]) - candidates = {} - truths = {} - for role in roles: - true_rotation = group_rotation * mount_rotations[role] - true_translation = origin + group_rotation.apply( - mount_positions[role] - ) - true_pose = SquareTagPose( - quaternion_xyzw=tuple(true_rotation.as_quat()), - translation_xyz_m=tuple(true_translation), - reprojection_error_px=0.10, - ) - false_rotation = true_rotation * Rotation.from_euler( - "x", - false_factors[role] * angle_deg, - degrees=True, - ) - false_pose = SquareTagPose( - quaternion_xyzw=tuple(false_rotation.as_quat()), - translation_xyz_m=tuple( - true_translation - + np.asarray( - [ - 0.0, - false_factors[role] * angle_deg / 10000.0, - 0.0, - ] - ) - ), - reprojection_error_px=0.05, - ) - candidates[role] = (false_pose, true_pose) - truths[role] = true_pose - frames.append(candidates) - true_frames.append(truths) - - selected, quality = select_rigid_group_trajectory( - frames, - roles=roles, - fixed_pairs=(("t3", "t4"), ("t4", "t5")), - reprojection_scale_px=0.1, - rotation_scale_rad=np.deg2rad(5.0), - translation_scale_m=0.01, - ) - - for role in roles: - assert ( - rotation_distance_rad( - selected[-1][role].quaternion_xyzw, - true_frames[-1][role].quaternion_xyzw, - ) - < 1.0e-8 - ) - assert quality["maximum_pair_rotation_drift_rad"] < np.deg2rad(5.0) - assert quality["p95_pair_rotation_drift_rad"] < np.deg2rad(5.0) - - -def test_trajectory_quality_uses_robust_rigid_reference() -> None: - frames = [] - for index in range(30): - child_rotation = Rotation.identity() - if index == 0: - child_rotation = Rotation.from_euler( - "x", 10.0, degrees=True - ) - frames.append( - { - "parent": ( - SquareTagPose( - quaternion_xyzw=tuple( - Rotation.identity().as_quat() - ), - translation_xyz_m=(0.0, 0.0, 0.3), - reprojection_error_px=0.1, - ), - ), - "child": ( - SquareTagPose( - quaternion_xyzw=tuple(child_rotation.as_quat()), - translation_xyz_m=(0.03, 0.0, 0.3), - reprojection_error_px=0.1, - ), - ), - } - ) - - _, quality = select_rigid_group_trajectory( - frames, - roles=("parent", "child"), - fixed_pairs=(("parent", "child"),), - reprojection_scale_px=0.1, - rotation_scale_rad=np.deg2rad(5.0), - translation_scale_m=0.01, - ) - - assert quality["maximum_pair_rotation_drift_rad"] == pytest.approx( - np.deg2rad(10.0) - ) - assert quality["p95_pair_rotation_drift_rad"] == pytest.approx(0.0) - assert quality["median_pair_rotation_drift_rad"] == pytest.approx(0.0) - - -def test_distance_geometry_ignores_planar_orientation_drift() -> None: - frames = [] - for angle_deg in np.linspace(0.0, 45.0, 30): - group = Rotation.from_euler("y", angle_deg, degrees=True) - origin = np.asarray([0.0, 0.0, 0.3]) - parent_position = origin - child_position = origin + group.apply([0.04, 0.0, 0.0]) - # The centres form a perfect rigid pair, while the planar-PnP parent - # orientation contains a pose-dependent error. - parent_rotation = group * Rotation.from_euler( - "z", 0.25 * angle_deg, degrees=True - ) - frames.append( - { - "parent": ( - SquareTagPose( - quaternion_xyzw=tuple(parent_rotation.as_quat()), - translation_xyz_m=tuple(parent_position), - reprojection_error_px=0.1, - ), - ), - "child": ( - SquareTagPose( - quaternion_xyzw=tuple(group.as_quat()), - translation_xyz_m=tuple(child_position), - reprojection_error_px=0.1, - ), - ), - } - ) - - _, quality = select_rigid_group_trajectory( - frames, - roles=("parent", "child"), - fixed_pairs=(("parent", "child"),), - reprojection_scale_px=0.1, - rotation_scale_rad=np.deg2rad(5.0), - translation_scale_m=0.01, - pair_geometry="distance", - ) - - assert quality["pair_geometry"] == "distance" - assert quality["maximum_pair_distance_drift_m"] < 1.0e-10 - assert quality["p95_pair_distance_drift_m"] < 1.0e-10 - assert quality["p95_pair_translation_drift_m"] > 0.001 - - -def test_distance_geometry_rejects_pose_branch_with_changing_length() -> None: - frames = [] - true_children = [] - for index in range(30): - parent = SquareTagPose( - quaternion_xyzw=tuple(Rotation.identity().as_quat()), - translation_xyz_m=(0.0, 0.0, 0.3), - reprojection_error_px=0.1, - ) - true_child = SquareTagPose( - quaternion_xyzw=tuple(Rotation.identity().as_quat()), - translation_xyz_m=(0.04, 0.0, 0.3), - reprojection_error_px=0.1, - ) - false_child = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("x", 10.0, degrees=True).as_quat() - ), - translation_xyz_m=( - 0.04, - 0.020 * np.sin(np.pi * index / 29.0), - 0.3, - ), - reprojection_error_px=0.05, - ) - frames.append( - { - "parent": (parent,), - "child": (false_child, true_child), - } - ) - true_children.append(true_child) - - selected, quality = select_rigid_group_trajectory( - frames, - roles=("parent", "child"), - fixed_pairs=(("parent", "child"),), - reprojection_scale_px=0.1, - rotation_scale_rad=np.deg2rad(5.0), - translation_scale_m=0.002, - pair_geometry="distance", - ) - - assert selected[len(selected) // 2]["child"] == ( - true_children[len(true_children) // 2] - ) - assert quality["p95_pair_distance_drift_m"] < 1.0e-6 diff --git a/src/linkerhand_calibration/test/test_pnp_branch_confirmation.py b/src/linkerhand_calibration/test/test_pnp_branch_confirmation.py deleted file mode 100644 index 09bdc82..0000000 --- a/src/linkerhand_calibration/test/test_pnp_branch_confirmation.py +++ /dev/null @@ -1,309 +0,0 @@ -"""Distinct image evidence must authorize planar branch changes before capture. - -The small real fixture records six O6 discontinuities, not physical ground -truth. Smooth or repeatedly preferred IPPE branches are not accuracy evidence. -""" - -from __future__ import annotations - -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 import tracking -from linkerhand_calibration.pnp import SquareTagPose, SquareTagPoseTracker, solve_square_tag_ippe - - -REAL = json.loads((Path(__file__).parent / "fixtures" / - "o6_thumb_20260914_branch_events.json").read_text()) -START_NS = 1_000_000_000 -STEP_NS = 33_000_000 - - -def _distance(first, second): - return float(np.degrees((Rotation.from_quat(first).inv() * - Rotation.from_quat(second)).magnitude())) - - -def _pose(angle, error): - return SquareTagPose(tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), - (0.0, 0.0, 0.5), float(error)) - - -@pytest.fixture -def source(monkeypatch): - """Control candidate evidence while exercising the real stateful tracker.""" - class Source: - candidates = [_pose(0, .05), _pose(60, .20)] - - def sample(self, tracker, stamp_ns, *, role="moving"): - return tracker.estimate(role, [(0, 0), (1, 0), (1, 1), (0, 1)], - tag_size_m=.016, camera_matrix=np.eye(3), stamp_ns=stamp_ns) - - instance = Source() - monkeypatch.setattr(tracking, "solve_square_tag_ippe", - lambda *args, **kwargs: list(instance.candidates)) - return instance - - -def _confirm(source, tracker, *, start=START_NS, role="moving"): - outcomes = [source.sample(tracker, start + i * STEP_NS, role=role) for i in range(5)] - assert all(pose is None and reason for pose, reason in outcomes[:-1]) - pose, reason = outcomes[-1] - assert pose is not None and reason == "" - return pose, start + 4 * STEP_NS - - -def _freeze(tracker, role="moving"): - revision = tracker.last_candidate_diagnostics_by_role[role]["branch_revision"] - assert tracker.freeze_roles({role: revision}) is True - return revision - - -def test_separated_initial_candidates_need_five_unique_images_and_elapsed_time(source): - tracker = SquareTagPoseTracker() - for i in range(8): - pose, reason = source.sample(tracker, START_NS + i * 1_000_000) - assert pose is None and reason - pose, reason = source.sample(tracker, START_NS + 100_000_000) - assert pose is not None and reason == "" - assert _distance(pose.quaternion_xyzw, _pose(0, .05).quaternion_xyzw) < 1e-9 - detail = tracker.last_candidate_diagnostics_by_role["moving"] - assert detail["observation_stamp_ns"] == START_NS + 100_000_000 - - -def test_duplicate_images_do_not_advance_confirmation(source): - tracker = SquareTagPoseTracker() - for _ in range(8): - pose, reason = source.sample(tracker, START_NS) - assert pose is None and reason - assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_confirmation_count"] == 1 - for i in range(1, 4): - pose, reason = source.sample(tracker, START_NS + i * STEP_NS) - assert pose is None and reason - pose, reason = source.sample(tracker, START_NS + 4 * STEP_NS) - assert pose is not None and reason == "" - - -def test_near_equal_image_fit_never_accumulates_false_initial_confidence(source): - tracker = SquareTagPoseTracker() - source.candidates = [_pose(0, .05), _pose(60, .079)] - for i in range(20): - pose, reason = source.sample(tracker, START_NS + i * STEP_NS) - assert pose is None and reason - assert tracker.reprojection_tie_px == .03 - assert tracker.freeze_roles({"moving": 0}) is False - - -def test_confirmation_does_not_pool_alternating_branches_or_other_roles(source): - tracker = SquareTagPoseTracker() - for i in range(16): - source.candidates = ([_pose(0, .05), _pose(60, .20)] if (i // 2) % 2 == 0 - else [_pose(0, .20), _pose(60, .05)]) - pose, reason = source.sample(tracker, START_NS + i * STEP_NS) - assert pose is None and reason - source.candidates = [_pose(0, .05), _pose(60, .20)] - pose, reason = source.sample(tracker, START_NS + 16 * STEP_NS, role="another") - assert pose is None and reason - assert tracker.last_candidate_diagnostics_by_role["another"]["branch_confirmation_count"] == 1 - - -@pytest.mark.parametrize("candidates", [ - [_pose(0, .05)], [_pose(0, .05), _pose(.5, .06)], - [_pose(0, .05), _pose(60, 1.51)], -]) -def test_single_or_equivalent_solution_confirms_without_an_image_error_margin(source, candidates): - source.candidates = candidates - pose, _ = _confirm(source, SquareTagPoseTracker()) - assert pose is not None - - -def test_unfrozen_stale_branch_is_revised_without_emitting_the_switch_frame(source): - tracker = SquareTagPoseTracker() - old, stamp = _confirm(source, tracker) - old_revision = tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] - source.candidates = [_pose(0, .20), _pose(60, .05)] - for i in range(1, 5): - pose, reason = source.sample(tracker, stamp + i * STEP_NS) - assert pose is None and reason - pose, reason = source.sample(tracker, stamp + 5 * STEP_NS) - assert pose is None and reason == "pose_branch_corrected" - assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == old_revision + 1 - assert tracker.branch_correction_counts["moving"] == 1 - pose, reason = source.sample(tracker, stamp + 6 * STEP_NS) - assert pose is not None and reason == "" - assert _distance(old.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(60) - assert tracker.freeze_roles({"moving": old_revision}) is False - assert tracker.freeze_roles({"moving": old_revision + 1}) is True - - -def test_freezing_multiple_roles_is_atomic_and_requires_confirmed_revisions(source): - tracker = SquareTagPoseTracker() - _confirm(source, tracker, role="first") - _confirm(source, tracker, role="second") - revisions = {role: tracker.last_candidate_diagnostics_by_role[role]["branch_revision"] - for role in ("first", "second")} - assert tracker.freeze_roles({**revisions, "second": revisions["second"] + 1}) is False - for role in revisions: - source.sample(tracker, START_NS + 5 * STEP_NS, role=role) - assert tracker.last_candidate_diagnostics_by_role[role]["branch_frozen"] is False - assert tracker.freeze_roles(revisions) is True - - -def test_short_conflict_discards_images_then_original_evidence_can_resume(source): - tracker = SquareTagPoseTracker() - old, stamp = _confirm(source, tracker) - revision = _freeze(tracker) - source.candidates = [_pose(0, .20), _pose(60, .05)] - for i in (1, 2): - pose, reason = source.sample(tracker, stamp + i * STEP_NS) - assert pose is None and reason - source.candidates = [_pose(.2, .05), _pose(60, .20)] - pose, reason = source.sample(tracker, stamp + 3 * STEP_NS) - assert pose is not None and reason == "" - assert _distance(old.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(.2) - assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision - assert tracker.branch_correction_counts.get("moving", 0) == 0 - - -def test_frozen_tracker_keeps_original_continuity_inside_the_unchanged_point03_tie(source): - tracker = SquareTagPoseTracker() - original, stamp = _confirm(source, tracker) - revision = _freeze(tracker) - source.candidates = [_pose(.2, .079), _pose(60, .05)] - pose, reason = source.sample(tracker, stamp + STEP_NS) - assert pose is not None and reason == "" - assert _distance(original.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(.2) - assert tracker.reprojection_tie_px == .03 - assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision - - -def test_observation_snapshot_matches_only_its_own_image(source): - tracker = SquareTagPoseTracker() - _, stamp = _confirm(source, tracker) - candidates, detail, corrections = tracker.observation_snapshot("moving", stamp) - assert len(candidates) == 2 - assert detail["observation_stamp_ns"] == stamp - assert corrections == 0 - source.sample(tracker, stamp + STEP_NS) - assert tracker.observation_snapshot("moving", stamp) is None - assert tracker.observation_snapshot("unknown", stamp + STEP_NS) is None - assert tracker.observation_snapshot("moving", stamp + STEP_NS) is not None - - -def test_persistent_ambiguity_keeps_the_frozen_path_and_can_recover_after_five_seconds(source): - tracker = SquareTagPoseTracker() - _, stamp = _confirm(source, tracker) - revision = _freeze(tracker) - source.candidates = [_pose(0, .20), _pose(60, .05)] - for i in range(1, 190): - pose, reason = source.sample(tracker, stamp + i * STEP_NS) - assert pose is None and reason == "pose_branch_ambiguous" - assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision - source.candidates = [_pose(0, .05), _pose(60, .20)] - pose, reason = source.sample(tracker, stamp + 190 * STEP_NS) - assert pose is not None and reason == "" - assert not tracker._branches.states['moving'].conflict - assert tracker.branch_correction_counts.get('moving', 0) == 0 - - -@pytest.mark.parametrize("ambiguous_frames", [0, 12]) -def test_frozen_tracker_still_stops_when_no_candidate_continues_the_original_path(source, ambiguous_frames): - tracker = SquareTagPoseTracker() - _, stamp = _confirm(source, tracker) - revision = _freeze(tracker) - source.candidates = [_pose(0, .20), _pose(60, .05)] - for index in range(1, ambiguous_frames + 1): - assert source.sample(tracker, stamp + index*STEP_NS) == (None, "pose_branch_ambiguous") - stamp += ambiguous_frames * STEP_NS - source.candidates = [_pose(60, .05), _pose(70, .2)] - for index in range(1, 7): - pose, reason = source.sample(tracker, stamp + index*STEP_NS) - assert pose is None - assert reason == ('pose_branch_unconfirmed' if index < 5 else 'pose_branch_conflict') - source.candidates = [_pose(0, .05), _pose(60, .2)] - assert source.sample(tracker, stamp + 7*STEP_NS) == (None, 'pose_branch_conflict') - assert tracker._branches.states['moving'].revision == revision - - -def test_unfrozen_actual_gap_allows_new_confirmation(source): - tracker = SquareTagPoseTracker() - _, stamp = _confirm(source, tracker) - source.candidates = [_pose(0, .20), _pose(60, .05)] - pose, _ = _confirm(source, tracker, start=stamp + 6_000_000_000) - assert _distance(pose.quaternion_xyzw, _pose(60, .05).quaternion_xyzw) < 1e-9 - - -def test_frozen_actual_gap_requires_original_pose_before_reconfirmation(source): - tracker = SquareTagPoseTracker() - old, stamp = _confirm(source, tracker) - revision = _freeze(tracker) - source.candidates = [_pose(2, .05), _pose(60, .20)] - start = stamp + 6_000_000_000 - for i in range(6): - pose, reason = source.sample(tracker, start + i * STEP_NS) - assert pose is None and reason == "pose_branch_recovery_required" - source.candidates = [_pose(.5, .05), _pose(60, .20)] - pose, _ = _confirm(source, tracker, start=start + 6 * STEP_NS) - assert _distance(old.quaternion_xyzw, pose.quaternion_xyzw) == pytest.approx(.5) - assert tracker.last_candidate_diagnostics_by_role["moving"]["branch_revision"] == revision - - -def _real_sample(tracker, role, frame): - return tracker.estimate(role, frame["corners_xy"], tag_size_m=REAL["tag_size_m"], - camera_matrix=REAL["camera_matrix"], stamp_ns=frame["stamp_ns"]) - - -@pytest.mark.parametrize("sequence", REAL["zero_sequences"], ids=lambda s: f"real_ID{s['tag_id']}") -def test_real_stationary_zero_frames_confirm_consistent_evidence_without_ground_truth(sequence): - tracker = SquareTagPoseTracker() - role = sequence["role"] - output = [_real_sample(tracker, role, frame) for frame in sequence["frames"]] - assert all(pose is None and reason for pose, reason in output[:4]) - assert all(pose is not None and reason == "" for pose, reason in output[4:]) - _freeze(tracker, role) - # Independent IPPE replay shows the recorded preference exceeds the existing - # 0.03 px tie tolerance; this is explicitly not a true-pose assertion. - for frame in sequence["frames"]: - candidates = solve_square_tag_ippe(frame["corners_xy"], - tag_size_m=REAL["tag_size_m"], camera_matrix=REAL["camera_matrix"]) - assert len(candidates) == 2 - errors = sorted(p.reprojection_error_px for p in candidates) - assert errors[1] - errors[0] > .03 - - -@pytest.mark.parametrize("event", REAL["jump_events"], - ids=lambda e: f"real_ID{e['tag_id']}_{e['event_stamp_ns']}") -def test_real_six_discontinuities_never_enter_a_frozen_observation_stream(event): - tracker = SquareTagPoseTracker() - role = event["role"] - warmup = event["warmup_frames"] - assert len({frame["stamp_ns"] for frame in warmup}) >= 5 - assert warmup[-1]["stamp_ns"] - warmup[0]["stamp_ns"] >= 100_000_000 - outputs = [_real_sample(tracker, role, frame) for frame in warmup] - previous, reason = outputs[-1] - assert previous is not None and reason == "" - revision = _freeze(tracker, role) - for frame in event["frames"]: - selected, reason = _real_sample(tracker, role, frame) - if selected is None: - assert reason - continue - assert reason == "" - assert _distance(previous.quaternion_xyzw, selected.quaternion_xyzw) <= 35 - previous = selected - detail = tracker.last_candidate_diagnostics_by_role[role] - assert detail["branch_revision"] == revision - assert detail["branch_frozen"] is True - assert tracker.branch_correction_counts.get(role, 0) == 0 - # The source really contains the claimed discontinuity; the regression - # cannot become vacuous by replacing the fixture with benign corners. - frames = event["frames"] - index = next(i for i, f in enumerate(frames) if f["stamp_ns"] == event["event_stamp_ns"]) - jump = _distance(frames[index - 1]["recorded_selected_quaternion_xyzw"], - frames[index]["recorded_selected_quaternion_xyzw"]) - assert jump == pytest.approx(event["recorded_jump_deg"], abs=1e-9) - assert jump > 35 diff --git a/src/linkerhand_calibration/test/test_pnp_metadata_concurrency.py b/src/linkerhand_calibration/test/test_pnp_metadata_concurrency.py deleted file mode 100644 index dec45d9..0000000 --- a/src/linkerhand_calibration/test/test_pnp_metadata_concurrency.py +++ /dev/null @@ -1,93 +0,0 @@ -"""PnP computation must not block freezing or commit across reset/image versions.""" - -from concurrent.futures import ThreadPoolExecutor -from threading import Event - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.tag_pose import tracking -from linkerhand_calibration.core.geometry.pnp import SquareTagPose, SquareTagPoseTracker - - -def pose(angle, error): - return SquareTagPose(tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), - (0., 0., .5), error) - - -def sample(tracker, stamp): - return tracker.estimate("tag", np.zeros((4, 2)), tag_size_m=.016, - camera_matrix=np.eye(3), stamp_ns=stamp) - - -def initialized(monkeypatch): - tracker = SquareTagPoseTracker() - monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **k: [pose(0, .05), pose(60, .2)]) - for i in range(5): - selected, _ = sample(tracker, 1_000_000_000 + i * 30_000_000) - assert selected is not None - return tracker - - -def test_freeze_during_inflight_pnp_applies_before_branch_selection(monkeypatch): - tracker = initialized(monkeypatch) - entered, release = Event(), Event() - - def solve(*args, **kwargs): - entered.set() - assert release.wait(5) - return [pose(0, .2), pose(60, .05)] - - monkeypatch.setattr(tracking, "solve_square_tag_ippe", solve) - with ThreadPoolExecutor(max_workers=2) as pool: - pending = pool.submit(sample, tracker, 1_150_000_000) - try: - assert entered.wait(5) - # This completes while the expensive solver is still blocked. - assert pool.submit(tracker.freeze_roles, {"tag": 1}).result(timeout=2) - finally: - release.set() - selected, reason = pending.result(timeout=2) - assert selected is None and reason == "pose_branch_ambiguous" - for i in range(1, 5): - selected, reason = sample(tracker, 1_150_000_000 + i * 30_000_000) - assert selected is None and reason == "pose_branch_ambiguous" - assert tracker.last_candidate_diagnostics_by_role["tag"]["branch_revision"] == 1 - - -@pytest.mark.parametrize("operation", ["reset", "newer_image"]) -def test_late_pnp_cannot_overwrite_newer_metadata_or_repopulate_reset(monkeypatch, operation): - tracker = initialized(monkeypatch) - entered, release = Event(), Event() - calls = 0 - - def solve(*args, **kwargs): - nonlocal calls - calls += 1 - if calls == 1: - entered.set() - assert release.wait(5) - return [pose(60, .05), pose(0, .2)] - return [pose(.1, .05), pose(60, .2)] - - monkeypatch.setattr(tracking, "solve_square_tag_ippe", solve) - with ThreadPoolExecutor(max_workers=1) as pool: - pending = pool.submit(sample, tracker, 1_150_000_000) - try: - assert entered.wait(5) - if operation == "reset": - tracker.reset() - else: - assert sample(tracker, 1_180_000_000)[0] is not None - finally: - release.set() - selected, reason = pending.result(timeout=2) - assert selected is None - if operation == "reset": - assert reason == "pose_observation_superseded" - assert not tracker.last_candidate_diagnostics_by_role - else: - assert reason == "pose_observation_not_new" - assert tracker.observation_snapshot("tag", 1_150_000_000) is None - assert tracker.observation_snapshot("tag", 1_180_000_000)[1]["branch_status"] == "tracking" diff --git a/src/linkerhand_calibration/test/test_pnp_parameter_wiring.py b/src/linkerhand_calibration/test/test_pnp_parameter_wiring.py deleted file mode 100644 index 4459931..0000000 --- a/src/linkerhand_calibration/test/test_pnp_parameter_wiring.py +++ /dev/null @@ -1,50 +0,0 @@ -"""ROS and offline capture share typed tracking defaults and parameter units.""" - -from dataclasses import asdict -import math -from types import SimpleNamespace - -import pytest - -from linkerhand_calibration.core.geometry.pnp import DEFAULT_POSE_TRACKING_PARAMETERS -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime import parameters as runtime_parameters -from linkerhand_calibration.runtime.ros.parameters import parameter_defaults, tracking_parameters - - -@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) -def test_every_profile_uses_same_shared_confirmation_defaults(layout): - values = parameter_defaults(load_bundled_hand_profile(layout)) - resolved = tracking_parameters(values.__getitem__) - assert resolved == DEFAULT_POSE_TRACKING_PARAMETERS - assert values["pnp_branch_confirmation_frames"] == 5 - assert values["pnp_branch_confirmation_minimum_seconds"] == .1 - assert values["pnp_branch_equivalence_deg"] == 1. - assert resolved.reprojection_tie_px == .03 - assert resolved.maximum_pose_jump_rad == math.radians(35) - assert resolved.maximum_tag_tilt_rad == math.radians(75) - assert resolved.maximum_reprojection_error_px == 1.5 - - -def test_ros_converts_branch_angle_and_preserves_custom_confirmation_values(): - values = parameter_defaults(load_bundled_hand_profile("o6_right_8")) - values.update(pnp_branch_confirmation_frames=7, - pnp_branch_confirmation_minimum_seconds=.25, pnp_branch_equivalence_deg=.5) - resolved = tracking_parameters(values.__getitem__) - assert resolved.branch_confirmation_frames == 7 - assert resolved.branch_confirmation_minimum_seconds == .25 - assert resolved.branch_equivalence_rad == math.radians(.5) - - -def test_runtime_passes_all_tracking_fields_to_each_independent_view(monkeypatch): - values = parameter_defaults(load_bundled_hand_profile("o6_right_8")) - values.update(pnp_branch_confirmation_frames=8, - pnp_branch_confirmation_minimum_seconds=.2, pnp_branch_equivalence_deg=.8) - tracking = tracking_parameters(values.__getitem__) - monkeypatch.setattr(runtime_parameters, "SquareTagPoseTracker", - lambda **kwargs: SimpleNamespace(parameters=kwargs)) - trackers = runtime_parameters.RuntimeParameters.new_trackers( - SimpleNamespace(tracking=tracking), ("front", "side", "top")) - assert set(trackers) == {"front", "side", "top"} - assert len({id(value) for value in trackers.values()}) == 3 - assert all(value.parameters == asdict(tracking) for value in trackers.values()) diff --git a/src/linkerhand_calibration/test/test_pnp_runtime_policy.py b/src/linkerhand_calibration/test/test_pnp_runtime_policy.py deleted file mode 100644 index d065bee..0000000 --- a/src/linkerhand_calibration/test/test_pnp_runtime_policy.py +++ /dev/null @@ -1,76 +0,0 @@ -"""Exercise branch selection with shipped parameters and capture defaults.""" - -from pathlib import Path -from dataclasses import asdict - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation -import yaml - -from linkerhand_calibration.core.geometry import pnp -from linkerhand_calibration.core.geometry.tag_pose import tracking -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.capture import ObservationCapture -from linkerhand_calibration.runtime.ros.parameters import parameter_defaults, tracking_parameters - - -PACKAGE = Path(__file__).resolve().parents[1] -LAYOUTS = ("g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16") - - -def _pose(angle, error): - return pnp.SquareTagPose( - tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), - (0.0, 0.0, 0.25), error, - ) - - -def _check_branch_selection(tracker, monkeypatch): - stale = _pose(55, 0.25) - accurate = _pose(2, 0.05) - continued = _pose(3, 0.11) - frames = iter([ - *[(stale,)] * 5, - *[(_pose(54, 0.24), accurate)] * 5, - # A real near-tie still favors continuous motion over a mirror flip. - (_pose(55, 0.10), continued), - ]) - monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **kw: next(frames)) - for index in range(11): - selected, reason = tracker.estimate( - "moving", np.zeros((4, 2)), tag_size_m=0.016, - camera_matrix=np.eye(3), stamp_ns=1_000_000_000 + index * 30_000_000, - ) - if index in (4, 10): - assert reason == "" - assert selected == (stale if index == 4 else continued) - else: - assert selected is None - assert reason == ("pose_branch_corrected" if index == 9 else "pose_branch_unconfirmed") - assert tracker.branch_correction_counts == {"moving": 1} - - -@pytest.mark.parametrize("layout,filename", [ - ("g20_right_19", "three_camera_calibration.yaml"), - ("l6_right_8", "l6_three_camera_calibration.yaml"), - ("o6_right_8", "o6_three_camera_calibration.yaml"), - ("o12_right_16", "o12_three_camera_calibration.yaml"), - ("g20_right_19", "calibration.yaml"), -]) -def test_shipped_ros_parameters_release_stale_branch(layout, filename, monkeypatch): - profile = load_bundled_hand_profile(layout) - config = yaml.safe_load((PACKAGE / "config" / filename).read_text()) - params = dict(next(iter(config.values()))["ros__parameters"]) - params = {**parameter_defaults(profile), **params} - for _view in profile.vision.view_names: - tracker = pnp.SquareTagPoseTracker(**asdict(tracking_parameters(params.__getitem__))) - _check_branch_selection(tracker, monkeypatch) - - -@pytest.mark.parametrize("layout", LAYOUTS) -def test_capture_defaults_release_stale_branch(layout, monkeypatch): - capture = ObservationCapture(load_bundled_hand_profile(layout), - reference_lock=None, extrinsics=None) - for tracker in capture.trackers.values(): - _check_branch_selection(tracker, monkeypatch) diff --git a/src/linkerhand_calibration/test/test_pose_capture_evidence.py b/src/linkerhand_calibration/test/test_pose_capture_evidence.py deleted file mode 100644 index ae965d3..0000000 --- a/src/linkerhand_calibration/test/test_pose_capture_evidence.py +++ /dev/null @@ -1,125 +0,0 @@ -"""Capture diagnostics preserve actual image evidence without changing poses.""" - -from dataclasses import replace -import json - -import numpy as np -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.domain.reference import build_joint_zero_reference -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.capture import CaptureTiming - -from test_observation_capture import frame, make_capture - - -def _lock(profile, lock, capture): - lock.begin_session() - lock.start_locking() - for stamp in range(1, 15): - capture.consume(frame(profile, stamp)) - - -def _sweep(profile): - return MotionCommand("sweep", tuple(profile.command.baseline_values), 1, - "thumb_pitch_dip_front", 0, 0, "decreasing") - - -def test_evidence_contains_this_images_corners_sizes_candidates_and_selected_pose(): - profile, _, capture = make_capture() - for stamp in range(1, 6): - capture.consume(frame(profile, stamp)) - observation = frame(profile, 20) - rows, movement = capture.consume(observation, _sweep(profile)) - assert movement is None and len(rows) == 2 - assert {row["kind"] for row in rows} == {"joint_sample"} - row = next(row for row in rows if row["joint"] == "rh_thumb_cmc_pitch") - evidence = row["pnp_observation_evidence"] - sizes = {tag.role: tag.size_m for tag in profile.vision.views[0].tags} - for item in evidence.values(): - role = item["tag_role"] - assert item["pose_source"] == "current_image" - assert item["corners_xy"] == observation.corners[role].tolist() - assert item["tag_size_m"] == sizes[role] - candidates = capture.trackers["front"].last_candidates_by_role[role] - assert len(item["reprojection_valid_candidates"]) == len(candidates) > 0 - for recorded, actual in zip(item["reprojection_valid_candidates"], candidates): - assert recorded["quaternion_xyzw"] == list(actual.quaternion_xyzw) - assert recorded["translation_xyz_m"] == list(actual.translation_xyz_m) - assert recorded["reprojection_error_px"] == actual.reprojection_error_px - assert item["candidate_diagnostics"] == capture.trackers["front"].last_candidate_diagnostics_by_role[role] - # The image fixture places both Tags at the same orientation, with a - # known translation. Adding diagnostics must retain the actual solve. - parent = evidence["parent"]["selected_pose"] - child = evidence["child"]["selected_pose"] - rp = Rotation.from_quat(parent["quaternion_xyzw"]) - rc = Rotation.from_quat(child["quaternion_xyzw"]) - np.testing.assert_allclose((rp.inv()*rc).as_quat(), row["relative_quaternion_xyzw"], atol=1e-12) - np.testing.assert_allclose(parent["translation_xyz_m"], [0, 0, .45], atol=1e-8) - np.testing.assert_allclose(child["translation_xyz_m"], [.05, .03, .45], atol=1e-8) - frozen = json.dumps(evidence, sort_keys=True) - observation.corners["thumb_pitch"][:] = 0 - capture.trackers["front"].last_candidate_diagnostics_by_role["thumb_pitch"]["solved_candidate_count"] = 999 - capture.consume(frame(profile, 21, shift=1), _sweep(profile)) - assert json.dumps(evidence, sort_keys=True) == frozen - - -def test_zero_reference_ignores_additional_diagnostics_and_retains_original_pose(): - profile, lock, capture = make_capture() - _lock(profile, lock, capture) - name = "rh_thumb_cmc_pitch" - spec = profile.motion.joint_zero_references[name] - motion = MotionCommand("joint_zero", spec.command, 1, task_key=spec.task_key, - command_index=0, cycle=0, zero_joints=(name,)) - rows = [] - for stamp in range(20, 30): - samples, movement = capture.consume(frame(profile, stamp, omit=("thumb_dip",)), motion) - assert movement is None - rows.extend(samples) - assert len(rows) == 10 and all(row["kind"] == "joint_zero_sample" for row in rows) - assert all(row["pnp_observation_evidence"]["child"]["tag_role"] == "thumb_pitch" for row in rows) - legacy = [{key: value for key, value in row.items() if key != "pnp_observation_evidence"} for row in rows] - assert build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=2) == ( - build_joint_zero_reference(profile, name, legacy, session_epoch=1, motion_version=2)) - - -def test_missing_fixed_tag_uses_frozen_pose_but_never_copies_earlier_image_candidates(): - profile, lock, capture = make_capture() - _lock(profile, lock, capture) - assert capture.trackers["front"].last_candidates_by_role["front_base"] - rows, movement = capture.consume(frame(profile, 20, omit=("front_base",)), _sweep(profile)) - assert movement is None and len(rows) == 2 - row = next(row for row in rows if row["joint"] == "rh_thumb_cmc_pitch") - parent = row["pnp_observation_evidence"]["parent"] - assert parent["pose_source"] == "cached_fixed_reference" - assert parent["corners_xy"] is None - assert parent["reprojection_valid_candidates"] == [] - assert parent["candidate_diagnostics"] == {} - assert parent["selected_pose"]["quaternion_xyzw"] == list(capture.locked_poses["front"].quaternion_xyzw) - assert "image_relative_xy_px" not in row - assert row["pnp_observation_evidence"]["child"]["pose_source"] == "current_image" - - -def test_rejected_current_fixed_tag_corners_are_distinct_from_cached_pose(): - profile, lock, capture = make_capture() - _lock(profile, lock, capture) - observation = frame(profile, 20) - observation = replace(observation, corners={**observation.corners, "front_base": np.zeros((4, 2))}) - rows, movement = capture.consume(observation, _sweep(profile)) - assert movement is None - assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) - - -def test_unmatched_diagnostic_frame_preserves_timing_without_claiming_zero_skew(): - profile, lock, capture = make_capture() - _lock(profile, lock, capture) - observation = replace(frame(profile, 20), feedback=None, - timing=CaptureTiming(30, 35, 10, 18)) - rows, movement = capture.consume(observation, _sweep(profile), diagnostic=True) - assert movement is None - row = next(row for row in rows if row["kind"] == "diagnostic_observation_frame") - assert row["feedback_vector"] is None - assert row["state_image_sync_error_ms"] is None - assert row["timing"] == {"received_stamp_ns": 30, "history_snapshot_stamp_ns": 35, - "latest_feedback_stamp_ns": 10, "latest_command_stamp_ns": 18} - assert not any(row.get("kind") == "joint_sample" for row in rows) diff --git a/src/linkerhand_calibration/test/test_pose_tracking_resume_policy.py b/src/linkerhand_calibration/test/test_pose_tracking_resume_policy.py deleted file mode 100644 index 7ae59e3..0000000 --- a/src/linkerhand_calibration/test/test_pose_tracking_resume_policy.py +++ /dev/null @@ -1,112 +0,0 @@ -"""Older pose and capture schedules remain readable without authorizing reuse.""" - -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.acquisition import load_capture -from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine -from linkerhand_calibration.runtime.joint_resume import JointResume -from linkerhand_calibration.runtime.resume import discover_resume_candidate - - -def header(profile, policy, *, schedule="current"): - result = {"kind": "session_start", "profile_id": profile.key.profile_id, - "serial_number": "TEST", "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "protected_hashes": {"source": "a" * 64}} - if policy is not None: - result["pose_tracking_policy_version"] = policy - if schedule is not None: - result["capture_schedule_version"] = ( - CalibrationEngine(profile).capture_schedule_version if schedule == "current" else schedule) - return result - - -def journal(directory, profile, policy, *, schedule="current"): - directory.mkdir() - rows = [header(profile, policy, schedule=schedule), {"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 rows), encoding="utf-8") - return path, rows - - -@pytest.mark.parametrize("policy", [None, "", "legacy_single_frame", "confirmed_branch_v0", "confirmed_branch_v1", - "confirmed_branch_v5_image_motion"]) -def test_discovery_skips_old_pose_policy_but_preserves_offline_reading(tmp_path, policy): - profile = load_bundled_hand_profile("o6_right_8") - current = tmp_path / "01_current" - journal(current, profile, POSE_TRACKING_POLICY_VERSION) - old, rows = journal(tmp_path / "02_old", profile, policy) - before = old.read_bytes() - found = discover_resume_candidate(tmp_path, profile_id=profile.key.profile_id, - serial_number="TEST", protected_hashes={"source": "a" * 64}) - assert found == current - assert load_capture(old) == rows - assert old.read_bytes() == before - - -@pytest.mark.parametrize("schedule", [None, "unified_schedule_v1_sweep_then_steady"]) -def test_old_capture_schedule_is_readable_but_cannot_be_discovered_or_staged(tmp_path, monkeypatch, schedule): - from linkerhand_calibration.runtime import joint_resume - profile = load_bundled_hand_profile("o6_right_8") - current = tmp_path / "01_current" - journal(current, profile, POSE_TRACKING_POLICY_VERSION) - old, rows = journal(tmp_path / "02_old", profile, POSE_TRACKING_POLICY_VERSION, - schedule=schedule) - before = old.read_bytes() - engine = CalibrationEngine(profile) - assert not engine.resume_compatible(rows[0]) - assert discover_resume_candidate(tmp_path, profile_id=profile.key.profile_id, - serial_number="TEST", protected_hashes={"source": "a" * 64}) == current - monkeypatch.setattr(joint_resume, "read_joint_zero_references", - lambda *args: pytest.fail("old schedule must be rejected before importing zeros")) - resume = JointResume(profile) - with pytest.raises(ValueError, match="joint_resume_checkpoint_policy_incompatible"): - resume.stage(engine, rows, set()) - assert resume.references == {} and resume.rows == () - assert load_capture(old) == rows - assert old.read_bytes() == before - - -@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) -@pytest.mark.parametrize("policy", [None, "old_unconfirmed_branch", "confirmed_branch_v5_image_motion", - POSE_TRACKING_POLICY_VERSION]) -def test_explicit_engine_resume_requires_current_pose_policy(layout, policy): - profile = load_bundled_hand_profile(layout) - assert CalibrationEngine(profile).resume_compatible(header(profile, policy)) == ( - policy == POSE_TRACKING_POLICY_VERSION) - - -@pytest.mark.parametrize("policy", [None, "old_unconfirmed_branch", "confirmed_branch_v5_image_motion"]) -def test_joint_staging_rejects_old_policy_before_reading_references(monkeypatch, policy): - from linkerhand_calibration.runtime import joint_resume - profile = load_bundled_hand_profile("o6_right_8") - monkeypatch.setattr(joint_resume, "read_joint_zero_references", - lambda *args: pytest.fail("read old zero references")) - resume = JointResume(profile) - with pytest.raises(ValueError, match="joint_resume_checkpoint_policy_incompatible"): - resume.stage(CalibrationEngine(profile), [header(profile, policy)], set()) - assert resume.references == {} - assert resume.rows == () - - -def test_joint_staging_accepts_current_policy_without_changing_it(monkeypatch): - from linkerhand_calibration.runtime import joint_resume - from linkerhand_calibration.runtime import motion_provenance - profile = load_bundled_hand_profile("o6_right_8") - rows = [header(profile, POSE_TRACKING_POLICY_VERSION)] - monkeypatch.setattr(joint_resume, "read_joint_zero_references", lambda *args: {}) - # This isolated test exercises policy dispatch only. Real reference and - # provenance acceptance is covered with complete evidence in dedicated tests. - validated = [] - monkeypatch.setattr(motion_provenance, "validate_motion_provenance", - lambda *args, **kwargs: validated.append((args, kwargs))) - resume = JointResume(profile) - resume.stage(CalibrationEngine(profile), rows, set()) - assert resume.rows == tuple(rows) - assert rows[0]["pose_tracking_policy_version"] == POSE_TRACKING_POLICY_VERSION - assert rows[0]["capture_schedule_version"] == CalibrationEngine(profile).capture_schedule_version - assert len(validated) == 1 and validated[0][1]["references"] == {} diff --git a/src/linkerhand_calibration/test/test_preparation_admission.py b/src/linkerhand_calibration/test/test_preparation_admission.py deleted file mode 100644 index 24a94ba..0000000 --- a/src/linkerhand_calibration/test/test_preparation_admission.py +++ /dev/null @@ -1,96 +0,0 @@ -"""Bad preparation images cannot replace valid evidence or authorize a model.""" - -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.branch_initialization import ( - BranchInitialization, initialization_artifacts, solve_initialization, -) -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 -from test_branch_initialization import arrival -from o30_capture_fixture import SOURCE - - -@pytest.mark.parametrize("defect", ["missing_candidates", "invalid_candidates", "unfrozen_root"]) -def test_bad_later_frame_cannot_replace_valid_motion_bin(defect): - profile = load_bundled_hand_profile("o6_right_8") - approach, zero, rows = arrival(profile) - image_geometry = defect == "unfrozen_root" - owner = BranchInitialization(profile, image_geometry=image_geometry) - owner.begin(approach, session_epoch=1, motion_version=7) - expected = BranchInitialization(profile, image_geometry=image_geometry) - expected.begin(approach, session_epoch=1, motion_version=7) - for row in rows: - owner.observe(row) - expected.observe(row) - bad = deepcopy(row) - bad["image_stamp_ns"] += 1 - for tag in bad["tags"].values(): - tag["candidate_diagnostics"]["observation_stamp_ns"] = bad["image_stamp_ns"] - root, child = bad["tags"].values() - if defect == "missing_candidates": - child["reprojection_valid_candidates"] = [] - elif defect == "invalid_candidates": - for candidate in child["reprojection_valid_candidates"]: - candidate["translation_xyz_m"] = [0., 0., -1.] - else: - root["candidate_diagnostics"]["branch_frozen"] = False - owner.observe(bad) - request = owner.request(rows[0]["view"], zero, session_epoch=1, motion_version=8) - original = expected.request(rows[0]["view"], zero, session_epoch=1, motion_version=8) - assert request.frames == original.frames - assert request.source_frame_hashes == original.source_frame_hashes - assert sum(dict(request.rejected_observations).values()) == len(rows) - # The admission gate keeps both alternatives, even if one looks worse. - assert all(len(frame.roles[1].candidates) == 2 for frame in request.frames) - if not image_geometry: - resolution = solve_initialization(request) - assert resolution.resolved - _, report = initialization_artifacts(request, resolution) - assert report["preparation_observation_rejections"] == dict(request.rejected_observations) - - -def test_dropping_all_invalid_images_never_produces_a_model(): - 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: - next(iter(row["tags"].values()))["selected_pose"]["translation_xyz_m"] = [0., 0., -1.] - owner.observe(row) - request = owner.request(rows[0]["view"], zero, session_epoch=1, motion_version=8) - assert not request.frames - assert not solve_initialization(request).resolved - - -def test_actual_ring_preparation_rejects_dropouts_without_claiming_geometry_passed(): - payload = json.loads(gzip.decompress((Path(__file__).parent / - "fixtures/o30_ring_preparation_admission.json.gz").read_bytes())) - profile = load_bundled_hand_profile("o30_right_18") - owner = BranchInitialization(profile, source_urdf=SOURCE) - reference = profile.motion.joint_zero_references["ring_mcp_pitch"] - motion = MotionCommand("zero_approach", reference.command, 200., - task_key="ring_mcp_pitch_side", reference_joints=("ring_mcp_pitch",)) - owner.begin(motion, session_epoch=1, motion_version=159) - for row in payload["records"]: - owner.observe(row) - zero = replace(motion, phase="joint_zero", zero_joints=motion.reference_joints) - request = owner.request("side", zero, session_epoch=1, motion_version=160) - assert payload["original_report"]["reason"] == "motion_candidates_missing_or_invalid" - assert len(payload["original_report"]["source_image_stamps"]) == 105 - assert len(request.frames) == 97 - assert dict(request.rejected_observations) == {"motion_candidates_missing_or_invalid": 14} - actual_hashes = {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in payload["records"]} - assert all(actual_hashes[stamp] == digest for stamp, digest in request.source_frame_hashes) - resolution = solve_initialization(request) - assert resolution.reason != "motion_candidates_missing_or_invalid" - # This historical arc also contains independent geometry/coverage defects. - # Removing broken detections must not relabel it as a successful capture. - assert not resolution.resolved and resolution.model is None diff --git a/src/linkerhand_calibration/test/test_preparation_contract.py b/src/linkerhand_calibration/test/test_preparation_contract.py deleted file mode 100644 index 700f3d1..0000000 --- a/src/linkerhand_calibration/test/test_preparation_contract.py +++ /dev/null @@ -1,157 +0,0 @@ -"""Preparation upgrades cannot silently reuse frozen upstream geometry.""" - -from copy import deepcopy -from dataclasses import replace -import json - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import STAGED, configure_training -from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_contract -from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine -from linkerhand_calibration.runtime.joint_resume import PreparedResume -from linkerhand_calibration.runtime.resume import discover_resume_candidate - - -def dense_profile(): - return configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), STAGED), - 'occlusion_aware_witness_v1') - - -def header(profile): - return dict(kind='session_start', serial_number='O30_TEST', profile_id=profile.key.profile_id, - acquisition_policy_version=ACQUISITION_POLICY_VERSION, - pose_tracking_policy_version=POSE_TRACKING_POLICY_VERSION, - capture_schedule_version=CalibrationEngine(profile).capture_schedule_version, - capture_plan={'policy': STAGED}, protected_hashes={'source': 'a'*64}, - preparation_contract=preparation_contract(profile), resume_checkpoint_requested=True) - - -def checkpoint(root, name, start, *, bad_tail=False): - path = root/name - path.mkdir() - rows = [start, dict(kind='fixed_base_reference_locked'), - dict(kind='scan_unit_complete', task_name='thumb_cmc_roll_front', - cycle=0, direction='increasing', passed=True)] - (path/'raw_samples.jsonl').write_text('\n'.join(json.dumps(row) for row in rows) - + ('\nnot-json' if bad_tail else '\n')) - return path - - -def discover(root, profile, rejected): - return discover_resume_candidate(root, profile_id=profile.key.profile_id, serial_number='O30_TEST', - protected_hashes={'source': 'a'*64}, training_policy=STAGED, - preparation_contract=preparation_contract(profile), rejected_candidates=rejected) - - -def test_old_budget_rejected_at_header_before_loading_large_history(tmp_path): - profile = dense_profile() - old = header(profile) - old.pop('preparation_contract') - checkpoint(tmp_path, '001', old, bad_tail=True) - rejected = [] - assert discover(tmp_path, profile, rejected) is None - assert rejected == [dict(session='001', reason='checkpoint preparation contract changed')] - assert not CalibrationEngine(profile).resume_compatible(old) - - -def test_matching_contract_resumes_and_json_roundtrip_is_identical(tmp_path): - profile = dense_profile() - start = json.loads(json.dumps(header(profile))) - source = checkpoint(tmp_path, '001', start) - assert CalibrationEngine(profile).resume_compatible(start) - assert discover(tmp_path, profile, []) == source - # Capture validity is separate: readable historical artifacts are not - # relabelled corrupt simply because a new live plan uses a different budget. - old = {key: value for key, value in start.items() if key != 'preparation_contract'} - assert CalibrationEngine(profile).capture_compatible(old) - - -@pytest.mark.parametrize('field', ['budget', 'witness', 'parent', 'version']) -def test_changed_dependency_graph_invalidates_whole_preparation_before_motion(tmp_path, field): - profile = dense_profile() - changed = header(profile) - contract = changed['preparation_contract'] - if field == 'budget': - contract['tasks']['pinky_pip_side']['command_intervals'] = 128 - elif field == 'witness': - contract['witnesses'].pop('pinky_mcp_pitch') - elif field == 'parent': - contract['tasks']['pinky_dip_side']['parent_reference']['geometry_joint'] = 'ring_pip' - else: - contract['version'] = 'future_strategy' - checkpoint(tmp_path, '001', changed) - assert discover(tmp_path, profile, []) is None - assert not CalibrationEngine(profile).resume_compatible(changed) - with pytest.raises(ValueError, match='identity/policy changed'): - PreparedResume.load(profile, CalibrationEngine(profile), tmp_path/'001/raw_samples.jsonl', 'O30_TEST') - - -def test_incompatible_fresh_run_cannot_resurrect_older_lineage(tmp_path): - profile = dense_profile() - checkpoint(tmp_path, '001', header(profile)) - fresh = header(profile) - fresh.pop('preparation_contract') - fresh['resume_checkpoint_requested'] = False - checkpoint(tmp_path, '002', fresh) - assert discover(tmp_path, profile, []) is None - - -def test_resume_upgrade_does_not_rewrite_historical_capture(tmp_path): - profile = dense_profile() - old = header(profile) - old.pop('preparation_contract') - path = checkpoint(tmp_path, '001', old)/'raw_samples.jsonl' - before = path.read_bytes() - assert discover(tmp_path, profile, []) is None - assert path.read_bytes() == before - - -def test_capture_cycle_supplement_keeps_preparation_but_changed_pose_does_not(): - from linkerhand_calibration.core.domain.capture_plan import select_task_training - profile = dense_profile() - supplemented = select_task_training(profile, profile.motion.tasks[0].key, (0, 1, 2)) - assert preparation_contract(supplemented) == preparation_contract(profile) - zeros = dict(profile.motion.joint_zero_references) - spec = zeros['pinky_pip'] - command = list(spec.command) - command[0] -= 1 - zeros['pinky_pip'] = replace(spec, command=tuple(command)) - changed = replace(profile, motion=replace(profile.motion, joint_zero_references=zeros)) - assert preparation_contract(changed) != preparation_contract(profile) - - -def test_live_coordinator_writes_effective_preparation_graph(tmp_path): - from runtime_host_fixture import coordinator_fixture - host, _ = coordinator_fixture(tmp_path, model='o30', profile_transform=lambda _: dense_profile()) - try: - from linkerhand_calibration.storage import load_jsonl - row = load_jsonl(host.raw_path)[0] - assert row['preparation_contract'] == preparation_contract(host.profile) - assert CalibrationEngine(host.profile).resume_compatible(row) - finally: - host.close() - - -def test_legacy_profiles_keep_existing_resume_contract(): - profile = load_bundled_hand_profile('o30_right_18') - assert preparation_contract(profile) is None - - -def test_new_header_cannot_relabel_old_parent_models(): - from linkerhand_calibration.profiles.preparation import validate_preparation_contract - from linkerhand_calibration.runtime.motion_provenance import validate_motion_provenance - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import DENSE_PREPARATION_SAMPLING - profile = dense_profile() - old = dict(kind='motion_branch_initialization', task_name='pinky_pip_side', - zero_joints=['pinky_pip'], status='resolved') - with pytest.raises(ValueError, match='model_budget_changed'): - validate_motion_provenance(profile, [header(profile), old]) - dense = {**old, 'image_sampling_policy': DENSE_PREPARATION_SAMPLING} - validate_preparation_contract(profile, [header(profile), dense]) - changed = deepcopy(header(profile)) - changed['preparation_contract']['version'] = 'another_graph' - with pytest.raises(ValueError, match='profile_changed'): - validate_preparation_contract(profile, [changed, dense]) diff --git a/src/linkerhand_calibration/test/test_preparation_density.py b/src/linkerhand_calibration/test/test_preparation_density.py deleted file mode 100644 index 2cc14c9..0000000 --- a/src/linkerhand_calibration/test/test_preparation_density.py +++ /dev/null @@ -1,141 +0,0 @@ -"""Replay real PIP/DIP pixels with a budget fixed before either fit. - -This is an offline counterfactual, not a passed capture: the new parent model -is projected onto the original DIP images. Original journals and frozen zeros -are never overwritten, and no artifact publication is authorized here. -""" -from copy import deepcopy -from dataclasses import asdict, replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import configure_training -from linkerhand_calibration.core.domain.reference import JointZeroReference -from linkerhand_calibration.core.geometry.image_motion_replay import frozen_image_model -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( - ImageMotionParameters, resolve_image_motion, select_image_motion_frame, -) -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceParameters -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_image_budget -from linkerhand_calibration.runtime.branch_initialization import ( - BranchInitialization, solve_initialization, initialization_artifacts, -) -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.passed_resume import restore_parent_models -from linkerhand_calibration.runtime.shared_tag_reference import bridge_frames -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope='module') -def replay(): - data = json.loads(gzip.decompress((Path(__file__).parent / - 'fixtures/o30_pinky_preparation_density.json.gz').read_bytes())) - profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), - 'staged_2_to_3'), 'occlusion_aware_witness_v1') - owner = BranchInitialization(profile, source_urdf=SOURCE) - old = data['pip_report'] - reference = JointZeroReference.from_record(old['shared_tag_pose_bridges'][0]['source_reference']) - restore_parent_models(owner.parent_references, [data['source_report']], 1) - for row in data['source_window']: - owner.parent_references.handoff.observe(row) - def request(joint, report, rows, references=None): - spec = profile.motion.joint_zero_references[joint] - motion = MotionCommand('zero_approach', spec.command, 200., - task_key=report['task_name'], reference_joints=(joint,)) - owner.begin(motion, session_epoch=1, motion_version=report['motion_version']) - for row in rows: - owner.observe(row) - result = owner.request('side', replace(motion, phase='joint_zero', zero_joints=(joint,)), - session_epoch=1, motion_version=report['motion_version']+1, zero_references=references) - assert not result.invalid_reason - return result - pip_request = request('pinky_pip', old, data['pip_rows'], {reference.joint: reference}) - pip = solve_initialization(pip_request) - assert pip.resolved, pip.reason - _, pip_report = initialization_artifacts(pip_request, pip) - owner.accept(pip_request, pip) - # Geometry extraction requires current-pose admission. This offline marker - # only exercises the request builder, not the full pose/zero authorization. - owner.parent_references.verified[(1, 'pinky_dip_side')] = {'offline_counterfactual': True} - rows = deepcopy(data['dip_rows']) - for row in rows: - image = bridge_frames([row], pip.model.relations[0])[0] - selected = select_image_motion_frame(pip.model, image) - assert selected.resolved, selected.reason - tag = row['tags']['pinky_pip'] - tag['selected_pose'] = asdict(next(s.pose for s in selected.selections if s.role == 'pinky_pip')) - tag['frozen_image_model'] = pip_report['frozen_image_model'] - tag['image_model_sha256'] = pip_report['image_model_sha256'] - tag['candidate_diagnostics']['motion_evidence_id'] = pip_report['evidence_ids']['pinky_pip'] - dip_request = request('pinky_dip', data['dip_report'], rows) - dip = solve_initialization(dip_request) - return data, profile, pip_request, pip, dip_request, dip - - -@pytest.mark.replay -def test_real_parent_child_precision_with_disjoint_more_complete_evidence(replay): - data, _, pip_request, pip, dip_request, dip = replay - assert data['dip_report']['reason'] == 'image_motion_no_observable_image_model' - assert all(h['reason'] == 'image_motion_geometry_uncertain' for h in data['dip_report']['hypotheses']) - assert dip.resolved, dip.reason - for request, result in ((pip_request, pip), (dip_request, dip)): - assert 200 < len(request.frames) <= 257 - model = result.model - assert len(model.training_stamps_ns) > 100 - assert len(model.validation_stamps_ns) > 100 - assert not set(model.training_stamps_ns) & set(model.validation_stamps_ns) - assert set(model.training_stamps_ns) | set(model.validation_stamps_ns) == set(model.source_stamps_ns) - winner = result.hypotheses[result.selected_hypothesis] - assert all(v <= MotionEvidenceParameters().maximum_split_axis_difference_rad - for _, v in winner.axis_uncertainty_95_rad) - assert model.maximum_reprojection_error_px == 1.5 - _, report = initialization_artifacts(request, result) - assert frozen_image_model(report) == model - assert all(p < .01 for _, p in dip.hypotheses[dip.selected_hypothesis].comparison_adjusted_p_values) - - -@pytest.mark.replay -def test_dense_child_does_not_relabel_coarse_frozen_parent(replay): - data, _, _, _, request, _ = replay - from linkerhand_calibration.core.geometry.tag_pose.source_hinge import SourceHinge - old = data['pip_report'] - source = SourceHinge(frozen_image_model(old).geometry[0], old['evidence_ids']['pinky_pip'], - old['image_model_sha256'], *old['geometry_uncertainty'][0][1:]) - result = resolve_image_motion(request.image_frames, request.relations, - parameters=request.image_parameters, source_hinges=(source,), - geometry_constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256) - assert not result.resolved - assert result.reason == 'image_motion_families_not_distinguishable' - - -def test_preparation_budget_does_not_expand_multi_joint_graph_or_other_profiles(): - profile = configure_training(load_bundled_hand_profile('o30_right_18'), 'staged_2_to_3') - assert preparation_image_budget(profile) == (128, ImageMotionParameters()) - dense = configure_preparation(profile, 'occlusion_aware_witness_v1') - assert preparation_image_budget(dense)[0] == 256 - assert preparation_image_budget(dense, joint_count=4) == (128, ImageMotionParameters()) - with pytest.raises(ValueError, match='limits_invalid'): - ImageMotionParameters(maximum_training_frames=145) - - -@pytest.mark.replay -def test_sampling_budget_is_bound_and_cannot_be_removed_on_readback(replay): - from linkerhand_calibration.runtime.motion_provenance import _initializations, _check_image_initialization - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import DENSE_PREPARATION_SAMPLING - data, _, request, result, _, _ = replay - _, report = initialization_artifacts(request, result) - assert report['image_sampling_policy'] == DENSE_PREPARATION_SAMPLING - assert _initializations([*data['pip_rows'], report]) - changed = deepcopy(report) - changed.pop('image_sampling_policy') - with pytest.raises(ValueError, match='binding_changed'): - _initializations([*data['pip_rows'], changed]) - payload = deepcopy(next(iter(report['evidence_payloads'].values()))) - payload.pop('image_sampling_policy') - observations = {(r['view'], r['image_stamp_ns']): r for r in data['pip_rows']} - with pytest.raises(ValueError, match='support_or_roles_changed'): - _check_image_initialization(payload, observations) diff --git a/src/linkerhand_calibration/test/test_preparation_sync.py b/src/linkerhand_calibration/test/test_preparation_sync.py deleted file mode 100644 index 74597eb..0000000 --- a/src/linkerhand_calibration/test/test_preparation_sync.py +++ /dev/null @@ -1,176 +0,0 @@ -"""Real capture gaps must not become identity changes or bypass geometric gates.""" -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation -from linkerhand_calibration.core.domain.capture_plan import configure_training -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.preparation_witness import select_witness_rows, build_preparation_witness -from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256 -from linkerhand_calibration.runtime.preparation_witness_evidence import validate_preparation_witnesses -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope='module') -def real_capture(): - data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_pinky_witness_sync.json.gz').read_bytes())) - profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), - 'staged_2_to_3'), 'occlusion_aware_witness_v1') - owner = BranchInitialization(profile, source_urdf=SOURCE) - failure = data['failure'] - motion = MotionCommand('zero_approach', profile.motion.joint_zero_references['pinky_mcp_pitch'].command, - 200., task_key=failure['task_name'], reference_joints=('pinky_mcp_pitch',)) - owner.begin(motion, session_epoch=failure['session_epoch'], motion_version=failure['motion_version']) - for row in data['rows']: - if row.get('preparation_witness_joint'): - owner.witness_capture.observe(row) - elif row['motion_version'] == failure['motion_version']: - owner.observe(row) - request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('pinky_mcp_pitch',)), - session_epoch=failure['session_epoch'], motion_version=failure['motion_version']+1) - assert not request.invalid_reason - return profile, data, request - - -@pytest.fixture(scope='module') -def replayed(real_capture): - profile, data, request = real_capture - result = solve_initialization(request) - assert result.resolved, result.reason - _, report = initialization_artifacts(request, result) - return result, json.loads(json.dumps([*data['rows'], report])) - - -@pytest.mark.replay -def test_real_gap_preserves_all_pixels_and_independent_acceptance(real_capture, replayed): - profile, _, request = real_capture - source = request.witness_sources[0] - assert len(source.rows) == 219 - missing = [r for r in source.rows if r['command_vector'] is None] - assert len(missing) == 2 - result, records = replayed - decision = result.preparation_witnesses[0] - assert decision['decision'] == 'independent_before_fit' - assert decision['reason'] == 'insufficient_rotation_evidence' - assert set(decision['source_frame_hashes']) == {str(r['image_stamp_ns']) for r in source.rows} - assert decision['sample_admission']['rejected_frames'] == [ - {'image_stamp_ns': r['image_stamp_ns'], 'missing': ['command_vector']} for r in missing] - assert not set(r['image_stamp_ns'] for r in missing) & set(decision['selected_image_stamps']) - assert all(r['command_vector'] is None for r in missing) # No imputation or mutation. - assert result.hypotheses[result.selected_hypothesis].comparison_adjusted_p_values[0][1] < .01 - evidence = _initializations(records) - validate_preparation_witnesses(profile, records, evidence) - - -@pytest.mark.parametrize('fault', ['held_feedback_on_missing_command', 'held_command_on_missing_feedback', - 'epoch', 'camera', 'vector_shape', 'nonfinite', 'endpoint', 'long_gap', 'many_missing']) -def test_missing_samples_cannot_hide_invalid_evidence(real_capture, fault): - profile, _, request = real_capture - source = request.witness_sources[0] - rows = deepcopy(source.rows) - index = next(i for i, r in enumerate(rows) if r['command_vector'] is None) - if fault == 'held_feedback_on_missing_command': - rows[index]['feedback_vector'][10] += 8 - elif fault == 'held_command_on_missing_feedback': - rows[index]['command_vector'] = list(profile.motion.preparation_witnesses[source.joint].command) - rows[index]['command_vector'][10] += 1 - rows[index]['feedback_vector'] = None - elif fault == 'epoch': - rows[index]['session_epoch'] += 1 - elif fault == 'camera': - rows[index]['camera_matrix'][0][0] += 1 - elif fault == 'vector_shape': - rows[index]['feedback_vector'] = [1.] - elif fault == 'nonfinite': - rows[index]['feedback_vector'][10] = float('nan') - elif fault == 'endpoint': - rows[-1]['command_vector'] = None - elif fault == 'long_gap': - for row in rows[index+2:]: - row['image_stamp_ns'] += 300_000_000 - else: - for row in rows[20:40]: - row['command_vector'] = None - with pytest.raises(ValueError): - select_witness_rows(profile, replace(source, rows=tuple(rows))) - - -def test_preflight_declines_before_any_auxiliary_fit(real_capture, monkeypatch): - from linkerhand_calibration.runtime import preparation_witness as module - profile, _, request = real_capture - monkeypatch.setattr(module, 'resolve_image_motion', lambda *a, **k: pytest.fail('auxiliary fitting must not start')) - bridge, record = build_preparation_witness(profile, request.witness_sources[0]) - assert bridge is None and record['decision'] == 'independent_before_fit' - - -def test_incomplete_candidate_is_not_proof_of_insufficient_motion(monkeypatch): - from linkerhand_calibration.runtime.preparation_witness import _insufficient_rotation_before_fit - from linkerhand_calibration.core.geometry.tag_pose import motion_image_diagnostics as diagnostics - monkeypatch.setattr(diagnostics, '_validate_frames', lambda *a: ({'tag': ((None,),)}, ('tag',))) - assert not _insufficient_rotation_before_fit((), None) - - -def test_declined_auxiliary_does_not_overrule_ambiguous_main_images(real_capture, monkeypatch): - from linkerhand_calibration.runtime import branch_initialization as module - from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionResolution - _, _, request = real_capture - monkeypatch.setattr(module, 'resolve_image_motion', lambda *a, **k: - ImageMotionResolution(False, 'image_motion_families_not_distinguishable')) - result = solve_initialization(request) - assert not result.resolved and result.reason == 'image_motion_families_not_distinguishable' - - -def test_auxiliary_fit_failure_cannot_choose_another_acceptance_route(real_capture, monkeypatch): - from linkerhand_calibration.runtime import branch_initialization as main, preparation_witness as auxiliary - from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionResolution - _, _, request = real_capture - monkeypatch.setattr(auxiliary, '_insufficient_rotation_before_fit', lambda *a: False) - monkeypatch.setattr(auxiliary, 'resolve_image_motion', lambda *a, **k: - ImageMotionResolution(False, 'image_motion_families_not_distinguishable')) - monkeypatch.setattr(main, 'resolve_image_motion', lambda *a, **k: pytest.fail('no fallback after auxiliary fit')) - result = solve_initialization(request) - assert not result.resolved and result.reason.startswith('preparation_witness_unobservable:') - - -def test_excessive_sync_loss_has_its_own_category_without_motion_retry(): - from linkerhand_calibration.runtime.zero_recovery import failure_category, FailureCategory, permits_recovery - from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh - for reason in ('preparation_sync_coverage_missing', 'preparation_sync_endpoint_missing', - 'preparation_sync_gap_too_long', 'preparation_state_vector_invalid'): - assert failure_category(reason) == FailureCategory.EVIDENCE_GAP - assert not permits_recovery([reason], 0) - assert reason_zh({'reason': 'joint_zero_motion_unresolved:'+reason}, model_name='O30')[0] == 'OBS-PREP-SYNC-119' - - -@pytest.mark.replay -@pytest.mark.parametrize('fault', ['erase_missing', 'change_rejection', 'change_reason', 'remove_record']) -def test_raw_gaps_and_admission_decision_remain_auditable(real_capture, replayed, fault): - profile, _, _ = real_capture - _, original = replayed - records = deepcopy(original) - report = records[-1] - if fault == 'erase_missing': - stamp = report['preparation_witnesses'][0]['sample_admission']['rejected_frames'][0]['image_stamp_ns'] - records = [r for r in records if r.get('image_stamp_ns') != stamp] - else: - for container in [report, *report['evidence_payloads'].values()]: - if fault == 'remove_record': - container.pop('preparation_witnesses') - elif fault == 'change_rejection': - container['preparation_witnesses'][0]['sample_admission']['rejected_frames'] = [] - else: - container['preparation_witnesses'][0]['reason'] = 'ignore_bad_fit' - if fault == 'remove_record': - report['image_model_selection_policy'] = 'training_squared_loss_holm_v1' - for role, payload in report['evidence_payloads'].items(): - report['evidence_ids'][role] = source_frame_sha256(payload) - with pytest.raises(ValueError): - evidence = _initializations(records) - validate_preparation_witnesses(profile, records, evidence) diff --git a/src/linkerhand_calibration/test/test_preparation_transition.py b/src/linkerhand_calibration/test/test_preparation_transition.py deleted file mode 100644 index 67bcce5..0000000 --- a/src/linkerhand_calibration/test/test_preparation_transition.py +++ /dev/null @@ -1,216 +0,0 @@ -"""The real 2026-09-20 MCP pause, through runtime admission and journal audit.""" - -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.domain.reference import JointZeroReference -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256, validate_source_geometry -from linkerhand_calibration.runtime.passed_resume import restore_parent_models -from linkerhand_calibration.runtime.preparation_transition import build_preparation_transition -from linkerhand_calibration.runtime.preparation_transition_evidence import validate_preparation_transitions -from o30_capture_fixture import SOURCE - -pytestmark = pytest.mark.replay - - -@pytest.fixture(scope='module') -def recording(): - path = Path(__file__).parent/'fixtures/o30_thumb_preparation_transition.json.gz' - return json.loads(gzip.decompress(path.read_bytes())) - - -def request_from_recording(recording): - profile = load_bundled_hand_profile('o30_right_18') - owner = BranchInitialization(profile, source_urdf=SOURCE) - reference = JointZeroReference.from_record(next(r for r in recording['records'] if r['kind'] == 'joint_zero_reference')) - report = next(r for r in recording['records'] if r['kind'] == 'motion_branch_initialization') - restore_parent_models(owner.parent_references, [report], 1) - owner.parent_references.remember_zero(reference, 1) - spec = profile.motion.joint_zero_references['thumb_mcp'] - motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, reference_joints=('thumb_mcp',)) - for version in (46, 48): - owner.begin(motion, session_epoch=1, motion_version=version) - for row in recording['records']: - if row['kind'] == 'motion_branch_observation' and row['task_name'] == spec.task_key and row['motion_version'] == version: - owner.observe(row) - request = owner.request('front', replace(motion, phase='joint_zero', zero_joints=('thumb_mcp',)), - session_epoch=1, motion_version=49, zero_references={reference.joint: reference}) - return owner, request - - -@pytest.fixture(scope='module') -def replayed(recording, tmp_path_factory): - owner, request = request_from_recording(recording) - assert not request.invalid_reason and request.transition_source is not None - result = solve_initialization(request) - assert result.resolved, result.reason - _, report = initialization_artifacts(request, result) - (tmp_path_factory.mktemp('preparation_transition')/'replay_report.json').write_text( - json.dumps(report, indent=2, allow_nan=False)) - records = [*recording['records'], report] - return owner.profile, request, result, records - - -def test_real_mcp_requires_transition_evidence_not_a_lower_error_winner(replayed): - _, request, result, _ = replayed - old = resolve_image_motion(request.image_frames, request.relations) - assert not old.resolved and old.reason == 'image_motion_families_not_distinguishable' - assert len(request.frames) == 128 and len(request.transition_source.rows) == 138 - assert len(result.hypotheses) == 4 - accepted = [h for h in result.hypotheses if not h.reason] - assert len(accepted) == 2 - assert result.model.maximum_reprojection_error_px == 1.5 - assert result.model.reprojection_tie_px == .03 - assert all(q.maximum_frame_rms_px < 1.5 for h in accepted - for q in (*h.training_quality, *h.validation_quality)) - assert all(h.reason == 'image_motion_reprojection_failed' for h in result.hypotheses if h.reason) - assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) - assert result.preparation_transitions[0]['source_frame_hashes'] == { - str(r['image_stamp_ns']): source_frame_sha256(r) for r in request.transition_source.rows} - - -def test_real_transition_rebuilds_through_formal_evidence_boundary(replayed): - profile, _, _, records = replayed - evidence = _initializations(records) - validate_source_geometry(profile, SOURCE, records) - validate_preparation_transitions(profile, records, evidence) - - -@pytest.mark.parametrize('fault', ['held_feedback', 'held_command', 'feedback_stuck', 'endpoint', - 'camera', 'epoch', 'validation', 'duplicate', 'installation']) -def test_invalid_entry_evidence_cannot_authorize_a_model(replayed, fault): - profile, request, _, _ = replayed - rows = deepcopy(request.transition_source.rows) - if fault == 'held_feedback': - rows[25]['feedback_vector'][6] += 8 - elif fault == 'held_command': - rows[25]['command_vector'][6] = 1 - elif fault == 'feedback_stuck': - for row in rows: - row['feedback_vector'][1] = 0 - elif fault == 'endpoint': - rows = tuple(r for r in rows if r['command_vector'][1] != 80) + tuple( - r for r in rows if r['command_vector'][1] == 80)[:9] - elif fault == 'camera': - for row in rows: - row['camera_matrix'][0][0] += 1 - elif fault == 'epoch': - rows[25]['session_epoch'] += 1 - elif fault == 'validation': - rows[25]['cycle'] = 3 - elif fault == 'duplicate': - rows[25]['image_stamp_ns'] = rows[24]['image_stamp_ns'] - else: - for row in rows[:7]: - row['tags']['thumb_mcp']['corners_xy'][0][0] += 30 - source = replace(request.transition_source, rows=tuple(rows)) - with pytest.raises(ValueError): - build_preparation_transition(profile, source, request.current_source_rows) - - -@pytest.mark.parametrize('fault', ['missing_pixels', 'policy', 'winner', 'source_zero', 'omitted_frame']) -def test_tampered_evidence_is_rejected_before_publication(replayed, fault): - _, _, _, records = replayed - records = deepcopy(records) - report = records[-1] - source_stamp = int(next(iter(report['preparation_transitions'][0]['source_frame_hashes']))) - if fault == 'missing_pixels': - records[:] = [r for r in records if not (r.get('kind') == 'motion_branch_observation' and r.get('image_stamp_ns') == source_stamp)] - elif fault == 'policy': - report['image_model_selection_policy'] = 'training_squared_loss_holm_v1' - elif fault == 'winner': - report['selected_hypothesis'] = 1 - elif fault == 'source_zero': - records[:] = [r for r in records if r.get('kind') != 'joint_zero_reference'] - else: - for payload in [report, *report['evidence_payloads'].values()]: - payload['preparation_transitions'][0]['source_frame_hashes'].pop(str(source_stamp), None) - for role, payload in report['evidence_payloads'].items(): - report['evidence_ids'][role] = source_frame_sha256(payload) - with pytest.raises(ValueError): - _initializations(records) - - -def test_cancellation_and_new_epoch_do_not_reuse_partial_transition(recording, replayed): - owner, request = request_from_recording(recording) - owner.clear() - assert owner.accept(request, replayed[2]) is None - assert not owner.transition_capture.rows - motion = MotionCommand('zero_approach', owner.profile.motion.joint_zero_references['thumb_mcp'].command, - 200., task_key='thumb_mcp_front', reference_joints=('thumb_mcp',)) - owner.begin(motion, session_epoch=2, motion_version=1) - assert owner.transition_capture.source is None - - -def test_transition_holdout_only_vetoes_without_retraining_or_selecting_another_family(replayed): - profile, request, original, _ = replayed - transition, _ = build_preparation_transition(profile, request.transition_source, request.current_source_rows) - witness = next(frame.evidence.stamp_ns for i, frame in enumerate(transition.frames) - if i % 2 == 1 and transition.endpoints[i] == -1) - rows = deepcopy(request.transition_source.rows) - row = next(r for r in rows if r['image_stamp_ns'] == witness) - for corner in row['tags']['thumb_mcp']['corners_xy']: - corner[0] += 40 - result = solve_initialization(replace(request, transition_source=replace(request.transition_source, rows=rows))) - assert not result.resolved - assert [h.training_quality for h in result.hypotheses] == [h.training_quality for h in original.hypotheses] - - -def test_accepted_transition_allows_runtime_to_continue(recording, replayed): - owner, request = request_from_recording(recording) - assert owner.accept(request, replayed[2]) is not None - spec = owner.profile.motion.joint_zero_references['thumb_mcp'] - motion = MotionCommand('joint_zero', spec.command, 200., task_key=spec.task_key, - zero_joints=('thumb_mcp',)) - assert owner.ready(motion) - assert owner.error() == '' - - -@pytest.mark.parametrize('independently_observable', [False, True]) -def test_missing_optional_transition_uses_original_observability_gate(replayed, independently_observable): - _, request, _, _ = replayed - request = replace(request, transition_source=replace(request.transition_source, - rows=request.transition_source.rows[:20])) - if independently_observable: - from test_motion_image_diagnostics import frames, RELATIONS - request = replace(request, image_frames=frames(), relations=RELATIONS) - result = solve_initialization(request) - assert result.resolved == independently_observable - assert result.preparation_transition_rejections == ('preparation_transition_samples_missing',) - assert not result.preparation_transitions - if not independently_observable: - assert result.reason == 'image_motion_families_not_distinguishable' - - -def test_changed_dependency_mask_cannot_hide_a_moving_joint(replayed): - profile, _, _, records = replayed - records = deepcopy(records) - report = records[-1] - for payload in [report, *report['evidence_payloads'].values()]: - payload['preparation_transitions'][0]['relevant_channels'] = [1] - for role, payload in report['evidence_payloads'].items(): - report['evidence_ids'][role] = source_frame_sha256(payload) - with pytest.raises(ValueError, match='preparation_transition_topology_changed'): - validate_source_geometry(profile, SOURCE, records) - - -def test_changed_frozen_model_cannot_replace_original_image_solution(replayed): - profile, _, _, records = replayed - evidence = deepcopy(_initializations(records)) - for payload in evidence.values(): - if payload.get('preparation_transitions'): - # Recomputing the outer digest cannot replace raw-image replay. - geometry = payload['frozen_image_model']['geometry'][0] - geometry['mount_child_xyz_m'] = tuple(v+.001 for v in geometry['mount_child_xyz_m']) - payload['image_model_sha256'] = source_frame_sha256(payload['frozen_image_model']) - with pytest.raises(ValueError, match='preparation_transition_frozen_model_changed'): - validate_preparation_transitions(profile, records, evidence) diff --git a/src/linkerhand_calibration/test/test_preparation_witness.py b/src/linkerhand_calibration/test/test_preparation_witness.py deleted file mode 100644 index 2a55004..0000000 --- a/src/linkerhand_calibration/test/test_preparation_witness.py +++ /dev/null @@ -1,343 +0,0 @@ -"""Real ambiguity stays rejected; independent synthetic views exercise the new path.""" -from copy import deepcopy -from dataclasses import asdict, replace -import gzip -import json -from pathlib import Path - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation -from linkerhand_calibration.core.domain.capture_plan import configure_training, CapturePlan -from linkerhand_calibration.core.geometry.tag_pose import production_image_motion as solver -from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points, solve_square_tag_ippe -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.preparation_witness import WitnessInput, build_preparation_witness, select_witness_rows -from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256 -from linkerhand_calibration.runtime.preparation_witness_evidence import validate_preparation_witnesses, validate_witness_topology -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope='module') -def ring_source(): - data = json.loads(gzip.decompress(Path(__file__).with_name('fixtures').joinpath('o30_ring_mcp_ambiguous.json.gz').read_bytes())) - profile = load_bundled_hand_profile('o30_right_18') - owner = BranchInitialization(profile, source_urdf=SOURCE) - failure = data['failure'] - spec = profile.motion.joint_zero_references['ring_mcp_pitch'] - motion = MotionCommand('zero_approach', spec.command, 200., task_key=failure['task_name'], reference_joints=('ring_mcp_pitch',)) - owner.begin(motion, session_epoch=1, motion_version=failure['motion_version']) - for row in data['rows']: - owner.observe(row) - request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('ring_mcp_pitch',)), - session_epoch=1, motion_version=failure['motion_version']+1) - fits = [] - original = solver._fit_hypothesis - def trace(*args, **kwargs): - fit = original(*args, **kwargs) - fits.append(fit) - return fit - with pytest.MonkeyPatch.context() as patch: - patch.setattr(solver, '_fit_hypothesis', trace) - result = solve_initialization(request) - return profile, owner, request, result, tuple(fits), tuple(data['rows']) - - -@pytest.mark.replay -def test_real_ring_is_not_authorized_without_new_pose_information(ring_source): - _, _, request, result, _, _ = ring_source - assert len(request.image_frames) == 127 - assert not result.resolved and result.reason == 'image_motion_families_not_distinguishable' - assert all(h.training_accepted for h in result.hypotheses) - - -@pytest.fixture(scope='module') -def witnessed_ring(ring_source): - old_profile, owner, request, _, fits, rows = ring_source - profile = configure_preparation(configure_training(old_profile, 'staged_2_to_3'), 'occlusion_aware_witness_v1') - channels = tuple(sorted(owner.tag_feedback_channels['ring_pip'])) - arc_stamps = {f.evidence.stamp_ns for f in request.image_frames} - endpoints = tuple(r for r in owner._endpoint_records['side'] if r['image_stamp_ns'] not in arc_stamps) - # This is a *synthetic* independent yaw arc, anchored to candidate 0 to - # exercise selection. It does not claim that real hardware chose branch 0. - model = fits[0][1] - pose = next(s.pose for s in select_image_motion_frame(model, request.image_frames[-1]).selections if s.role == 'ring_pip') - orientation = Rotation.from_quat(pose.quaternion_xyzw) - point = np.asarray(pose.translation_xyz_m) - pivot = point-np.array([0., .03, -.04]) - donor = profile.motion.preparation_witnesses['ring_mcp_pitch'] - target_spec = profile.motion.joint_zero_references['ring_mcp_pitch'] - start_command, excursion = donor.command[4], donor.approach_commands[1][4] - segments = [(start_command, excursion), (excursion, start_command)] - start = request.image_frames[0].evidence.stamp_ns-20_000_000_000 - sources = [] - for path, (before, after) in enumerate(segments, 1): - values = list(np.linspace(before, after, 33)) - if path == len(segments): - values += [after]*11 - for value in values: - row = deepcopy(rows[0]); stamp = start+len(sources)*40_000_000 - row.update(task_name=target_spec.task_key, zero_joints=['ring_mcp_pitch'], - preparation_witness_joint='ring_mcp_roll', - motion_version=100+path, reference_path_index=path, image_stamp_ns=stamp, - command_vector=list(donor.command), feedback_vector=list(donor.command)) - row['command_vector'][4] = row['feedback_vector'][4] = float(value) - turn = Rotation.from_rotvec(np.array([0., 1., 0.])*np.deg2rad((value-113)*.3)) - rotation, translation = turn*orientation, pivot+turn.apply(point-pivot) - points = rotation.apply(square_object_points(.0165))+translation - image = points@np.asarray(row['camera_matrix']).T - corners = image[:, :2]/image[:, 2:] - tag = row['tags']['ring_pip'] - tag['corners_xy'] = corners.tolist() - tag['selected_pose'] = None - tag['reprojection_valid_candidates'] = [asdict(p) for p in solve_square_tag_ippe(corners, - tag_size_m=.0165, camera_matrix=row['camera_matrix'])] - row['tags'] = {r: row['tags'][r] for r in ('side_base', 'ring_pip')} - for item in row['tags'].values(): - item['candidate_diagnostics']['observation_stamp_ns'] = stamp - sources.append(row) - source = WitnessInput('ring_mcp_pitch', 'ring_mcp_roll', tuple(sources), endpoints, channels) - bridge, record = build_preparation_witness(profile, source) - result = solver.resolve_image_motion(request.image_frames, request.relations, pose_bridges=(bridge,)) - assert result.resolved, result.reason - request = replace(request, witness_sources=(source,), witness_profile=profile) - result = replace(result, preparation_witnesses=(record,)) - _, report = initialization_artifacts(request, result) - # Journal round trip makes tuple/list normalization part of this test. - records = json.loads(json.dumps([*sources, *rows, report])) - return profile, source, bridge, record, request, result, records - - -def test_independent_witness_resolves_real_recipient_without_changing_limits(witnessed_ring): - profile, source, bridge, record, request, result, _ = witnessed_ring - assert result.resolved and len(result.hypotheses) == 2 - assert sum(h.training_accepted for h in result.hypotheses) == 1 - assert result.model.maximum_reprojection_error_px == 1.5 and result.model.reprojection_tie_px == .03 - assert bridge.rotation_uncertainty_rad > 0 and bridge.translation_uncertainty_m > 0 - assert set(record['source_model']['training_stamps_ns']).isdisjoint(record['source_model']['validation_stamps_ns']) - assert set(record['source_frame_hashes']).isdisjoint(record['endpoint_frame_hashes']) - assert set(record['endpoint_frame_hashes']).isdisjoint(dict(request.source_frame_hashes)) - assert source.rows[-1]['image_stamp_ns'] < request.image_frames[0].evidence.stamp_ns - - -def test_witness_formal_evidence_replays_original_pixels_and_model(witnessed_ring): - from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel - profile, _, _, _, _, _, records = witnessed_ring - evidence = _initializations(records) - validate_witness_topology(profile, records, evidence, UrdfKinematicModel(SOURCE)) - validate_preparation_witnesses(profile, records, evidence) - - -@pytest.mark.legacy -def test_legacy_witness_evidence_keeps_its_original_admission_contract(witnessed_ring): - profile, source, _, _, request, result, records = witnessed_ring - _, legacy = build_preparation_witness(profile, source, legacy=True) - assert legacy['version'] == 'preparation_witness_v1' and 'sample_admission' not in legacy - for hypothesis in legacy['source_hypotheses']: - hypothesis.pop('training_solver_attempts', None) - _, report = initialization_artifacts(request, replace(result, preparation_witnesses=(legacy,))) - records = json.loads(json.dumps([*records[:-1], report])) - evidence = _initializations(records) - validate_preparation_witnesses(profile, records, evidence) - - -@pytest.mark.parametrize('change', ['pixels', 'missing', 'policy', 'window']) -def test_witness_tampering_is_rejected_without_whole_hand_refitting(witnessed_ring, change): - _, _, _, _, _, _, original = witnessed_ring - records = deepcopy(original) - if change == 'pixels': - records[1]['tags']['ring_pip']['corners_xy'][0][0] += .1 - elif change == 'missing': - del records[1] - elif change == 'policy': - records[-1]['image_model_selection_policy'] = 'training_squared_loss_holm_v1' - else: - payload = next(iter(records[-1]['evidence_payloads'].values())) - # Even coordinated hash updates must not hide an inconvenient raw frame. - record = records[-1]['preparation_witnesses'][0] - stamp = sorted(record['source_frame_hashes'], key=int)[1] - del record['source_frame_hashes'][stamp] - payload['preparation_witnesses'] = deepcopy(records[-1]['preparation_witnesses']) - role = payload['tag_role']; records[-1]['evidence_ids'][role] = source_frame_sha256(payload) - with pytest.raises(ValueError): - _initializations(records) - - -@pytest.mark.parametrize('change', ['command', 'feedback', 'formal', 'endpoint', 'missing']) -def test_invalid_witness_never_triggers_silent_fallback(witnessed_ring, change): - profile, source, _, _, request, _, _ = witnessed_ring - rows = deepcopy(source.rows) - endpoints = deepcopy(source.endpoint_rows) - if change == 'command': - rows[3]['command_vector'][9] += 1 - elif change == 'feedback': - rows[3]['feedback_vector'][9] += 3 - elif change == 'formal': - rows[3]['cycle'] = 3 - elif change == 'endpoint': - endpoints[0]['feedback_vector'][4] += 11 - else: - rows = () - changed = replace(source, rows=tuple(rows), endpoint_rows=tuple(endpoints)) - result = solve_initialization(replace(request, witness_sources=(changed,))) - assert not result.resolved and result.reason.startswith('preparation_witness_') - - -def test_plan_changes_are_opt_in_and_preserve_existing_tag_and_motion_contracts(): - from linkerhand_calibration.profiles.validator import validate_executable_profile - old = configure_training(load_bundled_hand_profile('o30_right_18'), 'staged_2_to_3') - new = configure_preparation(old, 'occlusion_aware_witness_v1') - assert new.vision == old.vision and new.quality == old.quality - assert {t.key: t for t in new.motion.tasks} == {t.key: t for t in old.motion.tasks} - assert new.motion.joint_zero_references == old.motion.joint_zero_references - assert CapturePlan.from_profile(new).sha256 != CapturePlan.from_profile(old).sha256 - assert 'preparation_witnesses' not in CapturePlan.from_profile(old).as_dict()['motion_contract']['motion'] - validate_executable_profile(new, SOURCE) - - -def test_live_collector_and_request_use_same_witness_as_offline(witnessed_ring, ring_source): - profile, source, bridge, _, _, expected, _ = witnessed_ring - _, _, old_request, _, _, rows = ring_source - owner = BranchInitialization(profile, source_urdf=SOURCE) - # The donor's optional side view never becomes a formal roll measurement. - for row in source.rows: - owner.observe(row) - assert owner._frames == {} - target = profile.motion.joint_zero_references[source.joint] - motion = MotionCommand('zero_approach', target.command, 200., task_key=target.task_key, - reference_joints=(source.joint,)) - owner.begin(motion, session_epoch=1, motion_version=old_request.motion_version) - for row in rows: - owner.observe(row) - request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=(source.joint,)), - session_epoch=1, motion_version=old_request.motion_version+1) - assert len(request.witness_sources) == 1 - result = solve_initialization(request) - # The live budget now retains more recipient frames. The independent - # witness stays identical; reproduce the recipient with that exact budget. - reproduced = solver.resolve_image_motion(request.image_frames, request.relations, - pose_bridges=(bridge,), parameters=request.image_parameters) - assert result.resolved and result.model == reproduced.model - assert result.preparation_witnesses == expected.preparation_witnesses - model, report = owner.accept(request, result) - assert model is not None and report['preparation_witnesses'] - - -def test_collector_is_bounded_and_epoch_reset_does_not_relabel_history(witnessed_ring): - from linkerhand_calibration.runtime.preparation_witness import MAXIMUM_WINDOW_FRAMES - profile, source, *_ = witnessed_ring - owner = BranchInitialization(profile, source_urdf=SOURCE) - row = source.rows[0] - for _ in range(MAXIMUM_WINDOW_FRAMES+20): - owner.witness_capture.observe(row) - assert len(next(iter(owner.witness_capture.windows.values()))) == MAXIMUM_WINDOW_FRAMES+1 - owner.witness_capture.observe(dict(row, session_epoch=2)) - assert list(owner.witness_capture.windows) == [(2, row['task_name'], row['view'])] - assert len(next(iter(owner.witness_capture.windows.values()))) == 1 - - -def test_missing_optional_tag_does_not_block_donor_capture(witnessed_ring): - from linkerhand_calibration.runtime.preparation_witness import witness_roles - profile, source, *_ = witnessed_ring - owner = BranchInitialization(profile, source_urdf=SOURCE) - motion = MotionCommand('zero_approach', profile.motion.joint_zero_references[source.donor].command, - 200., task_key='fingers_mcp_roll_front', reference_joints=(source.donor,)) - motion = replace(motion, reference_joints=('ring_mcp_pitch',), preparation_witness_joint='ring_mcp_roll') - assert witness_roles(profile, motion) == {'side_base', 'ring_pip', 'front_base', 'ring_mcp_roll'} - row = deepcopy(source.rows[0]); row['tags'].pop('ring_pip') - owner.observe(row) - assert not owner._invalid_reason - assert owner._frames == {} - - -def test_reference_change_and_unobservability_cannot_trigger_same_arc_retry(): - from linkerhand_calibration.runtime.zero_recovery import failure_category, permits_recovery, FailureCategory - assert failure_category('preparation_witness_unobservable:ambiguous') == FailureCategory.UNOBSERVABLE - assert failure_category('preparation_witness_reference_feedback_changed') == FailureCategory.REFERENCE_CHANGED - assert not permits_recovery(('preparation_witness_unobservable:ambiguous',), 0) - - -@pytest.mark.parametrize('change', ['holdout', 'installation']) -def test_new_images_can_veto_but_cannot_replace_witness(witnessed_ring, change): - profile, source, _, record, request, _, _ = witnessed_ring - rows, endpoints = deepcopy(source.rows), deepcopy(source.endpoint_rows) - if change == 'holdout': - stamp = record['source_model']['validation_stamps_ns'][4] - affected = [r for r in rows if r['image_stamp_ns'] == stamp] - else: - affected = endpoints - for row in affected: - row['tags']['ring_pip']['corners_xy'] = [[x+10, y+8] for x, y in row['tags']['ring_pip']['corners_xy']] - result = solve_initialization(replace(request, witness_sources=(replace(source, - rows=tuple(rows), endpoint_rows=tuple(endpoints)),))) - assert not result.resolved and result.reason.startswith('preparation_witness_') - assert result.model is None - - -def test_required_witness_cannot_be_removed_by_updating_hashes(witnessed_ring): - profile, _, _, _, _, _, original = witnessed_ring - records = deepcopy(original) - report = records[-1] - del report['preparation_witnesses'] - report['image_model_selection_policy'] = 'training_squared_loss_holm_v1' - for role, payload in report['evidence_payloads'].items(): - del payload['preparation_witnesses'] - report['evidence_ids'][role] = source_frame_sha256(payload) - evidence = _initializations(records) - with pytest.raises(ValueError, match='required_evidence_missing'): - validate_preparation_witnesses(profile, records, evidence) - - -def test_witness_configuration_round_trip_and_topology_are_explicit(tmp_path): - from linkerhand_calibration.profiles.export import write_hand_profile - from linkerhand_calibration.profiles import load_hand_profile - from linkerhand_calibration.profiles.preparation import validate_preparation_witnesses - from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel - profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), - 'staged_2_to_3'), 'occlusion_aware_witness_v1') - path = tmp_path/'witness.yaml'; write_hand_profile(profile, path) - restored = load_hand_profile(path) - assert CapturePlan.from_profile(restored) == CapturePlan.from_profile(profile) - invalid = replace(profile, motion=replace(profile.motion, - preparation_witnesses={'ring_mcp_pitch': replace(profile.motion.preparation_witnesses['ring_mcp_pitch'], joint='pinky_mcp_roll')})) - with pytest.raises(ValueError, match='posture_not_shared'): - validate_preparation_witnesses(invalid, UrdfKinematicModel(SOURCE)) - - -@pytest.mark.parametrize('joint', ['pinky_mcp_pitch', 'ring_mcp_pitch', 'middle_mcp_pitch', 'index_mcp_pitch']) -def test_occluded_fingers_get_one_local_excitation_after_clearance(joint): - from linkerhand_calibration.runtime.execution import SessionExecution - from linkerhand_calibration.runtime.session import CalibrationPhase as Phase - from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint - profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), - 'staged_2_to_3'), 'occlusion_aware_witness_v1') - driver = SessionExecution(profile) - task_key = profile.motion.joint_zero_references[joint].task_key - driver.session._unit_index = next(i for i, unit in enumerate(driver.session._units) if unit.task_key == task_key) - driver.session.phase = Phase.PREPARE - effects = driver._make_effects(profile.command.baseline_values) - witness = profile.motion.preparation_witnesses[joint] - excitation = [m for m in effects if m.preparation_witness_joint] - assert len(excitation) == 3 and [m.reference_path_index for m in excitation] == [0, 1, 2] - first = effects.index(excitation[0]) - assert first > 0 and effects[first-1].phase == 'clearance' - assert effects[first-1].target == witness.command == excitation[-1].target - channel = channel_for_joint(profile, witness.joint) - for motion in excitation: - assert not motion.recording and motion.command_index == channel - assert all(v == int(v) for v in motion.target) - assert all(v == witness.command[i] for i, v in enumerate(motion.target) if i != channel) - normal = next(i for i, m in enumerate(effects) if m.phase == 'zero_approach' and not m.preparation_witness_joint) - assert effects.index(excitation[-1]) < normal - # Revisit and independent validation do not re-excite or refit a frozen zero. - driver = SessionExecution(profile) - driver.zero_references[joint] = object() - driver.session._unit_index = next(i for i, unit in enumerate(driver.session._units) - if unit.task_key == task_key and unit.cycle == 1) - driver.session.phase = Phase.PREPARE - assert not any(m.preparation_witness_joint for m in driver._make_effects(witness.command)) diff --git a/src/linkerhand_calibration/test/test_prepared_resume.py b/src/linkerhand_calibration/test/test_prepared_resume.py deleted file mode 100644 index 00293ad..0000000 --- a/src/linkerhand_calibration/test/test_prepared_resume.py +++ /dev/null @@ -1,221 +0,0 @@ -"""Checkpoint decoding/geometry replay must finish before live feedback starts.""" - -from copy import deepcopy -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.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine -from linkerhand_calibration.runtime.joint_resume import JointResume -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from runtime_host_fixture import coordinator_fixture, ready - - -@pytest.mark.parametrize("changed_reference", [False, True]) -@pytest.mark.parametrize("model,layout", [("o6", "o6_right_8"), ("l6", "l6_right_8"), - ("o12", "o12_right_16"), ("g20", "g20_right_19"), ("o30", "o30_right_18")]) -def test_resume_tick_uses_prepared_data_and_still_checks_current_reference( - tmp_path, monkeypatch, model, layout, changed_reference): - profile = load_bundled_hand_profile(layout) - transform = None - if model == "o30": - from linkerhand_calibration.core.domain.capture_plan import STAGED, CapturePlan, configure_training - from linkerhand_calibration.runtime import staged_resume - transform = lambda p: configure_training(p, STAGED) - profile = transform(profile) - fingerprint = {"kind": "fixed_base_reference_locked", "profile_id": profile.key.profile_id, - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "protected_hashes": {"source": "a" * 64}, - "fixed_corners_by_view": {v: [[10, 10], [20, 10], [20, 20], [10, 20]] for v in profile.vision.view_names}, - "fixed_poses": {v: {"rotation_xyzw": [0, 0, 0, 1], "translation_xyz_m": [0, 0, .5]} - for v in profile.vision.view_names}, "moving_tag_poses": {}} - rows = [{"kind": "session_start", "serial_number": "OFFLINE_COORDINATOR", - "profile_id": profile.key.profile_id, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, - "capture_schedule_version": CalibrationEngine(profile).capture_schedule_version}, fingerprint] - if model == "o30": - rows[0]["capture_plan"] = CapturePlan.from_profile(profile).as_dict() - path = tmp_path / "previous.jsonl" - path.write_text("\n".join(json.dumps(row) for row in rows)) - staged = [] - # Actual provenance and per-unit quality replay are covered by their own - # recorded-data tests; here assert that this expensive boundary is pre-live. - monkeypatch.setattr(JointResume, "stage", lambda self, *args: staged.append(self)) - prepared_imports = [] - if model == "o30": - original_import = staged_resume.reference_import - def prepare_visits(resume, engine, records): - resume.rows = tuple(records) - staged.append(resume) - def prepare_import(resume, **kwargs): - pending = original_import(resume, **kwargs) - prepared_imports.append(pending) - return pending - monkeypatch.setattr(staged_resume, "stage_visits", prepare_visits) - monkeypatch.setattr(staged_resume, "stage_passed_visits", prepare_visits) - monkeypatch.setattr(staged_resume, "reference_import", prepare_import) - host, clock = coordinator_fixture(tmp_path, model=model, resume_raw_samples_path=path, - profile_transform=transform) - try: - assert len(staged) == 1 and not host.started and not clock.positions - assert host.joint_resume is not staged[0] - if model == "o30": - assert len(prepared_imports) == 1 - assert host._resume_checkpoint.passed_import is prepared_imports[0] - assert host._passed_reference_import is None - monkeypatch.setattr(staged_resume, "reference_import", - lambda *args: pytest.fail("historical provenance indexing in live callback")) - path.unlink() # The immutable loaded snapshot is sufficient after startup. - monkeypatch.setattr(JointResume, "stage", lambda *args: pytest.fail("geometry replay in live callback")) - ready(host, clock) - assert host.start().success - clock.now += .1 - host.receive_feedback(profile.command.names, profile.command.baseline_values, host.ports.clock_ns()) - host._current_fingerprint = deepcopy(fingerprint) - if changed_reference: - view = profile.vision.view_names[0] - host._current_fingerprint["fixed_corners_by_view"][view][0][0] += 20 - host.execution.session.phase = Phase.RESUME_VERIFY - host.tick() - assert host.execution.session.phase == (Phase.PAUSED if model == "o30" and changed_reference else Phase.PREPARE) - if model == "o30": - assert (host._passed_reference_import is prepared_imports[0]) is not changed_reference - assert host.feedback_fresh() - assert (host.joint_resume is staged[0]) is not changed_reference - assert host._resumed_count == 0 # Each old zero still needs physical verification. - finally: - host.close() - - -def test_unreadable_checkpoint_is_rejected_without_delaying_live_motion(tmp_path): - host, clock = coordinator_fixture(tmp_path, resume_raw_samples_path=tmp_path / "missing.jsonl") - try: - assert host._resume_checkpoint is None and host._resume_checkpoint_error - ready(host, clock) - assert host.start().success - clock.now += .1 - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.execution.session.phase = Phase.RESUME_VERIFY - host.tick() - assert host.execution.session.phase == Phase.PREPARE - assert host._resumed_count == 0 and not host.joint_resume.references - finally: - host.close() - - -@pytest.mark.parametrize("abort_during_import", [False, True]) -def test_large_task_import_yields_to_feedback_and_commits_only_after_fsync( - tmp_path, monkeypatch, abort_during_import): - from linkerhand_calibration.runtime import coordinator - - host, clock = coordinator_fixture(tmp_path) - try: - ready(host, clock) - assert host.start().success - clock.now += .1 - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.execution.session.phase = Phase.PREPARE - unit = host.execution.session.current_unit - key = (unit.task_key, unit.cycle, unit.direction) - rows = tuple({"kind": "import_test", "index": i} for i in range(257)) - monkeypatch.setattr(host.joint_resume, "take_task", lambda task: ((key,), rows)) - writes, events = [], [] - original = coordinator.append_jsonl_many - def write(path, batch, *, durable=True): - assert not durable and len(batch) <= 64 - # The whole transfer would exceed the one-second feedback gate; - # each bounded callback permits another real feedback reception. - clock.now += len(batch) * .01 - writes.extend(batch) - original(path, batch, durable=durable) - monkeypatch.setattr(coordinator, "append_jsonl_many", write) - original_sync = coordinator.sync_jsonl - def sync(path): - assert host._resumed_count == 0 - original_sync(path) - events.append("durable") - monkeypatch.setattr(coordinator, "sync_jsonl", sync) - host.tick() - assert host._resumed_count == 0 and len(writes) == 64 - if abort_during_import: - assert host.abort().success - host.tick() - assert host.state == "ABORTED" and host._resumed_count == 0 - assert len(writes) == 64 - else: - while host._resume_import is not None: - assert host.feedback_fresh() - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.tick() - assert writes == list(rows) and events == ["durable"] - assert host._resumed_count == 1 and host.state != "PAUSED" - finally: - monkeypatch.undo() - host.close() - - -@pytest.mark.parametrize("interruption", [None, "abort", "feedback_loss"]) -def test_zero_provenance_import_yields_and_never_authorizes_an_incomplete_zero( - tmp_path, monkeypatch, interruption): - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from linkerhand_calibration.runtime import coordinator - from test_motion_reference_provenance import motion_capture - - host, clock = coordinator_fixture(tmp_path) - try: - ready(host, clock) - assert host.start().success - host.execution.session.phase = Phase.PREPARE - _, reference, _ = motion_capture(layout="o6_right_8") - motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 1., - task_key=reference.task_key, zero_joints=(reference.joint,)) - # Geometry and old/new pose comparisons have their own independent - # provenance tests. Exercise the real coordinator's transfer boundary. - monkeypatch.setattr(host.branch_initialization, "ready", lambda _: True) - monkeypatch.setattr(coordinator, "build_joint_zero_reference", lambda *a, **k: reference) - monkeypatch.setattr(coordinator, "reference_branch_revisions", lambda *a, **k: {}) - monkeypatch.setattr(host, "_freeze_reference_branches", lambda _: True) - provenance = tuple({"kind": "import_source", "index": i} for i in range(257)) - monkeypatch.setattr(host.joint_resume, "take_motion_provenance_records", lambda: provenance) - assert host._lock_joint_zeros(motion) is False - assert not host.execution.zero_references and host._reference_import is not None - - writes, barriers = [], [] - original_write, original_sync = coordinator.append_jsonl_many, coordinator.sync_jsonl - def write(path, rows, *, durable=True): - assert not durable and len(rows) <= 64 - clock.now += len(rows) * .01 - writes.extend(rows) - original_write(path, rows, durable=durable) - def sync(path): - assert not host.execution.zero_references - assert writes[-1]["kind"] == "joint_zero_reference" - original_sync(path) - barriers.append("durable") - monkeypatch.setattr(coordinator, "append_jsonl_many", write) - monkeypatch.setattr(coordinator, "sync_jsonl", sync) - - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.tick() - assert len(writes) == 64 and not host.execution.zero_references - if interruption: - if interruption == "abort": - assert host.abort().success - else: - clock.now += 2. - host.tick() - assert host.state in {"ABORTED", "PAUSED"} - assert len(writes) == 64 and not barriers and not host.execution.zero_references - else: - while not host._reference_import.complete: - assert not host.execution.zero_references - host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) - host.tick() - assert barriers == ["durable"] and writes[:-1] == list(provenance) - assert host._lock_joint_zeros(motion) is True - assert host.execution.zero_references == {reference.joint: reference} - assert host._reference_import is None and host.state != "PAUSED" - finally: - monkeypatch.undo() - host.close() diff --git a/src/linkerhand_calibration/test/test_process_finalizer.py b/src/linkerhand_calibration/test/test_process_finalizer.py deleted file mode 100644 index 2773f30..0000000 --- a/src/linkerhand_calibration/test/test_process_finalizer.py +++ /dev/null @@ -1,149 +0,0 @@ -import json -from concurrent.futures import ThreadPoolExecutor -from pathlib import Path -from threading import Event -import time -from functools import partial -import hashlib -import tracemalloc - -import pytest - -from linkerhand_calibration.runtime.artifacts.process_finalizer import ( - isolated_finalize_profile_session, read_journal_prefix, -) - - -def sample_finalizer(*, session_dir, records, publish, phase_changed): - assert publish is False - for phase in ("fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"): - phase_changed(phase) - return tuple(row["value"] for row in records) - - -def waiting_finalizer(*, session_dir, records, publish, phase_changed): - phase_changed("entered") - time.sleep(30) - Path(session_dir, "unexpected_completion").touch() - - -def test_bounded_journal_and_process_preserve_data_and_event_order(tmp_path): - path = tmp_path / "raw.jsonl" - prefix = b'{"value": 1}\n{"value": 2}\n' - path.write_bytes(prefix + b'{"value": 999}\n') - events = [] - result = isolated_finalize_profile_session(journal_path=path, journal_size=len(prefix), - records=({"value": -1},), phase_changed=events.append, cancelled=lambda: False, - finalizer=sample_finalizer, session_dir=tmp_path) - assert result == (1, 2) - assert events == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"] - timing = json.loads((tmp_path/"finalization_timing.json").read_text()) - assert timing["backend"] == "isolated_process" - assert timing["journal_prefix_bytes"] == len(prefix) - with pytest.raises(ValueError, match="incomplete"): - read_journal_prefix(path, len(prefix)-1) - with pytest.raises(ValueError, match="cannot_publish"): - isolated_finalize_profile_session(journal_path=path, journal_size=len(prefix), - records=(), phase_changed=events.append, cancelled=lambda: False, publish=True) - - -def test_cancel_terminates_child_without_waiting_for_fit_or_publication(tmp_path): - path = tmp_path/"raw.jsonl" - path.write_bytes(b'{"value": 1}\n') - entered, cancelled = Event(), Event() - with ThreadPoolExecutor(max_workers=1) as executor: - future = executor.submit(isolated_finalize_profile_session, journal_path=path, - journal_size=path.stat().st_size, records=(), phase_changed=lambda _: entered.set(), - cancelled=cancelled.is_set, finalizer=waiting_finalizer, session_dir=tmp_path) - assert entered.wait(10) - cancelled.set() - with pytest.raises(ValueError, match="operator_abort"): - future.result(timeout=3) - assert not (tmp_path/"unexpected_completion").exists() - - -def test_child_invalid_input_is_reported_without_a_result(tmp_path): - path = tmp_path/"raw.jsonl" - path.write_bytes(b'{"value": 1}') - with pytest.raises(RuntimeError, match="incomplete_finalization_journal_prefix"): - isolated_finalize_profile_session(journal_path=path, journal_size=path.stat().st_size, - records=(), phase_changed=lambda _: None, cancelled=lambda: False, - finalizer=sample_finalizer, session_dir=tmp_path) - - -def test_large_journal_has_bounded_raw_buffer_memory(tmp_path): - """Large raw evidence must not be duplicated when the fit process starts.""" - path = tmp_path / "large.jsonl" - line = b'{"value": 1}' + b" " * (256 * 1024) + b"\n" - with path.open("wb") as stream: - for _ in range(64): - stream.write(line) - tracemalloc.start() - try: - rows = read_journal_prefix(path, path.stat().st_size) - _, peak = tracemalloc.get_traced_memory() - finally: - tracemalloc.stop() - assert rows == ({"value": 1},) * 64 - assert peak < 4 * 1024 * 1024 - - -def test_finalizer_transparently_reads_indexed_compressed_prefix(tmp_path): - from linkerhand_calibration.core.artifacts.evidence_journal import initialize_evidence_journal - from linkerhand_calibration.storage import append_jsonl, append_jsonl_many - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=64) - rows = [{"value": 1}, {"value": 2, "evidence": list(range(1000))}] - append_jsonl(path, rows[0]) - append_jsonl_many(path, rows[1:]) - assert read_journal_prefix(path, path.stat().st_size) == tuple(rows) - - -@pytest.mark.parametrize("data,size,error", [ - (b'{"value": 1}\n', 100, "incomplete"), - (b'{"value": 1}\n[]\n', None, "invalid_finalization_journal_rows"), -]) -def test_journal_rejects_short_file_or_nonobject_record(tmp_path, data, size, error): - path = tmp_path / "invalid.jsonl" - path.write_bytes(data) - with pytest.raises(ValueError, match=error): - read_journal_prefix(path, len(data) if size is None else size) - - -def skip_standard_loader(_path): - pass - - -def test_real_finalizer_stages_in_child_and_only_controller_publishes(tmp_path): - from measured_capture_fixture import measured_capture - from linkerhand_calibration.runtime.artifacts.controller import FinalizationController, FinalizationInputs - from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session - from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper - profile, source, records, _ = measured_capture(tmp_path, layout="o6_right_8") - session = tmp_path/"session" - session.mkdir() - raw = session/"raw_samples.jsonl" - raw.write_text("".join(json.dumps(row)+"\n" for row in records)) - hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} - hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() - controller = FinalizationController(profile, session, isolated=True, - finalizer=partial(finalize_profile_session, standard_loader=skip_standard_loader)) - phases, authorizations = [], [] - def authorize(): - assert phases == ["fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"] - assert not (tmp_path/profile.artifacts.publication_pointer).exists() - authorizations.append(True) - try: - controller.start(FinalizationInputs(session, "SYNTHETIC_PROCESS_ONLY", source, hashes, - tuple(records), journal_path=raw, journal_size=raw.stat().st_size)) - deadline = time.monotonic()+60 - result = None - while result is None and time.monotonic() < deadline: - result = controller.poll(phases.append, authorize) - time.sleep(.01) - assert result is not None - assert authorizations == [True] - assert controller.poll(phases.append, authorize) is None - load_unified_mapper(session/"release_manifest.json") - finally: - controller.close() diff --git a/src/linkerhand_calibration/test/test_production_image_motion.py b/src/linkerhand_calibration/test/test_production_image_motion.py deleted file mode 100644 index 9f741a3..0000000 --- a/src/linkerhand_calibration/test/test_production_image_motion.py +++ /dev/null @@ -1,218 +0,0 @@ -"""Production image models preserve independent images and frozen geometry.""" -from dataclasses import asdict, replace -import json -import gzip -from pathlib import Path - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -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.motion_evidence import MotionRelation -from test_motion_image_diagnostics import frames, RELATIONS - - -def recorded_image_frames(rows): - from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates - from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation - from linkerhand_calibration.core.geometry.tag_pose.types import SquareTagPose - - source = [] - for raw in rows: - roles = tuple(RoleCandidates(item["role"], tuple(SquareTagPose( - tuple(p["quaternion_xyzw"]), tuple(p["translation_xyz_m"]), p["reprojection_error_px"]) - for p in item["candidates"]), item["frozen"]) for item in raw["evidence"]["roles"]) - source.append(ImageMotionFrame(MotionEvidenceFrame(raw["evidence"]["stamp_ns"], roles), - tuple(tuple(v) for v in raw["camera_matrix"]), tuple(ImageRoleObservation( - o["role"], o["tag_size_m"], tuple(tuple(p) for p in o["corners_xy"])) for o in raw["observations"]))) - return tuple(source) - - -@pytest.fixture(scope="module") -def resolved(): - source = frames(depth_error=.008) - result = resolve_image_motion(source, RELATIONS) - assert result.resolved, result - return source, result - - -def test_noisy_pnp_depth_is_replaced_by_current_image_geometry(resolved): - source, result = resolved - assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) - clean = frames() - for frame, reference in zip(source[::5], clean[::5]): - selected = select_image_motion_frame(result.model, frame) - assert selected.resolved, selected - estimate = dict((p.role, p.pose) for p in selected.selections)["moving"] - actual = reference.evidence.roles[1].candidates[0] - assert np.linalg.norm(np.subtract(estimate.translation_xyz_m, actual.translation_xyz_m)) < 1e-6 - assert (Rotation.from_quat(estimate.quaternion_xyzw).inv() - * Rotation.from_quat(actual.quaternion_xyzw)).magnitude() < 1e-5 - serialized = asdict(result.model) - json.dumps(serialized, allow_nan=False) - assert image_motion_model_from_dict(serialized) == result.model - - -def test_frozen_geometry_does_not_absorb_tag_slide(resolved): - source, result = resolved - before = asdict(result.model) - frame = source[-1] - shifted = replace(frame.observations[1], corners_xy=tuple((x + 12, y) for x, y in frame.observations[1].corners_xy)) - check = select_image_motion_frame(result.model, replace(frame, observations=(frame.observations[0], shifted))) - assert not check.resolved - assert asdict(result.model) == before - - -def test_current_roots_camera_and_source_identity_are_required(resolved): - source, result = resolved - frame = source[-1] - invalid = replace(frame, camera_matrix=((1700., 0., 640.), *frame.camera_matrix[1:])) - assert not select_image_motion_frame(result.model, invalid).resolved - unfrozen = replace(frame.evidence.roles[0], frozen=False) - invalid = replace(frame, evidence=replace(frame.evidence, roles=(unfrozen, *frame.evidence.roles[1:]))) - assert not select_image_motion_frame(result.model, invalid).resolved - changed = asdict(result.model) - changed["validation_stamps_ns"] = changed["training_stamps_ns"] - with pytest.raises(ValueError, match="source_overlap"): - image_motion_model_from_dict(changed) - changed = asdict(result.model) - changed["maximum_reprojection_error_px"] = 5. - with pytest.raises(ValueError, match="quality_policy"): - image_motion_model_from_dict(changed) - - -def test_no_rotation_cannot_authorize_model(): - source = frames() - still = tuple(replace(source[0], evidence=replace(source[0].evidence, - stamp_ns=f.evidence.stamp_ns)) for f in source) - result = resolve_image_motion(still, RELATIONS) - assert not result.resolved - assert result.model is None - - -def test_frozen_prediction_does_not_refit_angle_or_geometry(resolved, monkeypatch): - from linkerhand_calibration.core.geometry.tag_pose import image_motion_projection as projection - source, result = resolved - # Duplicate pixels with a new identity are solely a synthetic truth fixture. - frame = replace(source[-1], evidence=replace(source[-1].evidence, - stamp_ns=max(result.model.source_stamps_ns)+1)) - angles = tuple(a for _, a in select_image_motion_frame(result.model, frame).angles_rad) - before = asdict(result.model) - monkeypatch.setattr(projection, "least_squares", lambda *a, **k: - pytest.fail("Frozen prediction must not estimate validation angles")) - quality = projection.image_motion_prediction_errors(result.model, frame, angles) - assert max(q.maximum_frame_rms_px for q in quality) < 1e-5 - wrong = projection.image_motion_prediction_errors(result.model, frame, np.asarray(angles)+.2) - assert max(q.maximum_frame_rms_px for q in wrong) > 1.5 - with pytest.raises(ValueError, match="prediction_training_overlap"): - projection.image_motion_prediction_errors(result.model, source[-1], angles) - with pytest.raises(ValueError, match="prediction_training_overlap"): - projection.image_motion_prediction_errors(result.model, frame, angles, - dependency_stamps_ns=(frame.evidence.stamp_ns,)) - with pytest.raises(ValueError, match="prediction_angles_invalid"): - projection.image_motion_prediction_errors(result.model, frame, (float('nan'),)) - assert asdict(result.model) == before - - -def test_alternatives_within_image_resolution_are_not_ranked_as_truth(): - source = frames(alternatives=True) - result = resolve_image_motion(source, RELATIONS) - assert len(result.hypotheses) == 2 - # A near-frontal noiseless plane still admits distinct shallow-tilt models - # inside the public pixel tolerance. A lower score alone cannot resolve it. - assert not result.resolved and result.reason == "image_motion_families_not_distinguishable" - assert result.model is None - - -def test_serial_graph_and_previously_frozen_parent_share_same_solver(): - source = frames(chain=True, depth_error=.001) - chain = (*RELATIONS, MotionRelation("distal", "moving", "terminal")) - result = resolve_image_motion(source, tuple(reversed(chain))) - assert result.resolved, result - final = select_image_motion_frame(result.model, source[-1]) - assert final.resolved and len(final.angles_rad) == 2 - # Roots may themselves move; use each image's already selected parent. - clean = frames(chain=True) - source = tuple(replace(f, evidence=replace(f.evidence, roles=tuple( - replace(r, frozen=True) if r.role == "moving" else r for r in f.evidence.roles))) for f in clean) - distal = resolve_image_motion(source, (chain[1],)) - assert distal.resolved, distal - - -def test_motion_stratified_evidence_requires_mean_and_rank_improvement(): - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import _comparison - angles = np.linspace(0, 1.2, 72)[:, None] - baseline = np.full(72, .3) - assert _comparison(baseline, baseline + .02, angles, .03) == 1. - assert _comparison(baseline, baseline + .2, angles, .03) < .001 - alternative = baseline.copy() - alternative[-2:] += 20 - assert _comparison(baseline, alternative, angles, .03) > .01 - - -def test_recorded_preparation_only_freeze_predicts_both_later_directions(): - fixture = Path(__file__).parent / "fixtures" / "o6_image_motion_200044.json.gz" - payload = json.loads(gzip.decompress(fixture.read_bytes())) - assert payload["source_sha256"] == "52f9ed7510fc58a4271532a2dc13a678f2e4aa2f1dc3153bf749fcf377cb23c5" - models, validated = {}, [] - for stage in payload["stages"]: - source = recorded_image_frames(stage["frames"]) - metadata = stage["metadata"] - if metadata["sample_phase"] == "zero_approach": - result = resolve_image_motion(source, tuple(MotionRelation(**r) for r in stage["relations"])) - assert result.resolved, (metadata, result) - models[metadata["view"]] = result.model - else: - assert metadata["cycle"] == 0 - model = models[metadata["view"]] - frozen = asdict(model) - assert set(f.evidence.stamp_ns for f in source).isdisjoint(model.source_stamps_ns) - for frame in source: - check = select_image_motion_frame(model, frame) - assert check.resolved, (metadata, check) - assert max(q.rms_px for q in check.quality) < .5 - assert asdict(model) == frozen - validated.append((metadata["view"], metadata["direction"])) - assert set(validated) == {(view, direction) for view in ("front", "top") - for direction in ("increasing", "decreasing")} - - -def test_recorded_pinky_alternatives_converge_within_original_budget(): - """The side-view preparation previously exhausted 150 steps in two families.""" - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ImageMotionParameters - - fixture = Path(__file__).parent / "fixtures" / "o6_pinky_preparation_144514.json.gz" - payload = json.loads(gzip.decompress(fixture.read_bytes())) - assert payload["source_sha256"] == "77d3337dfd5384409fcf4352e76c925b48938912b4d47c5341cc2c92d8cef332" - source = recorded_image_frames(payload["frames"]) - assert len(source) == 120 - limits = ImageMotionParameters() - assert limits.maximum_solver_evaluations == payload["recorded_exhausted_solver_evaluations"] == 150 - result = resolve_image_motion(source, tuple(MotionRelation(**r) for r in payload["relations"])) - assert result.resolved, result - assert len(result.hypotheses) == 4 - for hypothesis in result.hypotheses: - assert hypothesis.converged and hypothesis.training_solver_status > 0 - assert 0 < hypothesis.training_solver_evaluations <= 150 - assert len(result.model.training_stamps_ns) == len(result.model.validation_stamps_ns) == 60 - assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) - assert result.model.maximum_reprojection_error_px == 1.5 - - -def test_hinge_pivot_parameterization_is_continuous_across_axis_helper_boundary(): - from linkerhand_calibration.core.geometry.tag_pose.motion_image_solver import ImageHingeBundle - source = frames() - poses = {role.role: tuple(frame.evidence.roles[index].candidates[0] for frame in source) - for index, role in enumerate(source[0].evidence.roles)} - bundle = ImageHingeBundle(source, RELATIONS, poses) - first, second = bundle.initial.copy(), bundle.initial.copy() - first[5:7] = second[5:7] = [.02, .01] - first[0], second[0] = np.arcsin(.9) - 1e-8, np.arcsin(.9) + 1e-8 - axis_a, _, pivot_a, _ = bundle.geometry_values(first, 0) - axis_b, _, pivot_b, _ = bundle.geometry_values(second, 0) - assert np.linalg.norm(pivot_b - pivot_a) < 1e-8 - assert abs(axis_a @ pivot_a) < 1e-12 - assert abs(axis_b @ pivot_b) < 1e-12 diff --git a/src/linkerhand_calibration/test/test_profile_finalization.py b/src/linkerhand_calibration/test/test_profile_finalization.py deleted file mode 100644 index c8e3345..0000000 --- a/src/linkerhand_calibration/test/test_profile_finalization.py +++ /dev/null @@ -1,244 +0,0 @@ -"""Production finalizer recovery using an independent elementary FK generator. - -The virtual URDF preserves the vendor's geometry but uses explicit synthetic -limits. It is not evidence that a real hand or its original limits passed. -""" - -from dataclasses import replace -from pathlib import Path -import hashlib -import xml.etree.ElementTree as ET - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.extrinsics import matrix_payload -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.core.domain.profile import ProfileKey -from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session - - -PACKAGE = Path(__file__).resolve().parents[1] - - -def rigid(rpy, xyz): - matrix = np.eye(4) - matrix[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() - matrix[:3, 3] = xyz - return matrix - - -def virtual_capture(tmp_path, layout, *, include_baseline=False, profile_transform=None): - base_layout = "l6_right_8" if layout == "virtual_reordered" else layout - profile = load_bundled_hand_profile(base_layout) - # This synthetic fixture generates the original interleaved partitions. - profile = replace(profile, acquisition=replace(profile.acquisition, - command_capture_mode="interleaved", steady_extra_training_nodes={})) - # This independent FK sensor fixture supplies measured 3D poses, not - # corner images or IPPE candidates. Its synthetic observation profile - # therefore has no image-branch reconstruction stage. - profile = replace(profile, measurement=replace(profile.measurement, - candidate_selection_tasks=frozenset())) - # This fixture preserves the legacy v2 acceptance contract. New measured - # behavior is exercised separately using include_baseline and real v3 refs. - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2, - directional_command_joints=profile.artifacts.directional_command_joints if include_baseline else frozenset())) - if include_baseline: - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=3)) - if profile_transform is not None: - profile = profile_transform(profile) - if layout == "virtual_reordered": - profile = replace(profile, key=ProfileKey("VIRTUAL", "right", layout), - namespace="/virtual_calibration", - artifacts=replace(profile.artifacts, output_schema_version=2), - motion=replace(profile.motion, tasks=tuple(reversed(profile.motion.tasks)))) - filename = {"l6_right_8": "l6_right/linkerhand_l6v3.1_right.urdf", - "o6_right_8": "o6_right/linkerhand_o6_right.urdf", - "g20_right_19": "g20_right/linkerhand_g20_right.urdf", - "o12_right_16": "o12_right/linkerhand_o12_t3_right-0703.urdf"}[base_layout] - original = PACKAGE / "urdf" / filename - if not original.exists(): - original = next((PACKAGE / "urdf/o6_right").glob("*.urdf")) - tree = ET.parse(original) - root = tree.getroot() - root.set("name", "virtual_geometry_acceptance_not_hardware") - joints = {node.get("name"): node for node in root.findall("joint")} - for node in joints.values(): - limit = node.find("limit") - if limit is not None and node.get("type") == "revolute": - limit.set("lower", "-10" if node.find("mimic") is not None else "-2") - limit.set("upper", "10" if node.find("mimic") is not None else "2") - for mesh in root.iter("mesh"): - filename = mesh.get("filename") - if not filename.startswith(("package://", "file://", "/")): - mesh.set("filename", (original.parent / filename).resolve().as_uri()) - source = tmp_path / "virtual_source.urdf" - tree.write(source) - parents = {node.find("child").get("link"): name for name, node in joints.items()} - base = rigid((0.11, -0.08, 0.17), (0.11, -0.04, 0.72)) - base_tag = rigid((0.2, -0.1, 0.3), (0.003, 0.008, 0.055)) - tags = {name: rigid((0.12, -0.09, 0.2), (0.009, -0.025, 0.016)) - for name in profile.measurement.measurements} - # Independent fixture declarations: one actual mount per physical Tag, - # including Tags reused by several measurement tasks. - native_tag_joint = {"thumb_cmc": "thumb_cmc_pitch", "thumb_mcp": "thumb_mcp", - "thumb_dip": "thumb_dip", "thumb_yaw": "thumb_cmc_yaw", - "middle_roll": "middle_mcp_roll", "index_roll": "index_mcp_roll", - "pinky_mcp": "pinky_mcp_pitch", "pinky_pip": "pinky_pip", "pinky_dip": "pinky_dip", - "middle_pip": "middle_pip", "middle_dip": "middle_dip", "index_pip": "index_pip", "index_dip": "index_dip"} - native_tag_joint.update(thumb_ip="thumb_ip", ring_roll="ring_mcp_roll", pinky_roll="pinky_mcp_roll", - ring_pip="ring_pip", ring_dip="ring_dip") - if base_layout == "o6_right_8": - # Confirmed physical layout: top ID7 and front ID2 share the distal - # segment, even though only yaw is swept while observing ID7. - native_tag_joint["thumb_yaw"] = "thumb_ip" - direct = profile.zero.direct_zero_joints - offsets = {name: (0.04 if name.endswith(("roll", "yaw")) else -0.02 if "thumb" in name else 0.02) - for name in direct} - for target, donor in profile.zero.transferred_zero_sources.items(): - offsets[target] = offsets[donor] - def elementary_fk(joint, angles): - # Deliberately independent of UrdfKinematicModel and patch helpers. - node = joints[joint] - parent = parents.get(node.find("parent").get("link")) - transform = np.eye(4) if parent is None else elementary_fk(parent, angles) - origin = node.find("origin") - if origin is not None: - transform = transform @ rigid([float(v) for v in origin.get("rpy", "0 0 0").split()], - [float(v) for v in origin.get("xyz", "0 0 0").split()]) - if node.get("type") != "fixed": - axis = np.asarray([float(v) for v in node.find("axis").get("xyz").split()]) - motion = np.eye(4) - motion[:3, :3] = Rotation.from_rotvec(axis/np.linalg.norm(axis)*angles[joint]).as_matrix() - transform = transform @ motion - return transform - records = [] - for task in profile.motion.tasks: - for cycle in range(4): - for direction in ("increasing", "decreasing"): - codes = (np.linspace(0, 255, 81).round().astype(int) if profile.command.unit == "u8" - else np.linspace(min(task.start_value, task.end_value), max(task.start_value, task.end_value), 81)) - if include_baseline: - from linkerhand_calibration.core.domain.sampling import command_nodes - codes = np.unique(np.r_[codes, command_nodes(profile, task), command_nodes(profile, task, holdout=True)]) - if direction == "decreasing": - codes = codes[::-1] - for index, code in enumerate(codes): - state = list(profile.command.baseline_values) - for channel, value in task.auxiliary_commands: - state[channel] = value - state[task.command_index] = float(code) - angles = {name: offsets.get(name, 0) + ((profile.command.baseline_values[channel]-state[channel])/255 * - (0.65 if "thumb" in name and "pitch" in name else 1.05) if profile.command.unit == "u8" - else profile.command.joint_directions[channel]*state[channel]*0.7) - for name, channel in profile.command.command_index_by_joint.items()} - def passive_angle(name): - if name in angles: - return angles[name] - mimic = joints[name].find("mimic") - angles[name] = float(mimic.get("multiplier", "1"))*passive_angle(mimic.get("joint")) + float(mimic.get("offset", "0")) - return angles[name] - for name in profile.zero.passive_joints: - passive_angle(name) - measurements = [(name, name, False) for name in task.joints] - measurements += [(name, profile.measurement.cross_view_sources[name], True) - for name in task.joints if name in profile.measurement.cross_view_sources] - for name, spec_name, secondary in measurements: - spec = profile.measurement.measurements[spec_name] - if base_layout in {"o12_right_16", "g20_right_19"}: - def tag_pose(role): - joint = native_tag_joint.get(role) - return base @ base_tag if joint is None else base @ elementary_fk(joint, angles) @ tags[joint] - parent, child = tag_pose(spec.parent_role), tag_pose(spec.child_role) - else: - child = base @ elementary_fk(name, angles) @ tags[name] - parent_joint = profile.zero.mimic_source_by_joint.get(name) - parent = base @ base_tag if parent_joint is None else base @ elementary_fk(parent_joint, angles) @ tags[parent_joint] - relative = np.linalg.inv(parent) @ child - records.append({"kind": "joint_sample", "observation_joint" if secondary else "joint": name, "task_name": task.key, - "view": spec.view, "sample_id": f"{spec.view}:{task.key}:{cycle}:{direction}:{index}", - "cycle": cycle, "direction": direction, "attempt": 1, f"feedback_{profile.command.unit}": float(code), - f"command_{profile.command.unit}": float(code), f"command_vector_{profile.command.unit}": list(state), - f"state_{profile.command.unit}": state, "relative_quaternion_xyzw": Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(), - "image_relative_xy_px": [30+100*np.cos(angles[name]+.23), 20+100*np.sin(angles[name]+.23)], - "relative_translation_xyz_m": relative[:3, 3].tolist(), - "parent_pose_common": matrix_payload(parent), "child_pose_common": matrix_payload(child), - "view_normal_common_xyz": [0.577350269]*3, "camera_center_common_xyz_m": [0, 0, -0.5]}) - # Independent sensor observations at the requested steady grid, using the - # same elementary FK truth above, not the fitted calibration as truth. - from linkerhand_calibration.runtime.engine import CalibrationEngine - from linkerhand_calibration.runtime.steady import steady_targets - steady = [] - for unit in CalibrationEngine(profile).scan_units(): - samples = [r for r in records if (r["task_name"], r["cycle"], r["direction"]) - == (unit.task_key, unit.cycle, unit.direction)] - for index, target in enumerate(steady_targets(profile, unit)): - matched = [r for r in samples if abs(r[f"command_{profile.command.unit}"]-target) < 1e-8] - assert matched - for row in matched: - for frame in range(3): - steady.append({**row, "sample_id": row["sample_id"]+f":steady:{frame}", - "sample_phase": "steady", "steady_index": index, "steady_target": target}) - records.extend(steady) - return profile, source, records, offsets - - -@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "virtual_reordered", "o12_right_16", "g20_right_19"]) -def test_profile_only_finalizer_recovers_geometry_and_serialized_urdf(tmp_path, layout): - profile, source, records, expected = virtual_capture(tmp_path, layout) - if layout == "virtual_reordered": - # The new hand is loaded through the product's real YAML contract; - # there is no Python registry entry or model-specific engine binding. - import yaml - from linkerhand_calibration.profiles import dump_hand_profile - from linkerhand_calibration.product import load_product_config - path = tmp_path/"new_hand.yaml" - path.write_text(dump_hand_profile(profile)) - product = yaml.safe_load((PACKAGE/"config/l6_right_product.yaml").read_text()) - product.update(profile_id=profile.key.profile_id, model=profile.key.model, - namespace=profile.namespace, tag_layout=profile.key.layout, - profile_config=str(path), profile_config_sha256=hashlib.sha256(path.read_bytes()).hexdigest()) - product["artifacts"].update(source_urdf=str(source), source_urdf_sha256=hashlib.sha256(source.read_bytes()).hexdigest()) - product_path = tmp_path/"new_hand_product.yaml" - product_path.write_text(yaml.safe_dump(product)) - profile = load_product_config(product_path, workspace=PACKAGE.parents[1], check_can=False).calibration_contract.typed_profile - from engine_capture_fixture import collect_with_engine - from linkerhand_calibration.runtime.session import CalibrationPhase as Phase - execution, records = collect_with_engine(profile, records) - phase_events = { - "fit_complete": execution.session.fit_complete, - "holdout_complete": lambda: execution.session.holdout_complete(passed=True), - "artifacts_built": execution.session.artifacts_built, - "urdf_validated": lambda: execution.session.urdf_validated(passed=True), - } - session = tmp_path / "session" - loaded = [] - hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} - hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() - payload, fit, correction = finalize_profile_session(profile=profile, session_dir=session, - serial_number="VIRTUAL_ONLY", source_urdf=source, protected_inputs=hashes, - records=records, standard_loader=lambda path: loaded.append(path), - phase_changed=lambda event: phase_events[event]()) - assert execution.session.phase == Phase.PUBLISH - execution.session.published() - assert loaded - assert (tmp_path / profile.artifacts.publication_pointer).resolve() == session - assert payload["format"] == "unified_calibration_v2" - assert payload["schema_version"] == 2 - assert (session / "calibration_report.json").is_file() - assert "quality" not in payload - for row in payload["joints"].values(): - assert "command_to_rad" not in row - assert set(row) == {"sdk_channel", "angle_rad"} | ({"input_values"} if profile.command.unit == "rad" else set()) - if profile.command.unit == "u8": - assert len(row["angle_rad"]) == 256 - if payload["schema_version"] == 7: - from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.artifacts import validate_o12_runtime_payload - validate_o12_runtime_payload(payload) - if payload["schema_version"] == 4: - from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import validate_compact_payload - validate_compact_payload(payload) - for name, value in expected.items(): - assert fit.zero_offsets_rad[name] == pytest.approx(value, abs=0.004) - assert correction.path.is_file() diff --git a/src/linkerhand_calibration/test/test_profile_loader.py b/src/linkerhand_calibration/test/test_profile_loader.py deleted file mode 100644 index d357e87..0000000 --- a/src/linkerhand_calibration/test/test_profile_loader.py +++ /dev/null @@ -1,136 +0,0 @@ -from __future__ import annotations - -from pathlib import Path -import pytest -import yaml - -from linkerhand_calibration.profiles import load_hand_profile -from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry -from linkerhand_calibration.profiles import dump_hand_profile -from linkerhand_calibration.runtime import CalibrationEngine -from linkerhand_calibration.profiles.loader import load_bundled_hand_profile - - -@pytest.mark.parametrize("section,key", [(None, "commmand"), ("command", "maximum_velocty"), - ("motion", "resume_verfication_waypoints"), ("zero", "cad_frozen_joint")]) -def test_profile_typo_is_rejected_before_hardware(tmp_path, section, key): - payload = yaml.safe_load(dump_hand_profile(load_bundled_hand_profile("l6_right_8"))) - (payload if section is None else payload[section])[key] = [] - path = tmp_path/"invalid.yaml" - path.write_text(yaml.safe_dump(payload)) - with pytest.raises(ValueError, match="unknown"): - load_hand_profile(path) - - -def test_new_model_loads_without_model_python_state_machine(tmp_path: Path) -> None: - profile_file = tmp_path / "virtual.yaml" - profile_file.write_text( - """schema_version: 1 -profile_id: VIRTUAL/right/one_joint/v1 -namespace: /virtual_calibration -sdk_adapter: legacy_byte_sdk -command: - names: [finger] - baseline_u8: [255] - command_index_by_joint: {finger_joint: 0} - urdf_joint_by_joint: {finger_joint: finger_joint} -vision: - common_frame: camera - extrinsic_reference_view: camera - views: - - name: camera - tags: - - {role: base, id: 0, fixed_reference: true} - - {role: finger, id: 1} -motion: - tasks: - - key: finger_sweep - view: camera - command_index: 0 - joints: [finger_joint] - start_u8: 255 - end_u8: 0 - formal_speed_u8: 40 -measurement: - measurements: - finger_joint: {kind: rotation, view: camera, parent_role: base, child_role: finger} -zero: - active_joints: [finger_joint] - passive_joints: [] - direct_zero_joints: [finger_joint] - axis_joints: [finger_joint] - mechanical_endpoint_joints: [] - post_solve_endpoint_joints: [] - mimic_source_by_joint: {} - cad_frozen_joints: [] -quality: - training_cycles: [0, 1, 2] - holdout_cycle: 3 - hard_threshold_keys: [holdout] - isolated_holdout: true -scope: - default_scope: full - calibrate_joints: {full: [finger_joint]} - frozen_joints: {full: []} -artifacts: - output_schema_version: 1 - calibration_filename: calibration.json - corrected_urdf_filename: corrected.urdf - protected_input_fields: [source_urdf_sha256] - publish_corrected_urdf: true -acquisition: {policy_version: unified_engine_v8_all_view_images} -urdf: - authorized_fields: - finger_joint: [origin.rpy, limit.lower, limit.upper] -joint_coverage: {finger_joint: measured_static_dynamic} -""", - encoding="utf-8", - ) - profile = load_hand_profile(profile_file) - assert profile.sdk_adapter == "legacy_byte_sdk" - assert profile.urdf_authorized_fields["finger_joint"] == { - "origin.rpy", "limit.lower", "limit.upper" - } - assert len(CalibrationEngine(profile).scan_units()) == 8 - - -def test_all_product_profiles_have_lossless_declarative_contracts( - tmp_path: Path, -) -> None: - expected = {"g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"} - checked = set() - for registered in get_default_registry(): - profile = registered.profile - if profile.key.layout not in expected: - continue - assert profile == load_bundled_hand_profile(profile.key.layout) - path = tmp_path / f"{profile.key.layout}.yaml" - path.write_text(dump_hand_profile(profile), encoding="utf-8") - assert load_hand_profile(path) == profile - checked.add(profile.key.layout) - assert checked == expected - - -def test_product_loading_a_new_profile_does_not_construct_model_registry(tmp_path, monkeypatch): - import hashlib - import yaml - from linkerhand_calibration import product - root = Path(__file__).resolve().parents[1] - profile_data = yaml.safe_load((root / "config/profiles/g20_right_19.yaml").read_text()) - profile_data["profile_id"] = "VIRTUAL/right/new_configuration_only/v1" - profile_data["namespace"] = "/virtual_calibration" - profile_path = tmp_path / "virtual.yaml" - profile_path.write_text(yaml.safe_dump(profile_data)) - data = yaml.safe_load((root / "config/g20_right_product.yaml").read_text()) - data.update(profile_id=profile_data["profile_id"], model="VIRTUAL", - tag_layout="new_configuration_only", profile_config=str(profile_path), - profile_config_sha256=hashlib.sha256(profile_path.read_bytes()).hexdigest()) - path = tmp_path / "product.yaml" - path.write_text(yaml.safe_dump(data)) - def forbidden(): - raise AssertionError("declarative product must not resolve a Python model") - monkeypatch.setattr(product, "get_default_registry", forbidden) - loaded = product.load_product_config(path, workspace=root.parents[1], check_can=False) - assert loaded.model == "VIRTUAL" - assert loaded.calibration_contract.typed_profile.key.layout == "new_configuration_only" - # This is the input-contract test, deliberately not an end-to-end claim. diff --git a/src/linkerhand_calibration/test/test_progress_status.py b/src/linkerhand_calibration/test/test_progress_status.py deleted file mode 100644 index c9b750c..0000000 --- a/src/linkerhand_calibration/test/test_progress_status.py +++ /dev/null @@ -1,107 +0,0 @@ -from __future__ import annotations -import pytest - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry -from linkerhand_calibration.runtime import ( - CalibrationPhase, - CalibrationSession, - ResumeDecision, - SweepQuality, -) - - -def _profile(layout: str): - return next( - item.profile for item in get_default_registry() - if item.profile.key.layout == layout - ) - - -@pytest.mark.parametrize("reason", ["fixed_reference_moved", "feedback_stale", "hardware_fault", - "duplicate_controller", "physical_range_exceeded", "protected_camera_model_changed"]) -def test_live_failures_have_specific_chinese_reasons_and_suggestions(reason): - from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh - code, message, suggestion = reason_zh({"reason": reason+":details"}, model_name="VIRTUAL") - assert code != "CALIBRATION-500" and message and suggestion - session = CalibrationSession(_profile("l6_right_8")) - session.pause(code, message, {"raw_reason": reason}, suggestion=suggestion) - assert session.status().pause.suggestion == suggestion - - -def test_one_session_machine_reports_task_tags_and_locked_warning() -> None: - session = CalibrationSession(_profile("o12_right_16")) - session.device_ready() - session.start(resume_requested=True) - session.baseline_complete() - session.reference_locked() - session.resume_checked(ResumeDecision(False, "基准变化，已重新采集")) - session.preparation_complete() - while session.phase == CalibrationPhase.MAPPING_PROBE: - session.mapping_probe_complete() - status = session.status() - assert status.state == "SWEEP" - assert status.reference_locked - assert "请勿移动" in status.reference_message - assert status.task.required_tag_ids_by_view - - -def test_one_same_speed_rescan_then_pause_with_reason() -> None: - session = CalibrationSession(_profile("l6_right_8")) - session.device_ready() - session.start() - session.baseline_complete() - session.reference_locked() - session.preparation_complete() - failed = SweepQuality(False, ("frames=20",), (), {}) - session.sweep_complete() - session.evaluation_complete(failed) - assert session.phase == CalibrationPhase.PREPARE - session.preparation_complete() - assert session.phase == CalibrationPhase.RESCAN - session.sweep_complete() - session.evaluation_complete(failed) - assert session.phase == CalibrationPhase.PAUSED - assert session.status().pause.code == "sweep_quality_failed" - - -def test_physical_mapping_probe_occurs_after_each_task_prepare() -> None: - session = CalibrationSession(_profile("o12_right_16")) - session.device_ready() - session.start() - session.baseline_complete() - session.reference_locked() - session.preparation_complete() - assert session.phase == CalibrationPhase.MAPPING_PROBE - session.mapping_probe_complete() - passed = SweepQuality(True, (), (), {}) - first_task = session.current_unit.task_key - while session.current_unit is not None and session.current_unit.task_key == first_task: - if session.phase == CalibrationPhase.PREPARE: - session.preparation_complete() - assert session.phase == CalibrationPhase.SWEEP - session.sweep_complete() - session.evaluation_complete(passed) - assert session.phase == CalibrationPhase.PREPARE - session.preparation_complete() - assert session.phase == CalibrationPhase.MAPPING_PROBE - - -def test_resume_skips_only_complete_units_and_still_prepares_current_task() -> None: - session = CalibrationSession(_profile("l6_right_8")) - first_task = session.current_unit.task_key - completed = tuple( - (unit.task_key, unit.cycle, unit.direction) - for unit in session.engine.scan_units() - if unit.task_key == first_task - ) - session.device_ready() - session.start(resume_requested=True) - session.baseline_complete() - session.reference_locked() - session.resume_checked( - ResumeDecision(True, "fixed_reference_verified; pending_joint_zero") - ) - assert session.current_unit.task_key == first_task - session.authorize_task_reuse(completed) - assert session.phase == CalibrationPhase.PREPARE - assert session.current_unit.task_key != first_task diff --git a/src/linkerhand_calibration/test/test_raw_draft.py b/src/linkerhand_calibration/test/test_raw_draft.py new file mode 100644 index 0000000..dee2bba --- /dev/null +++ b/src/linkerhand_calibration/test/test_raw_draft.py @@ -0,0 +1,552 @@ +"""Draft precision may fail; source identity, zero semantics and files may not.""" +from dataclasses import asdict +from pathlib import Path +from types import SimpleNamespace +import json +import xml.etree.ElementTree as ET + +import numpy as np +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.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 linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.limits import MotionRangeEvidence +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.raw_completion import file_digest +from linkerhand_calibration.runtime.artifacts.raw_joint_draft import ( + DRAFT_FORMAT, capture_profile_for_draft, load_training_initialization, read_draft_plan, +) +from linkerhand_calibration.runtime.artifacts.raw_joint_report import prepare_raw_motion_ranges +from linkerhand_calibration.runtime.artifacts.serializers.unified_v1 import _mapping +from linkerhand_calibration.runtime.artifacts.urdf_from_json import serialize_urdf_correction + + +@pytest.fixture +def inputs(): + package = Path(__file__).resolve().parents[1] + profile = load_bundled_hand_profile('o30_right_18') + source = package/'urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf' + return package, profile, source, UrdfKinematicModel(source) + + +def payload_for(inputs): + _, profile, source, model = inputs + name, delta = 'thumb_mcp', -.03 + mapping = JointMapping(name, 6, 'command_u8', (0., 255.), (0., .825), (0., .8), (0., .85)) + joint = model.joints[name] + evidence = MotionRangeEvidence(name, name, True, (joint.lower, joint.upper), (0., .85), + None, mapping.bounds_rad, delta, 0., (0, 1), ('front:1', 'front:2'), ('front:1',), + 'calibrated_motion_envelope', 'synthetic_training', None, '', (0, 1), ('front:1', 'front:2'), 'steady_command') + plan = build_standard_correction_plan(source_urdf=source, source_sha256=file_digest(source), + zero_offsets_rad={name: delta}, authorized_fields=profile.urdf_authorized_fields, + measured_ranges_output_rad={name: (0., .85)}, range_evidence={name: evidence}, preserve_source_mimic=True) + return json.loads(json.dumps(dict(format=DRAFT_FORMAT, profile_id=profile.key.profile_id, + accuracy_passed=False, publication_allowed=False, + joints={name: dict(zero_offset_rad=delta, zero_status='estimated_draft', command_to_rad=_mapping(mapping))}, + motion_range_evidence={name: asdict(evidence)}, urdf_correction=serialize_urdf_correction(plan)))) + + +def test_draft_exports_actual_zero_and_ranges_with_precision_unpassed(inputs, tmp_path): + _, profile, source, before = inputs + payload = payload_for(inputs) + plan, mappings = read_draft_plan(payload, source, profile) + output = plan.write(source, tmp_path/'draft.urdf') + after = UrdfKinematicModel(output) + assert after.joints['thumb_mcp'].upper == .85 + assert set(plan.patches.joints) == {'thumb_mcp'} + source_nodes = {node.get('name'): ET.tostring(node) for node in ET.parse(source).findall('joint')} + for node in ET.parse(output).findall('joint'): + if node.get('name') != 'thumb_mcp': + assert ET.tostring(node) == source_nodes[node.get('name')] + for direction in ('increasing', 'decreasing'): + for command in (0, 127, 255): + angles = {'thumb_mcp': mappings['thumb_mcp'].evaluate(command, direction)} + assert np.allclose(before.link_transform('thumb_ip', zero_offsets={'thumb_mcp': -.03}, + joint_angles=angles, independent_mimic_angles=True), + after.link_transform('thumb_ip', zero_offsets={}, joint_angles=angles, independent_mimic_angles=True), atol=1e-12) + assert not (tmp_path/'latest_passed').exists() + + +def test_operator_upright_splay_is_a_definition_and_leaves_ranges_free(inputs, tmp_path): + from raw_image_fixture import raw_image_rows + from linkerhand_calibration.profiles.observations import compile_tag_links + _, profile, source, model = inputs + selected = define_upright_splay_zero(profile, model) + scope = draft_targets(profile, selected) + assert scope['full_target_covered'] and not scope['missing_zero_joints'] + assert len(scope['estimated_zero_joints']) == 11 + assert set(scope['defined_zero_joints']) == UPRIGHT_SPLAY_JOINTS + assert len(scope['retained_leaf_zero_joints']) == 5 + assert selected.command == profile.command and selected.motion == profile.motion + bundle = DraftCommandImages(selected, model, SimpleNamespace(transform=lambda view: np.eye(4)), + raw_image_rows(cycles=(0,)), compile_tag_links(selected, model)) + assert not UPRIGHT_SPLAY_JOINTS.intersection(bundle.zero_columns) + assert UPRIGHT_SPLAY_JOINTS <= bundle.curve_columns.keys() + offsets = {name: .025 for name in bundle.zero_columns} + offsets.update({name: 0. for name in UPRIGHT_SPLAY_JOINTS}) + plan = build_standard_correction_plan(source_urdf=source, source_sha256=file_digest(source), + zero_offsets_rad=offsets, authorized_fields=profile.urdf_authorized_fields, + measured_ranges_output_rad={}, preserve_source_mimic=True) + corrected = UrdfKinematicModel(plan.write(source, tmp_path/'upright.urdf')) + # Verify the exported chain in the palm frame, with nonzero flexion offsets. + for name in UPRIGHT_SPLAY_JOINTS: + finger = name.split('_')[0] + positions = [corrected.link_transform(finger+'_'+joint, zero_offsets={}, joint_angles={}, + independent_mimic_angles=True)[:3, 3] for joint in ('mcp_pitch', 'pip', 'dip')] + assert np.ptp(np.asarray(positions)[:, 1]) < 1e-12 + + +def test_operator_zero_does_not_allow_forged_measured_offset(inputs): + _, profile, source, _ = inputs + payload = payload_for(inputs) + row = payload['joints']['thumb_mcp'] + row['zero_status'] = 'operator_defined' + with pytest.raises(ValueError, match='zero_outside_authorized_model'): + read_draft_plan(payload, source, profile) + row['zero_offset_rad'] = 0. + row['zero_assumption'] = UPRIGHT_SPLAY_REFERENCE + payload['baseline_command'] = list(profile.command.baseline_values) + with pytest.raises(ValueError, match='operator_zero_reference_invalid'): + read_draft_plan(payload, source, profile) + + +@pytest.mark.parametrize('mutation', ['zero', 'curve', 'certification', 'source']) +def test_saved_draft_cannot_change_zero_curve_source_or_claim_precision(inputs, mutation): + _, profile, source, _ = inputs + payload = payload_for(inputs) + if mutation == 'zero': + payload['joints']['thumb_mcp']['zero_offset_rad'] = .04 + elif mutation == 'curve': + payload['joints']['thumb_mcp']['command_to_rad']['increasing_rad'][-1] = 1.4 + elif mutation == 'source': + payload['urdf_correction']['source_urdf_sha256'] = '0'*64 + else: + payload['accuracy_passed'] = True + with pytest.raises(ValueError): + read_draft_plan(payload, source, profile) + + +def test_partial_training_keeps_root_gauge_and_missing_ancestors_out(inputs): + _, profile, _, model = inputs + units = CalibrationEngine(profile).scan_units() + completed = [u.identity for u in units if u.cycle in (0, 1) and + (u.task_key.startswith('thumb_') or u.task_key == 'pinky_mcp_front')] + tags = {tag.role: {} for view in profile.vision.views for tag in view.tags} + selected = draft_profile(profile, model, completed, units, [{'tags': tags}]) + assert selected.zero.active_joints == frozenset(n for n in profile.zero.active_joints if n.startswith('thumb_')) + assert set(selected.zero.cad_zero_assumptions) == {'thumb_cmc_roll', 'thumb_ip'} + assert {'thumb_cmc_yaw', 'thumb_mcp'}.isdisjoint(selected.zero.cad_zero_assumptions) + scope = draft_targets(profile, selected) + assert not scope['full_target_covered'] + assert 'thumb_cmc_roll' in scope['missing_zero_joints'] + assert len(scope['estimated_zero_joints']) == 2 + + +def test_complete_hand_targets_fifteen_zeros_and_all_twenty_ranges(inputs): + _, profile, _, model = inputs + units = CalibrationEngine(profile).scan_units() + tags = {tag.role: {} for view in profile.vision.views for tag in view.tags} + selected = draft_profile(profile, model, [unit.identity for unit in units], units, [{'tags': tags}]) + scope = draft_targets(profile, selected) + assert scope['full_target_covered'] + assert len(scope['estimated_zero_joints']) == 15 + assert len(scope['estimated_range_joints']) == 20 + assert set(scope['retained_leaf_zero_joints']) == {'thumb_ip', 'pinky_dip', 'ring_dip', 'middle_dip', 'index_dip'} + assert 'thumb_cmc_roll' in scope['estimated_zero_joints'] + assert set(scope['estimated_zero_joints']) == set(profile.zero.direct_zero_joints) + + +@pytest.mark.parametrize('missing_cycle', [0, 1]) +def test_one_missing_splay_direction_keeps_full_hand_draft_and_audits_gap(inputs, missing_cycle): + _, profile, _, model = inputs + units = CalibrationEngine(profile).scan_units() + completed = [unit.identity for unit in units if not (unit.task_key == 'fingers_mcp_roll_front' + and unit.cycle == missing_cycle and unit.segment_key == 'linked_increasing')] + tags = {tag.role: {} for view in profile.vision.views for tag in view.tags} + coverage = draft_training_coverage(completed, units) + splay = coverage['fingers_mcp_roll_front'] + assert splay['complete_cycles'] == [1-missing_cycle] + assert splay['initialization_cycle'] == 1-missing_cycle + assert splay['cycles'][missing_cycle] == dict(expected_units=4, accepted_units=3, complete=False) + selected = draft_profile(profile, model, completed, units, [{'tags': tags}]) + assert draft_targets(profile, selected)['full_target_covered'] + assert selected.zero.active_joints == profile.zero.active_joints + + +def test_holdout_and_complementary_partial_cycles_cannot_replace_complete_training(inputs): + _, profile, _, model = inputs + units = CalibrationEngine(profile).scan_units() + completed = [unit.identity for unit in units if not (unit.task_key == 'fingers_mcp_roll_front' + and ((unit.cycle == 0 and unit.segment_key == 'linked_increasing') + or (unit.cycle == 1 and unit.segment_key == 'linked_decreasing')))] + tags = {tag.role: {} for view in profile.vision.views for tag in view.tags} + coverage = draft_training_coverage(completed, units) + assert coverage['fingers_mcp_roll_front']['complete_cycles'] == [] + selected = draft_profile(profile, model, completed, units, [{'tags': tags}]) + assert all(name.startswith('thumb_') for name in selected.zero.active_joints) + + +def test_draft_initialization_uses_the_available_complete_training_cycle(inputs): + from raw_image_fixture import raw_image_rows + from linkerhand_calibration.profiles.observations import compile_tag_links + _, profile, _, model = inputs + rows = raw_image_rows(cycles=(0,)) + extrinsics = SimpleNamespace(transform=lambda view: np.eye(4)) + links = compile_tag_links(profile, model) + first = DraftCommandImages(profile, model, extrinsics, rows, links) + second = DraftCommandImages(profile, model, extrinsics, + [{**row, 'cycle': 1} for row in rows], links, + initialization_cycles={task.key: 1 for task in profile.motion.tasks}) + assert np.allclose(first.initialize((0,)*6), second.initialize((0,)*6)) + with pytest.raises(ValueError, match='training_cycle_required'): + DraftCommandImages(profile, model, extrinsics, rows, links, + initialization_cycles={task.key: 3 for task in profile.motion.tasks}) + + +def test_repeated_pose_reduction_preserves_full_objective_gradient_and_information(inputs): + from copy import copy + from raw_image_fixture import raw_image_rows + from linkerhand_calibration.profiles.observations import compile_tag_links + from linkerhand_calibration.core.geometry.tag_pose.urdf_command_images import UrdfCommandImages + _, profile, _, model = inputs + rows = raw_image_rows(cycles=(0,)) + # Separate real repeated-pose noise from the shared prediction. It must + # remain in the loss, even though its derivative is identically zero. + noisy = [] + for row in rows: + noisy.append({**row, 'cycle': 1, 'tags': {role: {**payload, + 'corners_xy': (np.asarray(payload['corners_xy'])+.2).tolist()} + for role, payload in row['tags'].items()}}) + bundle = DraftCommandImages(profile, model, SimpleNamespace(transform=lambda view: np.eye(4)), + [*rows, *noisy], compile_tag_links(profile, model)) + plain = copy(bundle) + plain.observations = bundle.original_observations + parameters = bundle.initialize((0,)*6) + parameters[bundle.zero_columns['index_pip']] += .01 + residual = bundle.residual(parameters) + original = UrdfCommandImages.residual(plain, parameters) + jacobian = bundle.jacobian(parameters) + full_jacobian = UrdfCommandImages.jacobian(plain, parameters) + assert bundle.within_group_error > 0 + assert len(residual) < len(original)/2 + assert np.allclose(bundle.original_residual(parameters), original) + assert np.isclose(residual @ residual, original @ original, rtol=1e-11) + gradient, full_gradient = jacobian.T @ residual, full_jacobian.T @ original + # Summation order changes cancellation near zero; compare the full norm. + assert np.linalg.norm(gradient-full_gradient) < 1e-11*np.linalg.norm(full_gradient) + assert np.allclose(jacobian.T @ jacobian, full_jacobian.T @ full_jacobian, rtol=1e-10, atol=1e-5) + + +def test_missing_child_observations_retain_only_its_parent_zero(inputs): + from dataclasses import replace + _, profile, _, model = inputs + units = CalibrationEngine(profile).scan_units() + completed = [unit.identity for unit in units if not unit.task_key.startswith('index_')] + tags = {tag.role: {} for view in profile.vision.views for tag in view.tags} + # A tag on the middle link also needs the missing pitch/PIP mappings. + with pytest.raises(ValueError, match='required_training_tags_missing'): + draft_profile(profile, model, completed, units, [{'tags': tags}]) + # Historical root-mounted layout can still measure root travel alone. + profile = replace(profile, vision=replace(profile.vision, views=tuple( + replace(view, tags=tuple(replace(tag, link='index_metacarpals') + if tag.role == 'index_mcp_roll' else tag for tag in view.tags)) + for view in profile.vision.views))) + selected = draft_profile(profile, model, completed, units, [{'tags': tags}]) + # The shared four-finger roll task remains complete, so index roll has a + # measured travel but lacks its downstream axis needed for an absolute zero. + assert 'index_mcp_roll' in selected.zero.active_joints + assert selected.zero.cad_zero_assumptions['index_mcp_roll'] == 'partial_missing_zero_observer:index_mcp_pitch' + scope = draft_targets(profile, selected) + assert not scope['full_target_covered'] + assert len(scope['estimated_zero_joints']) == 12 + assert len(scope['estimated_range_joints']) == 17 + assert scope['missing_zero_joints'] == ['index_mcp_pitch', 'index_mcp_roll', 'index_pip'] + + +def test_old_capture_model_correction_does_not_allow_motion_policy_changes(inputs, tmp_path): + package, profile, _, _ = inputs + text = (package/'config/profiles/o30_right_18.yaml').read_text() + original = tmp_path/'original.yaml' + # Only ID17 changes, not the front tag on the same physical link. + text = text.replace(' # 顶机位标签实际贴在近端指节，随 CMC 和 MCP 运动，不随 IP 运动。\n link: thumb_proximal', + ' link: thumb_metacarpals_base2') + text = text.replace('sdk_to_joint_direction: [1, 1, 1, 1, 1, 1,', + 'sdk_to_joint_direction: [1, 1, -1, -1, -1, -1,') + original.write_text(text) + captured = capture_profile_for_draft(profile, {'profile_config_sha256': file_digest(original)}, original) + assert captured != profile + original.write_text(text.replace('formal_speed_u8: 200', 'formal_speed_u8: 201', 1)) + assert captured.command.baseline_values == profile.command.baseline_values + assert captured.command.joint_directions != profile.command.joint_directions + with pytest.raises(ValueError, match='only_correct_model_bindings_or_directions'): + capture_profile_for_draft(profile, {'profile_config_sha256': file_digest(original)}, original) + + +def test_parallel_root_motion_rejects_reversed_sdk_signs_before_zero_fit(inputs): + from dataclasses import replace + import gzip + from linkerhand_calibration.profiles.observations import compile_tag_links + _, profile, _, model = inputs + # Explicit historical wrong signs; production now contains the correction. + signs = list(profile.command.joint_directions) + signs[2:6] = [-1]*4 + profile = replace(profile, command=replace(profile.command, sdk_to_joint_direction=tuple(signs))) + path = Path(__file__).parent/'fixtures/o30_recorded_root_directions.json.gz' + with gzip.open(path, 'rt') as stream: + rows = json.load(stream)['rows'] + assert {row['cycle'] for row in rows} == {0} + links = compile_tag_links(profile, model) + extrinsics = SimpleNamespace(transform=lambda view: np.eye(4)) + # All compared roots were recorded by the same front camera, so no + # cross-camera transform or fitted palm pose enters their relative axes. + bundle = DraftCommandImages(profile, model, extrinsics, rows, links) + audit = bundle.audit_root_directions() + assert not audit['passed'] and not audit['validation_used'] + conflicts = [row['joints'] for row in audit['comparisons'] if row['reversed_for_all_candidates']] + assert conflicts == [['thumb_cmc_roll', finger+'_mcp_roll'] + for finger in ('index', 'middle', 'ring', 'pinky')] + # The pixels stay immutable: correct only the four declared model signs. + signs = list(profile.command.joint_directions) + signs[2:6] = [1]*4 + corrected = replace(profile, command=replace(profile.command, sdk_to_joint_direction=tuple(signs))) + bundle = DraftCommandImages(corrected, model, extrinsics, rows, links) + assert bundle.audit_root_directions()['passed'] + + +def test_unsynchronized_saved_frames_do_not_supply_motion_range_baseline(inputs): + _, profile, _, model = inputs + values = list(profile.command.baseline_values) + row = dict(task_name='thumb_mcp_front', sample_phase='steady', accepted_sample_window_open=True, + command_vector=values, feedback_vector=values, target_command_vector=values, + command_image_skew_ns=0, state_image_sync_error_ms=0., view='front', cycle=0, + observation_protocol='raw_joint_images_v2', image_stamp_ns=1, + tags={'thumb_mcp': dict(tag_role='thumb_mcp', tag_id=0, image_stamp_ns=1, + pose_source='raw_corners', corners_xy=[[0, 0], [20, 0], [20, 20], [0, 20]])}) + tag = next(tag for view in profile.vision.views for tag in view.tags if tag.role == 'thumb_mcp') + row['tags']['thumb_mcp']['tag_id'] = tag.tag_id + invalid = {**row, 'image_stamp_ns': 2, 'command_vector': None} + mapping = JointMapping('thumb_mcp', 6, 'command_u8', (0., 255.), (0., .8), (0., .8), (0., .8)) + bundle = SimpleNamespace(profile=profile, model=model, cad_names={'thumb_mcp': 'thumb_mcp'}, + output_mappings=lambda _: {'thumb_mcp': mapping}, zero_offsets=lambda _: {'thumb_mcp': -.03}) + evidence = prepare_raw_motion_ranges(bundle, None, [invalid, row])['thumb_mcp'] + assert evidence.reference_sample_ids == ('front:1',) + assert evidence.training_sample_ids == ('front:1',) + assert evidence.training_cycles == (0,) + for invalid in ({**row, 'tags': {}}, {**row, 'cycle': 3}): + with pytest.raises(ValueError, match='baseline_support_missing'): + prepare_raw_motion_ranges(bundle, None, [invalid]) + + +def test_large_image_error_can_generate_draft_but_unobservable_zero_cannot(): + bundle = SimpleNamespace(tags={'tag': object()}, scale=np.ones(2), zero_columns={'joint': 0}, + fit=lambda _: SimpleNamespace(success=True, x=np.array([.02, .1]), fun=np.ones(8)*8, + nfev=1, initialization_evaluations=0), output_mappings=lambda _: {}, + zero_offsets=lambda x: {'joint': x[0]}, jacobian=lambda _: np.tile(np.eye(2), (4, 1))) + _, audit, _ = fit_draft(bundle) + assert audit['precision_certified'] is False + assert audit['candidates'][0]['original_image_precision_passed'] is False + assert audit['estimated_zeros']['joint']['estimated_deg'] > 0 + bundle.jacobian = lambda _: np.tile([[0., 1.]], (8, 1)) + with pytest.raises(ValueError, match='zero_unobservable'): + fit_draft(bundle) + + +@pytest.mark.parametrize('error', [ValueError('angle_measurement_failed'), + IndexError('index 1 is out of bounds for axis 1 with size 1')]) +def test_draft_holdout_evaluator_failure_is_not_an_accuracy_pass(monkeypatch, error): + from linkerhand_calibration.runtime.artifacts import raw_joint_draft as draft + parameters = np.array([.01, .02]) + parameters.flags.writeable = False + def fail(bundle, candidate, holdout): + assert candidate.parameters is parameters + raise error + monkeypatch.setattr(draft, 'validate_raw_joint_candidate', fail) + result = draft.evaluate_draft_holdout(object(), parameters, ()) + assert result['passed'] is False and result['status'] == 'evaluation_failed' + assert result['exception_type'] == type(error).__name__ + assert str(error) in result['traceback'] + assert np.array_equal(parameters, [.01, .02]) + + +def test_draft_stops_after_four_converged_candidates_and_reports_partial_search(): + calls = [] + def fit(seed): + calls.append(seed) + return SimpleNamespace(success=True, x=np.array([.02,.1]), fun=np.ones(8), + nfev=1, initialization_evaluations=0) + bundle = SimpleNamespace(tags={str(i): object() for i in range(6)}, scale=np.ones(2), + zero_columns={'joint': 0}, fit=fit, output_mappings=lambda _: {}, + zero_offsets=lambda x: {'joint': x[0]}, jacobian=lambda _: np.tile(np.eye(2),(4,1))) + _, audit, candidates = fit_draft(bundle) + assert len(calls) == len(candidates) == audit['initializations_evaluated'] == 4 + assert audit['available_initializations'] == 64 + assert not audit['candidate_search_exhaustive'] and not audit['precision_certified'] + assert not audit['validation_used_for_selection'] + + +def test_training_initialization_requires_same_capture_and_never_uses_holdout(inputs, tmp_path, monkeypatch): + from linkerhand_calibration.runtime import runner_support + _, profile, source, _ = inputs + hashes, data = {'source': file_digest(source)}, {'capture': 'same_batch'} + config = SimpleNamespace(source_urdf=source, calibration_contract=SimpleNamespace(typed_profile=profile)) + monkeypatch.setattr(runner_support, 'protected_inputs', lambda _: hashes) + payload = payload_for(inputs) + training = dict(selected_seed=[0], selection='minimum_training_coordinate_rms', + validation_used_for_selection=False) + payload['training'] = training + state = dict(format=DRAFT_FORMAT, generated=True, input_sha256=data, solver_protected_inputs=hashes) + (tmp_path/'draft_result.json').write_text(json.dumps(state)) + (tmp_path/'draft_calibration.json').write_text(json.dumps(payload)) + (tmp_path/'draft_training.json').write_text(json.dumps(training)) + np.savez(tmp_path/'draft_candidates.npz', seeds=[[0]], parameters=[[.1,.2,.3,0.,0.,1.]]) + hint, audit = load_training_initialization(tmp_path, config, data) + assert set(hint['mappings']) == {'thumb_mcp'} + assert hint['offsets']['thumb_mcp'] == -.03 + assert audit['shared_parameters_frozen'] is False and not audit['validation_used'] + with pytest.raises(ValueError, match='source_and_training_only'): + load_training_initialization(tmp_path, config, {'capture': 'different_batch'}) + training['validation_used_for_selection'] = True + (tmp_path/'draft_training.json').write_text(json.dumps(training)) + with pytest.raises(ValueError, match='source_and_training_only'): + load_training_initialization(tmp_path, config, data) + + +@pytest.mark.parametrize('args', [['--offline-draft'], ['--offline-draft', '--offline-raw', 'unused', '--publish-offline'], + ['--draft-initialization-from', 'unused'], ['--draft-upright-splay-zero']]) +def test_draft_cli_cannot_reach_online_or_publication(args, monkeypatch): + from linkerhand_calibration.runtime import runner + monkeypatch.setattr(runner, 'load_product_config', lambda *a, **k: pytest.fail('invalid mode must be rejected first')) + monkeypatch.setattr(runner, 'run_online', lambda *a, **k: pytest.fail('draft must never move hardware')) + with pytest.raises(SystemExit) as caught: + runner.main(args) + assert caught.value.code == 2 + + +@pytest.mark.parametrize('outcome', ['complete', 'partial']) +def test_capture_closes_control_and_never_runs_solver(inputs, tmp_path, monkeypatch, outcome): + import itertools + import rclpy + from linkerhand_calibration.product import load_product_config + from linkerhand_calibration import hikrobot_camera + from linkerhand_calibration.runtime import runner, runner_support + from linkerhand_calibration.runtime import raw_completion + from linkerhand_calibration.runtime.artifacts import raw_joint_draft as draft + + package, _, _, _ = inputs + config = load_product_config(package/'config/o30_right_product.yaml', check_can=False) + session = tmp_path/'session' + session.mkdir() + events, context = [], [False] + def init(**kwargs): + context[0] = True + def shutdown(): + context[0] = False + events.append('context_closed') + monkeypatch.setattr(rclpy, 'ok', lambda: context[0]) + monkeypatch.setattr(rclpy, 'init', init) + monkeypatch.setattr(rclpy, 'shutdown', shutdown) + monkeypatch.setattr(rclpy, 'spin_once', lambda *a, **k: None) + monkeypatch.setattr(hikrobot_camera, 'configure_fastdds_large_image_transport', lambda: None) + monkeypatch.setattr(runner.time, 'monotonic', lambda: next(ticks)) + ticks = itertools.count(0., 2.) + monitor = SimpleNamespace(status_topic='/draft_test/status', count_publishers=lambda _: 0, + destroy_node=lambda: events.append('monitor_closed')) + monkeypatch.setattr(runner_support, 'CalibrationMonitor', lambda **kwargs: monitor) + monkeypatch.setattr(runner_support, 'create_session_directory', lambda _: session) + monkeypatch.setattr(runner_support, 'launch_command', lambda *a, **k: ['mock_control_stack']) + def start(*args, **kwargs): + events.append('stack_started') + return object() + monkeypatch.setattr(runner.subprocess, 'Popen', start) + monkeypatch.setattr(runner_support, 'stop_stack', lambda _: events.append('stack_closed')) + monkeypatch.setattr(runner, 'drive_session', lambda *a, **k: {'state': 'CAPTURE_COMPLETE'}) + def verify(_): + assert events == ['stack_started', 'stack_closed', 'monitor_closed', 'context_closed'] + return {'raw_capture_complete': outcome != 'partial'} + monkeypatch.setattr(raw_completion, 'verify_capture_manifest', verify) + def export(*args, **kwargs): + pytest.fail('capture must not invoke the offline solver') + monkeypatch.setattr(draft, 'replay_draft', export) + result = runner.run_online(config, allow_resume=False) + assert result == (0 if outcome == 'complete' else 3) + assert events.count('stack_started') == 1 + assert events[-1] == 'context_closed' + report = json.loads((session/'capture_result.json').read_text()) + assert file_digest(session/'capture_profile.yaml') == config.profile_config_sha256 + assert report['capture_finished'] and report['control_closed'] + assert not report['accuracy_passed'] and not report['automatic_recapture'] + assert not report['fit_started'] + + +@pytest.mark.parametrize('cycle', [0, 1]) +def test_actual_single_training_cycle_exports_only_as_uncertified_draft(inputs, cycle): + from linkerhand_calibration.core.urdf.limits import validate_range_evidence + _, profile, source, model = inputs + payload = payload_for(inputs) + row = payload['motion_range_evidence']['thumb_mcp'] + row['training_cycles'] = row['command_training_cycles'] = [cycle] + plan, _ = read_draft_plan(payload, source, profile) + evidence = plan.range_evidence['thumb_mcp'] + assert evidence.training_cycles == (cycle,) + with pytest.raises(ValueError, match='invalid_training_motion_range_evidence'): + validate_range_evidence('thumb_mcp', model.joints['thumb_mcp'], -.03, (0., .85), evidence) + row['training_cycles'] = row['command_training_cycles'] = [3] + with pytest.raises(ValueError, match='invalid_training_motion_range_evidence'): + read_draft_plan(payload, source, profile) + + +def test_confirmed_front_tags_follow_mcp_and_pip_without_rewriting_capture(inputs, tmp_path): + from dataclasses import replace + from linkerhand_calibration.profiles.observations import compile_tag_links, compile_tag_feedback_channels + package, profile, _, model = inputs + original = tmp_path/'captured.yaml' + original.write_bytes((package/'config/profiles/o30_right_18.yaml').read_bytes()) + captured_hash = file_digest(original) + links = {finger+'_mcp_roll': model.joints[finger+'_pip'].child + for finger in ('pinky', 'ring', 'middle', 'index')} + corrected = replace(profile, vision=replace(profile.vision, views=tuple( + replace(view, tags=tuple(replace(tag, link=links.get(tag.role, tag.link)) for tag in view.tags)) + for view in profile.vision.views))) + captured = capture_profile_for_draft(corrected, {'profile_config_sha256': captured_hash}, original) + assert captured == profile and file_digest(original) == captured_hash + assert corrected.command == captured.command and corrected.motion == captured.motion + channels = compile_tag_feedback_channels(corrected, model) + for role, link in links.items(): + assert compile_tag_links(corrected, model)[role] == link + pitch = role.replace('_roll', '_pitch') + assert channels[role] == {profile.command.command_index_by_joint[role], + profile.command.command_index_by_joint[pitch], + profile.command.command_index_by_joint[role.replace('_mcp_roll', '_pip')]} + + +@pytest.mark.parametrize('confidence_deg, expected', [(2., False), (.1, True)]) +def test_raw_holdout_verdict_is_json_boolean(monkeypatch, confidence_deg, expected): + from linkerhand_calibration.core.fitting import raw_joint_validation as validation + name = 'thumb_cmc_yaw' + bundle = SimpleNamespace(joints=(name,), cad_names={name: name}, + model=SimpleNamespace(joints={name: SimpleNamespace(child='thumb')}), + profile=SimpleNamespace(acquisition=SimpleNamespace(minimum_valid_samples=2), + measurement=SimpleNamespace(measurements={name: SimpleNamespace(view='top')}))) + poses = {'thumb': np.repeat(np.eye(4)[None], 2, axis=0)} + monkeypatch.setattr(validation, '_groups', lambda *_: {('top', (name,), ('tag',)): [{}, {}]}) + monkeypatch.setattr(validation, '_measure_group', lambda *_: ( + np.zeros((2, 1)), np.zeros((1, 2, 8)), np.zeros((1, 2, 8)), + np.full((2, 1), np.radians(confidence_deg)), poses, poses)) + result = validation.validate_raw_joint_candidate(bundle, SimpleNamespace(parameters=[]), []) + assert result['passed'] is expected + assert json.loads(json.dumps(result))['passed'] is expected + bundle.joints = (name, 'index_pip') + partial = validation.validate_raw_joint_candidate(bundle, SimpleNamespace(parameters=[]), [], + report_joints=(name,)) + assert set(partial['joints']) == {name} and partial['passed'] is expected + with pytest.raises(ValueError, match='holdout_measurements_missing:index_pip'): + validation.validate_raw_joint_candidate(bundle, SimpleNamespace(parameters=[]), []) + with pytest.raises(ValueError, match='report_scope_invalid'): + validation.validate_raw_joint_candidate(bundle, SimpleNamespace(parameters=[]), [], + report_joints=('unknown',)) diff --git a/src/linkerhand_calibration/test/test_raw_failure_regression.py b/src/linkerhand_calibration/test/test_raw_failure_regression.py deleted file mode 100644 index dcc8f37..0000000 --- a/src/linkerhand_calibration/test/test_raw_failure_regression.py +++ /dev/null @@ -1,28 +0,0 @@ -"""Historical failures must remain data, not online geometry authorization.""" -import gzip -import json -from pathlib import Path - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.observation_coverage import visible_roles -from linkerhand_calibration.runtime.raw_quality import evaluate_raw_unit -from linkerhand_calibration.runtime.engine import CalibrationEngine - - -def test_actual_id5_id9_id12_ambiguity_and_id10_missing_images(): - path = Path(__file__).parent/'fixtures/o30_raw_failure_observations.json.gz' - with gzip.open(path, 'rt') as stream: - fixture = json.load(stream) - profile = load_bundled_hand_profile('o30_right_18') - declarations = {v.name: {t.role: t for t in v.tags} for v in profile.vision.views} - for tag_id, row in zip((5, 9, 12), fixture['ambiguous']): - role = next(k for k, value in row['tags'].items() if value['tag_id'] == tag_id) - assert len(row['tags'][role]['reprojection_valid_candidates']) == 2 - assert role in visible_roles(row, declarations[row['view']]) - assert fixture['missing_id10_total'] > 0 - for row in fixture['missing_id10']: - assert 'index_pip' not in visible_roles(row, declarations[row['view']]) - unit = next(u for u in CalibrationEngine(profile).scan_units() if u.task_key == 'index_mcp_pitch_side') - # Old preparation observations are not promoted to a new protocol scan. - quality = evaluate_raw_unit(profile, unit, 1, fixture['missing_id10'], first_cycle_spans={}) - assert not quality.passed diff --git a/src/linkerhand_calibration/test/test_raw_joint_candidate_gates.py b/src/linkerhand_calibration/test/test_raw_joint_candidate_gates.py deleted file mode 100644 index dcc2d3c..0000000 --- a/src/linkerhand_calibration/test/test_raw_joint_candidate_gates.py +++ /dev/null @@ -1,38 +0,0 @@ -from dataclasses import replace -from pathlib import Path - -import numpy as np - -from test_urdf_command_images import graph -from linkerhand_calibration.core.fitting.raw_joint_fit import RawJointCandidate, certify_candidate_union -from linkerhand_calibration.core.fitting.raw_joint_selection import reject_training_distinguishable_candidates - - -def test_candidate_union_rejects_physical_difference_without_demanding_unique_mount(graph): - bundle, truth = graph - original = RawJointCandidate((0,)*6, truth, np.eye(len(truth))*1e-14, 0.) - installed = truth.copy() - column = next(iter(bundle.mount_columns.values())) - installed[column:column+3] += [.1,.2,.3] - accepted, _ = certify_candidate_union(bundle,(original,replace(original,parameters=installed))) - assert accepted['passed'] - changed = truth.copy() - changed[bundle.zero_columns['thumb_cmc_yaw']] += np.radians(5) - rejected, _ = certify_candidate_union(bundle,(original,replace(original,parameters=changed))) - assert not rejected['passed'] and 'zero_rad' in rejected['failures'] - - -def test_training_loss_rejection_keeps_all_indistinguishable_seeds(graph): - bundle, _ = graph - path = Path(__file__).parent/'fixtures/o30_training_candidate_parameters.npz' - stored = np.load(path) - candidates = tuple(RawJointCandidate((i,)*6,x,np.eye(len(x))*1e-14,float(rms)) - for i,(x,rms) in enumerate(zip(stored['parameters'],stored['rms']))) - # Exact numerical duplicates are still retained, not ranked for release. - duplicate = replace(candidates[0],seed=(2,)*6) - retained, audit = reject_training_distinguishable_candidates(bundle,(*candidates,duplicate)) - assert [c.seed for c in retained] == [candidates[0].seed,duplicate.seed] - assert audit['excluded_seeds'] == [list(candidates[1].seed)] - assert audit['familywise_alpha'] == .01 - assert any(r['holm_probability'] < .01 for r in audit['comparisons']) - assert not any(row['cycle'] == 3 for row in bundle.rows) diff --git a/src/linkerhand_calibration/test/test_raw_joint_capture.py b/src/linkerhand_calibration/test/test_raw_joint_capture.py deleted file mode 100644 index 3ad70f6..0000000 --- a/src/linkerhand_calibration/test/test_raw_joint_capture.py +++ /dev/null @@ -1,227 +0,0 @@ -"""The deferred protocol captures evidence without authorizing geometry.""" -from itertools import groupby - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import ( - RAW_JOINT, CapturePlan, configure_training, select_task_training, -) -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.engine import CalibrationEngine, SweepQuality -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase - - -@pytest.fixture -def profile(): - return configure_training(load_bundled_hand_profile('o30_right_18'), RAW_JOINT) - - -def test_raw_protocol_has_separate_identity_and_three_visits(profile): - units = CalibrationEngine(profile).scan_units() - assert len(units) == 108 - assert [key for key, _ in groupby(units, key=lambda u: (u.task_key, u.cycle))] == [ - (task.key, cycle) for task in profile.motion.tasks for cycle in (0, 1, 3)] - assert CapturePlan.from_profile(profile).as_dict()['version'] == 'capture_plan_v6_raw_joint' - with pytest.raises(ValueError, match='cannot_add_training'): - select_task_training(profile, profile.motion.tasks[0].key, (0, 1, 2)) - - -def test_no_task_requires_zero_model_or_auxiliary_preparation(profile): - execution = SessionExecution(profile) - current = profile.command.baseline_values - for index, unit in enumerate(execution.session.scheduled_units): - execution.session._unit_index = index - execution.session.phase = Phase.PREPARE - effects = execution._make_effects(current) - assert not any(effect.phase in {'joint_zero', 'zero_approach', - 'observer_preflight', 'observer_probe'} for effect in effects) - assert not execution.zero_references - current = effects[-1].target if effects else current - - -def test_failed_unit_gets_one_tail_retry_and_returns_before_failure(profile): - session = CalibrationSession(profile) - first, second = session.scheduled_units[:2] - bad = SweepQuality(False, ('missing_images',), (), {}) - good = SweepQuality(True, (), (), {}) - session.phase = Phase.EVALUATE - session.evaluation_complete(bad) - assert session.current_unit == second - assert session.scheduled_units[-1] == first - assert session.status().task.cycle_count == 3 - assert session.status().resume['pending_unit_recaptures'] == [list(first.identity)] - while session.current_unit != first: - session.phase = Phase.EVALUATE - session.evaluation_complete(good) - assert session.action().retry == 1 - session.phase = Phase.EVALUATE - session.evaluation_complete(bad) - assert session.phase == Phase.RETURN_BASELINE - session.return_complete() - assert session.phase == Phase.FAILED - assert first.identity in session.incomplete_units - assert str(first.identity) in session.status().resume['incomplete_units'] - - -def test_real_coordinator_collects_raw_chain_without_any_online_fit(tmp_path, monkeypatch): - import json - import numpy as np - from runtime_host_fixture import coordinator_fixture, ready - from test_chain_capture_runtime import chain_frame - from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection - from linkerhand_calibration.runtime.resume_storage import load_passed_capture - - host, clock = coordinator_fixture(tmp_path, model='o30', - profile_transform=lambda p: configure_training(p, RAW_JOINT)) - session = host.execution.session - session._units = [u for u in session.scheduled_units if u.task_key in { - 'pinky_mcp_pitch_side', 'pinky_pip_side', 'pinky_dip_side'}] - ready(host, clock) - for view in host.profile.vision.view_names: - host.cameras.matrices[view] = np.array([[1400., 0., 320.], [0., 1400., 240.], [0., 0., 1.]]) - monkeypatch.setattr(host.branch_initialization, 'request', lambda *a, **kw: - pytest.fail('raw acquisition must not request an online model')) - monkeypatch.setattr(host.finalization, 'start', lambda *a, **kw: - pytest.fail('test stops at the raw acquisition boundary')) - # The measured optical gate is tested separately with projected board data. - monkeypatch.setattr(host, '_verify_optical_preflight', lambda: None) - assert host.start().success - try: - for _ in range(18000): - if session.phase in {Phase.FIT, Phase.FAILED, Phase.PAUSED}: - 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)) - for view in host.profile.vision.views: - if view.name == 'side': - continue - base = next(t for t in view.tags if t.fixed_reference) - host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), - (TagDetection(base.tag_id, ((280., 280.), (360., 280.), - (360., 200.), (280., 200.)), 0, 100.),))) - assert session.phase == Phase.FIT, (host.reason, session.incomplete_units) - assert len(host.execution.completed_units) == 18 - assert not host.execution.zero_references - assert tuple(clock.positions[-1]) == host.profile.command.baseline_values - for channel in (10, 14, 19): - commands = np.asarray([host.profile.command.baseline_values, *clock.positions])[:, 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.] - rows = list(load_passed_capture(host.raw_path)) - assert not any(r.get('kind') in {'joint_sample', 'joint_zero_sample', - 'motion_branch_initialization', 'diagnostic_observation_frame'} for r in rows) - assert {r['cycle'] for r in rows if r.get('kind') == 'raw_observation_frame' - and r['sample_phase'] == 'steady'} == {0, 1, 3} - from linkerhand_calibration.runtime.raw_evidence import partition_raw_capture - evidence = partition_raw_capture(host.profile, rows, require_complete=False) - assert len(evidence.completed_units) == 18 - assert {r['cycle'] for r in evidence.training} == {0, 1} - assert {r['cycle'] for r in evidence.validation} == {3} - with pytest.raises(TypeError): - evidence.training[0]['cycle'] = 3 - with pytest.raises(ValueError, match='raw_capture_incomplete'): - partition_raw_capture(host.profile, rows) - with pytest.raises(ValueError, match='mixed_authorization_protocol'): - partition_raw_capture(host.profile, [*rows, {'kind': 'joint_sample'}]) - image = next(r for r in rows if r.get('kind') == 'raw_observation_frame') - with pytest.raises(ValueError, match='image_identity_invalid'): - partition_raw_capture(host.profile, [*rows, image], require_complete=False) - # A later interrupted retry cannot resurrect the older passed attempt. - interrupted = {**image, 'attempt': 2, - 'image_stamp_ns': max(r.get('image_stamp_ns', 0) for r in rows) + 1} - partial = partition_raw_capture(host.profile, [*rows, interrupted], require_complete=False) - assert len(partial.completed_units) == 17 - assert all(r['task_name'] != image['task_name'] or r['cycle'] != image['cycle'] - or r['direction'] != image['direction'] for r in partial.training) - finally: - host.close() - - -@pytest.mark.parametrize('command_ns,state_ms', [ - (None, 0.), (0, None), (-1, 0.), (0, -1.), (50_000_001, 0.), - (0, 50.001), (True, 0.), (0, float('nan')), -]) -def test_raw_replay_excludes_unsynchronized_images(command_ns, state_ms): - from linkerhand_calibration.runtime.raw_quality import synchronized_frame - assert not synchronized_frame(dict(command_image_skew_ns=command_ns, - state_image_sync_error_ms=state_ms)) - - -@pytest.mark.parametrize('fixed_reference', [False, True]) -def test_raw_branch_conflict_preserves_only_reference_stop(tmp_path, fixed_reference, monkeypatch): - from runtime_host_fixture import coordinator_fixture, ready - from test_runtime_pose_branch_guard import branch_event - host, clock = coordinator_fixture(tmp_path, model='o30', - profile_transform=lambda p: configure_training(p, RAW_JOINT)) - try: - ready(host, clock) - monkeypatch.setattr(host, '_verify_optical_preflight', lambda: None) - assert host.start().success - session = host.execution.session - session.phase = Phase.SWEEP - task = next(t for t in host.profile.motion.tasks if t.key == session.current_unit.task_key) - spec = host.profile.measurement.measurements[task.joints[0]] - view = next(v for v in host.profile.vision.views if v.name == spec.view) - tag = next(t for t in view.tags if t.fixed_reference) if fixed_reference else next( - t for t in view.tags if t.role == spec.child_role) - row = branch_event(host.ports.clock_ns(), view=view.name, role=tag.role, tag=tag.tag_id) - assert host._accept_branch_event(row) == fixed_reference - assert (session.phase == Phase.PAUSED) == fixed_reference - finally: - host.close() - - -def test_raw_start_requires_measured_optical_check_before_any_motion(tmp_path): - from runtime_host_fixture import coordinator_fixture, ready - host, clock = coordinator_fixture(tmp_path, model='o30') - try: - ready(host, clock) - result = host.start() - assert not result.success - assert 'missing_measured_optical_observations' in host.reason - assert not clock.positions and not clock.settings - finally: - host.close() - - -def test_full_hand_motion_effects_have_three_formal_returns_and_no_preparation_lap(profile): - import numpy as np - driver = SessionExecution(profile) - session = driver.session - session.device_ready(); session.start() - current = profile.command.baseline_values - motions = [] - for _ in range(6000): - if session.phase == Phase.FIT: - break - if session.phase == Phase.REFERENCE_LOCKING: - session.reference_locked(); continue - if session.phase == Phase.EVALUATE: - session.evaluation_complete(SweepQuality(True, (), (), {})); continue - motion = driver.motion(current) - assert motion is not None, session.phase - motions.append(motion) - current = motion.target - driver.motion_complete() - assert session.phase == Phase.FIT - assert current == profile.command.baseline_values - assert not any(m.phase in {'zero_approach','joint_zero','observer_probe','mapping_probe'} for m in motions) - for task in profile.motion.tasks: - scans = [m for m in motions if m.task_key == task.key and m.phase == 'steady'] - assert {m.cycle for m in scans} == {0,1,3} - for cycle in (0,1,3): - assert {m.direction for m in scans if m.cycle == cycle} == {'increasing','decreasing'} - # Avoidance and entry commands remain visible in this same ledger. - if not task.segments: - channel = task.command_index - values = np.asarray([m.target[channel] for m in scans]) - changed = values[np.r_[True, np.diff(values) != 0]] - assert np.count_nonzero(np.diff(np.sign(np.diff(changed)))) == 5 diff --git a/src/linkerhand_calibration/test/test_raw_joint_release.py b/src/linkerhand_calibration/test/test_raw_joint_release.py deleted file mode 100644 index ffdbd1f..0000000 --- a/src/linkerhand_calibration/test/test_raw_joint_release.py +++ /dev/null @@ -1,91 +0,0 @@ -"""Release gates use independent images and read the actual serialized files.""" -import hashlib -from dataclasses import replace -from types import SimpleNamespace - -import numpy as np -import pytest - -from test_urdf_command_images import graph -from raw_image_fixture import raw_image_rows -from linkerhand_calibration.core.fitting.raw_joint_fit import RawJointCandidate, RawJointFit -from linkerhand_calibration.core.fitting.raw_joint_validation import validate_raw_joint_candidate -from linkerhand_calibration.core.artifacts.storage import atomic_write_json -from linkerhand_calibration.runtime.artifacts.raw_joint_report import prepare_raw_report -from linkerhand_calibration.runtime.artifacts.raw_joint_validator import RawJointArtifactValidator -from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import from_report -from linkerhand_calibration.runtime.artifacts.serializers.compact_v2 import REPORT_FILENAME -from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json -from linkerhand_calibration.runtime.raw_evidence import RawCalibrationEvidence - - -@pytest.fixture(scope='module') -def frozen_validation(graph): - bundle, truth = graph - candidate = RawJointCandidate((0,)*6, truth, np.eye(len(truth))*1e-12, 0.) - rows = raw_image_rows(cycles=(3,)) - result = validate_raw_joint_candidate(bundle, candidate, rows) - return candidate, rows, result - - -def test_independent_holdout_accepts_truth_and_does_not_update_it(graph, frozen_validation): - bundle, truth = graph - candidate, rows, result = frozen_validation - assert result['passed'], result - assert result['shared_parameters_updated'] is False - np.testing.assert_array_equal(candidate.parameters, truth) - with pytest.raises(ValueError, match='not_independent'): - validate_raw_joint_candidate(bundle, candidate, (*rows, bundle.rows[0])) - - -def test_wrong_zero_fails_frozen_holdout(graph, frozen_validation): - bundle, truth = graph - candidate, rows, _ = frozen_validation - wrong = truth.copy(); wrong[bundle.zero_columns['thumb_cmc_yaw']] += np.radians(5) - with pytest.raises(ValueError, match='holdout_angle_measurement_failed'): - validate_raw_joint_candidate(bundle, replace(candidate, parameters=wrong), rows) - - -def test_report_rebuild_and_final_file_fk(graph, frozen_validation, tmp_path): - bundle, truth = graph - candidate, rows, validation = frozen_validation - fit = RawJointFit((candidate,), 0, dict(passed=True), ()) - evidence = RawCalibrationEvidence(bundle.rows, rows, (), 'a'*64) - protected = dict(source_urdf_sha256=hashlib.sha256(bundle.model.source.read_bytes()).hexdigest()) - report, plan = prepare_raw_report(bundle, fit, evidence, validation, - serial_number='OFFLINE_TRUTH', protected_inputs=protected) - json_path, urdf_path = tmp_path/'calibration.json', tmp_path/'corrected.urdf' - atomic_write_json(json_path, from_report(report)) - atomic_write_json(tmp_path/REPORT_FILENAME, report) - rebuild_urdf_from_json(json_path, bundle.model.source, urdf_path, - authorized_fields=bundle.profile.urdf_authorized_fields, expected_profile_id=bundle.profile.key.profile_id) - import xml.etree.ElementTree as ET - validator = RawJointArtifactValidator(bundle, candidate, report, plan, rows, validation, - dict(passed=True, scope='fixture_only'), ET.parse) - result = validator(json_path, urdf_path) - assert result['final_file_fk_maximum_difference'] < 1e-8 - altered = from_report(report) - altered['joints']['thumb_cmc_yaw']['angle_rad'][10] += .1 - atomic_write_json(json_path, altered) - with pytest.raises(ValueError, match='serialized_report_changed'): - validator(json_path, urdf_path) - - -def test_formal_finalizer_rejects_missing_provenance_before_fitting(graph, tmp_path, monkeypatch): - import json - from linkerhand_calibration.runtime.artifacts import raw_joint_finalization as finalization - - bundle, _ = graph - monkeypatch.setattr(finalization, 'fit_raw_joint_candidates', - lambda *args, **kwargs: pytest.fail('unverified capture reached the solver')) - protected = {key: 'a'*64 for key in bundle.profile.artifacts.protected_input_fields} - directory = tmp_path/'incomplete' - with pytest.raises(ValueError, match='requires_unique_session_and_locked_reference'): - finalization.finalize_raw_joint_session(profile=bundle.profile, session_dir=directory, - serial_number='OFFLINE_TRUTH', source_urdf=bundle.model.source, - protected_inputs=protected, records=(), camera_extrinsics_file=tmp_path/'missing.json', - standard_loader=lambda _: pytest.fail('invalid capture reached artifact loading')) - state = json.loads((directory/'raw_joint_result.json').read_text()) - assert not any(state[key] for key in ('raw_capture_complete', 'fit_complete', - 'accuracy_passed', 'publication_allowed', 'automatic_geometry_recapture')) - assert not (tmp_path/bundle.profile.artifacts.publication_pointer).exists() diff --git a/src/linkerhand_calibration/test/test_raw_joint_update.py b/src/linkerhand_calibration/test/test_raw_joint_update.py new file mode 100644 index 0000000..cf62605 --- /dev/null +++ b/src/linkerhand_calibration/test/test_raw_joint_update.py @@ -0,0 +1,64 @@ +"""Frozen references must remain immutable when only one chain is refitted.""" +from types import SimpleNamespace + +import numpy as np +import pytest +from scipy.sparse import csr_matrix + +from linkerhand_calibration.core.fitting.raw_joint_update import ( + check_update_zero_observability, fit_joint_update, transfer_parameters, +) + + +def test_update_freezes_reference_and_unobserved_travel(): + # Coupling to a frozen reference makes accidentally freeing it observable. + matrix = np.zeros((12, 10)) + matrix[:3, 6] = 1. + matrix[3:6, 8] = 1. + matrix[6:9, 9] = 1. + matrix[9:, 6] = matrix[9:, 9] = 1. + initial = np.zeros(10) + initial[7], initial[9] = .7, .4 + target = initial.copy() + target[6], target[8] = .9, .03 + bundle = SimpleNamespace(joints=('thumb', 'finger'), tags={}, + zero_columns={'thumb': 8}, curve_columns={'thumb': np.array([[6, 7]])}, + scale=np.ones(10), solver_options={'bounds': (np.full(10, -2.), np.full(10, 2.))}, + solver_ftol=1e-8, jacobian=lambda _: matrix, + solver_jacobian=lambda _: csr_matrix(matrix), + residual=lambda p: matrix @ (p-target), original_residual=lambda p: matrix @ (p-target)) + result, audit = fit_joint_update(bundle, initial, ('thumb',), (), fit_palm=False) + assert audit['converged'] + assert np.allclose(result, target, atol=1e-7) + assert np.array_equal(result[[7, 9]], initial[[7, 9]]) + assert audit['unobserved_retained_columns'] == [7] + with pytest.raises(ValueError, match='scope_invalid'): + fit_joint_update(bundle, initial, ('missing',), ()) + + +def test_transfer_uses_names_and_rejects_different_mount_link(): + source = SimpleNamespace(knots={'joint': np.array([0, 255])}, + curve_columns={'joint': np.array([[-1, 6], [-1, 7]])}, + zero_offsets=lambda p: {'joint': p[8]}, mount_columns={'tag': 9}, + tag_links={'tag': 'middle'}) + target = SimpleNamespace(scale=np.ones(15), knots=source.knots, + curve_columns={'joint': np.array([[-1, 14], [-1, 13]])}, + zero_columns={'joint': 12}, mount_columns={'tag': 6}, tag_links={'tag': 'middle'}) + parameters = np.arange(15, dtype=float) + result = transfer_parameters(source, parameters, target) + assert np.array_equal(result[:6], parameters[:6]) + assert np.array_equal(result[6:12], parameters[9:15]) + assert np.array_equal(result[12:15], [8, 7, 6]) + target.tag_links['tag'] = 'proximal' + with pytest.raises(ValueError, match='tag_link_changed'): + transfer_parameters(source, parameters, target) + + +def test_update_rejects_zero_absorbed_by_free_palm(): + # Both columns move the image identically: a low image residual cannot + # establish whether the palm or the joint zero changed. + bundle = SimpleNamespace(jacobian=lambda _: np.ones((8, 2)), + scale=np.ones(2), zero_columns={'thumb': 1}) + with pytest.raises(ValueError, match='zero_unobservable:thumb'): + check_update_zero_observability(bundle, np.zeros(2), np.array([0, 1]), ('thumb',)) + assert check_update_zero_observability(bundle, np.zeros(2), np.array([1]), ('thumb',)) == 1 diff --git a/src/linkerhand_calibration/test/test_raw_lightweight.py b/src/linkerhand_calibration/test/test_raw_lightweight.py new file mode 100644 index 0000000..cfb8707 --- /dev/null +++ b/src/linkerhand_calibration/test/test_raw_lightweight.py @@ -0,0 +1,581 @@ +"""Focused regression for raw corners, local retries and a sealed acquisition.""" +from dataclasses import replace +import json +from types import SimpleNamespace + +import numpy as np +import pytest + +from runtime_host_fixture import coordinator_fixture, ready +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.domain.capture_plan import CapturePlan +from linkerhand_calibration.runtime.engine import CalibrationEngine, SweepQuality +from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution +from linkerhand_calibration.runtime.direction_progress import DirectionProgress +from linkerhand_calibration.runtime.capture import CaptureFrame +from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection +from linkerhand_calibration.runtime.raw_resume import RawCheckpoint +from linkerhand_calibration.runtime.raw_completion import verify_capture_manifest +from linkerhand_calibration.runtime.observation_coverage import visible_roles + + +@pytest.fixture +def profile(): + return load_bundled_hand_profile('o30_right_18') + + +def test_immediate_retry_and_full_schedule_end_at_capture_boundary(profile): + session = CalibrationSession(profile) + units = session.scheduled_units + assert len(units) == 108 + assert CapturePlan.from_profile(profile).as_dict()['observation_protocol']['recapture_order'] == 'immediate' + bad, good = SweepQuality(False, ('missing_images',), (), {}), SweepQuality(True, (), (), {}) + session.phase = Phase.EVALUATE + session.evaluation_complete(bad) + assert session.current_unit == units[0] and session.action().retry == 1 + assert session.scheduled_units == units + session.phase = Phase.EVALUATE + session.evaluation_complete(bad) + assert session.current_unit == units[1] + while session.current_unit: + session.phase = Phase.EVALUATE + session.evaluation_complete(good) + assert session.phase == Phase.RETURN_BASELINE + session.return_complete() + assert session.phase == Phase.CAPTURE_COMPLETE + assert units[0].identity in session.incomplete_units + + +def test_interrupted_second_attempt_cannot_obtain_a_third(profile): + session = CalibrationSession(profile) + unit = session.current_unit + base = dict(task_name=unit.task_key, cycle=unit.cycle, direction=unit.direction) + rows = ({**base, 'kind': 'scan_unit_complete', 'attempt': 1, 'passed': False}, + {**base, 'kind': 'raw_observation_frame', 'attempt': 2}) + checkpoint = RawCheckpoint({}, {}, rows, frozenset()) + interrupted = checkpoint.restore(session) + assert interrupted[0]['attempt'] == 2 + assert session.current_unit != unit + assert unit.identity in session.incomplete_units + assert session._retry_by_unit[unit.identity] == 1 + + +def test_short_segment_encoder_quantization_keeps_native_density(profile): + from linkerhand_calibration.runtime.scan_quality import _feedback_motion_coverage + # A physical 0–72 byte traversal at native ~3-byte steps is dense for an + # 0–80 command segment, but not for a claimed full-range traversal. + values = tuple(value for value in range(0, 73, 3) for _ in range(3)) + full, _ = _feedback_motion_coverage(profile, values) + short, metrics = _feedback_motion_coverage(profile, values, command_span_fraction=80/255) + assert not full.passed and short.passed + assert metrics['observed_span'] == 72 and metrics['required_bins'] == 11 + for stationary in ([0.]*80, [0., 1.]*40, [0., 4.]*40): + invalid, _ = _feedback_motion_coverage(profile, stationary, command_span_fraction=80/255) + assert not invalid.passed + + +def test_raw_frame_tracks_only_fixed_tag_and_keeps_moving_corners(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path, model='o30') + try: + view = host.profile.vision.views[0] + fixed = next(t for t in view.tags if t.fixed_reference) + calls = [] + tracker = host.trackers[view.name] + original = tracker.estimate + def estimate(role, *args, **kwargs): + assert role == fixed.role + calls.append(role) + return original(role, *args, **kwargs) + monkeypatch.setattr(tracker, 'estimate', estimate) + monkeypatch.setattr(host.capture, '_motion_model_schedule', lambda *a: pytest.fail('online model')) + points = np.array([[280.,280.],[360.,280.],[360.,200.],[280.,200.]]) + frame = CaptureFrame(view.name, host.ports.clock_ns(), np.array([[1400.,0.,320.],[0.,1400.,240.],[0.,0.,1.]]), + {t.role: points for t in view.tags}, host.profile.command.baseline_values, + host.profile.command.baseline_values, command_skew_ns=0, + detection_quality={t.role: {'decision_margin': 100.} for t in view.tags}) + unit = host.execution.session.current_unit + motion = MotionCommand('steady', tuple(frame.command), 200., task_key=unit.task_key, + command_index=0, cycle=0, direction='increasing', steady_index=0) + events, _ = host.capture.consume(frame, motion, raw_joint=True) + row = next(r for r in events if r['kind'] == 'raw_observation_frame') + assert visible_roles(row, {t.role:t for t in view.tags}) == {t.role for t in view.tags} + assert all('candidate_diagnostics' not in item and 'selected_pose' not in item for item in row['tags'].values()) + assert row['observation_protocol'] == 'raw_joint_images_v2' + assert not any(r.get('kind') == 'joint_sample' for r in events) + assert host.capture.consume(frame, motion, raw_joint=True) == ((), None) + finally: + host.close() + + +def run_direction(profile, feedback_fn): + driver = SessionExecution(profile) + driver.session.phase = Phase.SWEEP + unit = driver.session.current_unit + from linkerhand_calibration.runtime.trajectory import build_calibration_motion_command + task = next(t for t in profile.motion.tasks if t.key == unit.task_key) + current = build_calibration_motion_command(task, unit.start, profile=profile) + feedback = current.copy() + feedback[0] = feedback_fn(current[0]) + guard = DirectionProgress(profile, unit, 1, feedback, 0.) + effects = driver._make_effects(current) + now, stamp = 0., 0 + for motion in effects: + execution = MotionExecution(profile, motion, initial_command=current, + initial_feedback=feedback, now=now, identity=str(stamp), raw_scan=True) + for _ in range(200): + now += .05; stamp += 1 + current = execution.sample(now) + feedback = list(current) + feedback[0] = feedback_fn(current[0]) + execution.observe(feedback, stamp=stamp, now=now) + result = guard.observe(motion, execution, current, feedback, stamp, now) + if not result.safe: + return result, guard + if execution.arrived(now): + break + else: + pytest.fail('unbounded raw waypoint') + return guard.finish(), guard + + +def test_initial_dead_zone_and_endpoint_plateau_complete(profile): + result, guard = run_direction(profile, lambda x: min(195., max(0., 1.25*(x-40.)))) + assert result.safe, result + assert guard.travel[0] >= 190. + + +@pytest.mark.parametrize('feedback_fn', [lambda x: 0., lambda x: min(x, 60.), lambda x: 255.-x, + lambda x: .4 if int(x)%2 else 0., + lambda x: 4. if int(x)%2 else 0., + lambda x: x if x < 60. else 60.+(4. if int(x)%2 else 0.)]) +def test_stationary_blocked_reverse_or_noise_still_stop(profile, feedback_fn): + result, _ = run_direction(profile, feedback_fn) + assert not result.safe and result.code == 'mechanical_stall' + + +def test_known_return_is_not_a_raw_sample(profile): + baseline = profile.command.baseline_values + start = list(baseline); start[0] = 255. + motion = MotionCommand('task_exit', baseline, 200., feedback_targets=((0,3.),)) + execution = MotionExecution(profile, motion, initial_command=start, initial_feedback=start, + now=0., identity='return', raw_scan=True) + assert not execution.raw_scan + for stamp in range(400): + now = stamp*.05 + execution.sample(now) + execution.observe(start, stamp=stamp, now=now) + assert not execution.arrived(now) + + +def test_all_motion_effects_preserve_three_rounds_and_return(profile): + execution = SessionExecution(profile) + session = execution.session + session.device_ready(); session.start() + current = profile.command.baseline_values + scans = [] + for _ in range(6000): + if session.phase == Phase.CAPTURE_COMPLETE: + break + if session.phase == Phase.REFERENCE_LOCKING: + session.reference_locked(); continue + if session.phase == Phase.EVALUATE: + session.evaluation_complete(SweepQuality(True, (), (), {})); continue + motion = execution.motion(current) + assert motion is not None + assert motion.phase not in {'joint_zero', 'observer_probe', 'mapping_probe'} + if motion.phase == 'steady': + scans.append((motion.task_key, motion.cycle, motion.direction)) + current = motion.target + execution.motion_complete() + assert session.phase == Phase.CAPTURE_COMPLETE + assert tuple(current) == profile.command.baseline_values + for task in profile.motion.tasks: + assert {(cycle, direction) for key, cycle, direction in scans if key == task.key} == { + (cycle, direction) for cycle in (0,1,3) for direction in ('increasing','decreasing')} + + +def heartbeat(host, clock, feedback): + 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, feedback, host.ports.clock_ns()) + + +@pytest.mark.parametrize('recover', [False, True]) +def test_real_coordinator_finishes_missing_data_without_online_fit(tmp_path, monkeypatch, recover): + host, clock = coordinator_fixture(tmp_path, model='o30') + try: + ready(host, clock) + host.parameters = replace(host.parameters, reuse_camera_calibration=True) + session = host.execution.session + # One complete direction and its immediate retry, with no moving tags. + session._units = session._units[:1] + from linkerhand_calibration.storage import append_jsonl + for view in host.profile.vision.views: + matrix = np.array([[1400.,0.,320.],[0.,1400.,240.],[0.,0.,1.]]) + host.cameras.matrices[view.name] = matrix + append_jsonl(host.raw_path, dict(kind='rectified_camera_model', view=view.name, + matrix_source='CameraInfo.P[:3,:3]', input_is_rectified=True, camera_matrix=matrix.tolist())) + monkeypatch.setattr(host.finalization, 'start', lambda *a, **kw: pytest.fail('online fit')) + assert host.start().success + for _ in range(2200): + clock.now += .1 + feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values + heartbeat(host, clock, feedback) + host.tick() + for view in host.profile.vision.views: + base = next(t for t in view.tags if t.fixed_reference) + host.cameras.matrices[view.name] = np.array([[1400.,0.,320.],[0.,1400.,240.],[0.,0.,1.]]) + tags = (view.tags if recover and host._motion is not None + and host._motion.attempt == 2 and view.name == 'front' else (base,)) + host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), tuple( + TagDetection(t.tag_id, ((280.,280.),(360.,280.),(360.,200.),(280.,200.)),0,100.) for t in tags))) + if host.state in {'CAPTURE_COMPLETE', 'PAUSED', 'FAILED'}: + break + assert host.state == 'CAPTURE_COMPLETE', host.reason + assert bool(session.incomplete_units) != recover + assert len(host.execution.completed_units) == int(recover) + assert list(session._retry_by_unit.values()) == [1] + assert tuple(clock.positions[-1]) == host.profile.command.baseline_values + manifest = verify_capture_manifest(host.raw_path) + assert manifest['capture_finished'] and not manifest['raw_capture_complete'] + from linkerhand_calibration.runtime.acquisition import load_capture + rows = load_capture(host.raw_path) + assert not any(r.get('kind') == 'image_observation_frame' for r in rows) + assert any(r.get('kind') == 'raw_observation_frame' and r.get('sample_phase') == 'steady' + and not r.get('accepted_sample_window_open') for r in rows) + completions = [r for r in rows if r.get('kind') == 'scan_unit_complete'] + assert [r['attempt'] for r in completions] == [1,2] + assert [r['passed'] for r in completions] == [False,recover] + if recover: + from linkerhand_calibration.runtime.raw_evidence import partition_raw_capture + evidence = partition_raw_capture(host.profile, rows, require_complete=False) + assert len(evidence.completed_units) == 1 + assert {r['attempt'] for r in evidence.training} == {2} + sent = len(clock.positions) + for _ in range(3): + clock.now += .1 + heartbeat(host, clock, feedback) + host.tick() + host._publish_command(list(feedback)) + host._write_position(list(feedback)) + assert len(clock.positions) == sent + host.close() + verify_capture_manifest(host.raw_path) + if not recover: + from pathlib import Path + from linkerhand_calibration.product import load_product_config + from linkerhand_calibration.runtime.artifacts.replay import replay_capture + from linkerhand_calibration.runtime.raw_completion import file_digest + package = Path(__file__).resolve().parents[1] + config = load_product_config(package/'config/o30_right_product.yaml', + workspace=package.parents[1], check_can=False) + config = replace(config, serial_number='OFFLINE_COORDINATOR') + before = file_digest(host.raw_path) + output = tmp_path/'offline_missing' + with pytest.raises(ValueError, match='raw_joint_capture_incomplete'): + replay_capture(config, host.raw_path, output=output, publish=False) + assert file_digest(host.raw_path) == before + verify_capture_manifest(host.raw_path) + assert len(clock.positions) == sent + run = json.loads((output/'offline_run.json').read_text()) + assert run['commands_enabled'] is False + assert len(run['implementation_sha256']) == 64 + finally: + host.close() + + +def test_unstable_raw_endpoint_moves_into_bounded_hold(profile): + start = profile.command.baseline_values + target = list(start); target[0] = 32. + motion = MotionCommand('sweep', tuple(target), 200., direction='increasing') + segment = MotionExecution(profile, motion, initial_command=start, initial_feedback=start, + now=0., identity='endpoint', raw_scan=True) + for i in range(200): + now = i*.05 + segment.sample(now) + values = list(target); values[0] += 8. if i%2 else 0. + segment.observe(values, stamp=i, now=now) + assert segment.arrived(now) + hold = MotionExecution(profile, replace(motion, phase='steady', command_index=0), + initial_command=target, initial_feedback=target, now=now, identity='hold', raw_scan=True) + began = now + for i in range(60): + now = began+i*.05 + hold.sample(now) + values = list(target); values[0] += 8. if i%2 else 0. + hold.observe(values, stamp=i, now=now) + assert not hold.steady_ready(now) + assert hold.arrived(now) + + +def test_camera_latency_preserves_validation_endpoint_coverage(tmp_path): + host, clock = coordinator_fixture(tmp_path, model='o30') + try: + ready(host, clock) + host.parameters = replace(host.parameters, reuse_camera_calibration=True) + session = host.execution.session + # Camera processing plus callback scheduling can cross a phase boundary. + # Keep the original validation nodes and endpoint coverage gates. + session._units = session._units[4:6] + for view in host.profile.vision.views: + host.cameras.matrices[view.name] = np.array([[1400.,0.,320.],[0.,1400.,240.],[0.,0.,1.]]) + assert host.start().success + for _ in range(6500): + clock.now += .05 + feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values + feedback = list(feedback) + if host._motion is not None and host._motion.direction: + feedback[0] = min(240., max(10., feedback[0])) + heartbeat(host, clock, feedback) + host.tick() + for view in host.profile.vision.views: + tags = [t for t in view.tags if t.fixed_reference or view.name == 'front'] + host.receive_detections(DetectionInput(view.name, host.ports.clock_ns()-300_000_000, tuple( + TagDetection(t.tag_id, ((280.,280.),(360.,280.),(360.,200.),(280.,200.)),0,100.) + for t in tags))) + if host.state in {'CAPTURE_COMPLETE', 'PAUSED', 'FAILED'}: + break + assert host.state == 'CAPTURE_COMPLETE', host.reason + assert len(host.execution.completed_units) == 2, session.incomplete_units + assert not session._retry_by_unit + from linkerhand_calibration.runtime.acquisition import load_capture + from linkerhand_calibration.runtime.raw_evidence import partition_raw_capture + rows = load_capture(host.raw_path) + frames = [r for r in rows if r.get('kind') == 'raw_observation_frame'] + assert any(r['command_vector'][0] == 255 and r['feedback_vector'][0] == 240 for r in frames) + completions = [r for r in rows if r.get('kind') == 'scan_unit_complete'] + # Historical false negatives can be rechecked offline, with an audit; + # a recorded interruption still cannot become a completed direction. + for row in completions: + row.update(passed=False, failures=['endpoint_observation']) + conservative = partition_raw_capture(host.profile, rows, require_complete=False) + assert not conservative.completed_units + reassessed = partition_raw_capture(host.profile, rows, require_complete=False, reassess_quality=True) + assert len(reassessed.completed_units) == len(reassessed.reassessed_units) == 2 + completions[0]['failures'] = ['interrupted_attempt'] + interrupted = partition_raw_capture(host.profile, rows, require_complete=False, reassess_quality=True) + assert len(interrupted.completed_units) == 1 + # A configured target cannot stand in for the actual sent command. + for row in frames: + if row['command_vector'][0] == 255: + row['command_vector'][0] = 252. + missing = partition_raw_capture(host.profile, rows, require_complete=False, reassess_quality=True) + assert not missing.completed_units + finally: + host.close() + + +def test_legacy_protocol_is_readable_but_not_resumable(profile): + from linkerhand_calibration.runtime.raw_evidence import partition_raw_capture + legacy = CapturePlan.from_profile(profile).as_dict(legacy_raw=True) + rows = [{'kind': 'session_start', 'capture_plan': legacy}] + result = partition_raw_capture(profile, rows, require_complete=False) + assert result.capture_plan == legacy and len(result.missing_units) == 108 + unit = CalibrationEngine(profile).scan_units()[0] + rows.append(dict(kind='raw_observation_frame', observation_protocol='raw_joint_images_v2', + task_name=unit.task_key, cycle=unit.cycle, direction=unit.direction, attempt=1)) + with pytest.raises(ValueError, match='mixed_observation_protocol'): + partition_raw_capture(profile, rows, require_complete=False) + + +def test_completed_file_changes_are_rejected(tmp_path, profile): + from linkerhand_calibration.runtime.raw_completion import save_capture_manifest + raw = tmp_path/'raw_samples.jsonl'; raw.write_text('{}\n') + save_capture_manifest(raw, profile=profile, serial_number='TEST', protected_inputs={}, + completed_units=set(), incomplete_units={}, retries={}) + verify_capture_manifest(raw) + raw.write_text('{"changed":true}\n') + with pytest.raises(ValueError, match='sealed_file_changed'): + verify_capture_manifest(raw) + + +@pytest.mark.parametrize('fault', ['feedback_stale', 'duplicate_controller', 'sdk_identity']) +def test_raw_scan_keeps_non_motion_protection(tmp_path, fault): + host, clock = coordinator_fixture(tmp_path, model='o30') + try: + ready(host, clock) + host.parameters = replace(host.parameters, reuse_camera_calibration=True) + assert host.start().success + assert host.stall_retry.maximum_failures == 2 + if fault == 'feedback_stale': + clock.now += 1.1 + elif fault == 'sdk_identity': + heartbeat(host, clock, host.profile.command.baseline_values) + clock.health_receiver(json.dumps(dict(hand_type='left', model='O30', side='LEFT', + uid='WRONG_DEVICE', joint_names=list(host.profile.command.names), online=True))) + else: + clock.publishers = 2 + heartbeat(host, clock, host.profile.command.baseline_values) + host.tick() + expected = 'o30_sdk_identity_mismatch' if fault == 'sdk_identity' else fault + assert host.state == 'PAUSED' and expected in host.reason + if fault == 'sdk_identity': + assert 'WRONG_DEVICE' in host.reason + from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh + assert reason_zh({'reason':host.reason}, model_name='O30')[0] == 'DEVICE-IDENTITY-203' + finally: + host.close() + + +def test_full_hand_coordinator_single_view_capture_is_sealed(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path, model='o30') + try: + ready(host, clock) + host.parameters = replace(host.parameters, reuse_camera_calibration=True) + for view in host.profile.vision.views: + host.cameras.matrices[view.name] = np.array([[1400.,0.,320.],[0.,1400.,240.],[0.,0.,1.]]) + monkeypatch.setattr(host.branch_initialization, 'request', lambda *a, **kw: pytest.fail('online geometry')) + monkeypatch.setattr(host.finalization, 'start', lambda *a, **kw: pytest.fail('online fit')) + assert host.start().success + tasks = {task.key: task for task in host.profile.motion.tasks} + for _ in range(65000): + clock.now += .1 + feedback = clock.positions[-1] if clock.positions else host.profile.command.baseline_values + heartbeat(host, clock, feedback) + host.tick() + motion = host._motion + task = tasks.get(motion.task_key) if motion is not None else None + primary = set() if task is None else {host.profile.measurement.measurements[j].view for j in task.joints} + for view in host.profile.vision.views: + tags = [t for t in view.tags if t.fixed_reference or view.name in primary] + host.receive_detections(DetectionInput(view.name, host.ports.clock_ns(), tuple( + TagDetection(t.tag_id, ((280.,280.),(360.,280.),(360.,200.),(280.,200.)), 0, 100.) for t in tags))) + if host.state in {'CAPTURE_COMPLETE', 'PAUSED', 'FAILED'}: + break + assert host.state == 'CAPTURE_COMPLETE', host.reason + assert len(host.execution.completed_units) == 108, host.execution.session.incomplete_units + assert not host.execution.session._retry_by_unit + assert tuple(clock.positions[-1]) == host.profile.command.baseline_values + manifest = verify_capture_manifest(host.raw_path) + assert manifest['raw_capture_complete'] and not manifest['accuracy_passed'] + from linkerhand_calibration.runtime.acquisition import load_capture + from linkerhand_calibration.runtime.raw_evidence import partition_raw_capture + evidence = partition_raw_capture(host.profile, load_capture(host.raw_path)) + assert len(evidence.completed_units) == 108 + assert {r['cycle'] for r in evidence.training} == {0,1} + assert {r['cycle'] for r in evidence.validation} == {3} + finally: + host.close() + + +@pytest.mark.parametrize('legacy', [True, False]) +def test_existing_synthetic_report_rebuilds_same_urdf(tmp_path, profile, legacy): + from pathlib import Path + import xml.etree.ElementTree as ET + from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import from_report + from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json + root = Path(__file__).resolve().parents[3] + fixture = root/'calibration_output/software_review_20260922_raw_joint/known_truth' + report_path = fixture/'calibration_report.json' + if not report_path.exists(): + pytest.skip('local immutable synthetic artifact is unavailable') + report = json.loads(report_path.read_text()) + if not legacy: + report['capture_plan'] = CapturePlan.from_profile(profile).as_dict() + report['raw_joint_audit']['protocol'] = 'raw_joint_images_v2' + payload = from_report(report) + destination = tmp_path/'synthetic.json'; destination.write_text(json.dumps(payload)) + source = root/'src/linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf' + output = tmp_path/'synthetic.urdf' + rebuild_urdf_from_json(destination, source, output, + authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id) + expected = next(fixture.glob('*.urdf')) + def joints(path): + return {j.get('name'): {key: None if j.find(key) is None else j.find(key).attrib + for key in ('origin','axis','limit','parent','child','mimic')} for j in ET.parse(path).findall('joint')} + assert joints(output) == joints(expected) + report['raw_joint_audit']['candidate_uncertainty']['passed'] = False + with pytest.raises(ValueError, match='not_certified'): + from_report(report) + + +def test_runner_recognizes_capture_complete_without_starting_anything(): + from linkerhand_calibration.runtime.runner import drive_session + status = {'state':'CAPTURE_COMPLETE'} + monitor = SimpleNamespace(status=status) + result = drive_session(monitor, None, spin=lambda:None, ok=lambda:True, + request_start=lambda:pytest.fail('must not restart capture')) + assert result == status + + +def test_hold_freezes_budget_and_next_waypoint_cannot_reset_it(profile): + unit = CalibrationEngine(profile).scan_units()[0] + start = profile.command.baseline_values + guard = DirectionProgress(profile, unit, 1, start, 0.) + command = list(start); command[0] = 32. + segment = SimpleNamespace(phase=.5, deltas=[32.]+[0.]*19) + sweep = SimpleNamespace(phase='sweep') + hold = SimpleNamespace(phase='steady') + assert guard.observe(sweep, segment, command, start, 1, 0.).safe + assert guard.observe(hold, segment, command, start, 2, 3.).safe + before = guard.elapsed[0] + assert guard.observe(hold, segment, command, start, 3, 5.).safe + assert guard.elapsed[0] == before == 3. + command[0] = 64. + assert guard.observe(sweep, segment, command, start, 4, 5.).safe + result = guard.observe(sweep, segment, command, start, 5, 6.1) + assert not result.safe and guard.elapsed[0] == pytest.approx(4.1) + + +@pytest.mark.parametrize('outcome', ['still_stuck', 'recovered', 'feedback_stale', 'duplicate_controller']) +def test_raw_hardware_stall_has_one_delayed_retry(tmp_path, outcome): + from linkerhand_calibration.runtime.trajectory import build_calibration_motion_command + host, clock = coordinator_fixture(tmp_path, model='o30') + try: + ready(host, clock) + host.parameters = replace(host.parameters, reuse_camera_calibration=True) + assert host.start().success + session = host.execution.session + session._unit_index = 1 # First thumb roll return, no geometry needed. + session.phase = Phase.SWEEP + unit = session.current_unit + task = host.profile.motion.tasks[0] + start = build_calibration_motion_command(task, unit.start, profile=host.profile) + host.last_command = tuple(start) + feedback = list(start) + fault_sent = False + for _ in range(1000): + if host.state == 'PAUSED': + break + waiting = host.stall_retry.waiting + sent = len(clock.positions) + remaining = host.stall_retry.remaining(clock.now) + clock.now += .05 + if outcome in {'feedback_stale', 'duplicate_controller'} and waiting and not fault_sent: + fault_sent = True + if outcome == 'feedback_stale': + clock.now += 1.1 + host.tick() + continue + clock.publishers = 2 + if outcome == 'recovered' and host.stall_retry.failures and not waiting: + feedback = list(host.last_command) + heartbeat(host, clock, feedback) + host.tick() + if waiting and remaining > .1 and host.state != 'PAUSED': + assert len(clock.positions) == sent + if outcome == 'recovered' and host.stall_retry.failures and not host.stall_retry.waiting: + if host._direction_progress.travel[0] > 20.: + break + from linkerhand_calibration.runtime.acquisition import load_capture + rows = load_capture(host.raw_path) + failures = [r for r in rows if r.get('kind') == 'motion_stall_retry'] + resumes = [r for r in rows if r.get('kind') == 'motion_stall_retry_resumed'] + assert all(r['maximum_failures'] == 2 for r in failures) + if resumes: + assert resumes[0]['motion_id'] == failures[0]['motion_id'] + if outcome == 'still_stuck': + assert host.state == 'PAUSED' and 'consecutive_failures=2' in host.reason + assert [r['failure_count'] for r in failures] == [1,2] + assert len(resumes) == 1 and resumes[0]['waited_seconds'] >= 3. + elif outcome == 'recovered': + assert host.state != 'PAUSED', host.reason + assert len(failures) == len(resumes) == 1 + else: + assert host.state == 'PAUSED' and outcome in host.reason + assert len(failures) == 1 and not resumes + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_raw_partial_scope.py b/src/linkerhand_calibration/test/test_raw_partial_scope.py new file mode 100644 index 0000000..e153414 --- /dev/null +++ b/src/linkerhand_calibration/test/test_raw_partial_scope.py @@ -0,0 +1,68 @@ +"""Partial O30 capture keeps the lightweight path and the original motion contract.""" +from dataclasses import replace +from pathlib import Path + +from linkerhand_calibration.core.domain.capture_plan import configure_training, raw_joint_capture +from linkerhand_calibration.core.domain.profile import RetainedCadScope +from linkerhand_calibration.profiles import load_bundled_hand_profile, load_hand_profile +from linkerhand_calibration.profiles.export import dump_hand_profile +from linkerhand_calibration.profiles.scope import compile_capture_scope +from linkerhand_calibration.profiles.validator import validate_executable_profile, require_release_geometry +from linkerhand_calibration.runtime.engine import CalibrationEngine, SweepQuality +from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase + + +def thumb_profile(): + full = load_bundled_hand_profile('o30_right_18') + selected = frozenset(n for n in full.zero.active_joints if n.startswith('thumb_')) + retained = full.zero.active_joints-selected + scope = replace(full.scope, default_scope='thumb_only', + calibrate_joints={**full.scope.calibrate_joints, 'thumb_only': selected}, + frozen_joints={**full.scope.frozen_joints, 'thumb_only': retained}, + retained_cad_scopes={**full.scope.retained_cad_scopes, 'thumb_only': RetainedCadScope( + 'Only thumb is measured; other CAD joints remain uncalibrated.', + ('thumb_cmc_roll',), 'thumb_cmc_yaw', ('thumb_cmc_roll', 'thumb_cmc_yaw'))}) + return full, compile_capture_scope(replace(full, scope=scope)) + + +def test_partial_raw_schedule_preserves_routes_partitions_and_roundtrip(tmp_path): + full, partial = thumb_profile() + assert raw_joint_capture(partial) + assert configure_training(partial, 'raw_joint_2_plus_1') == partial + assert partial.motion.tasks == tuple(t for t in full.motion.tasks if t.key.startswith('thumb_')) + assert partial.command == full.command + assert partial.motion.return_groups == full.motion.return_groups + assert partial.acquisition == full.acquisition + assert partial.quality == full.quality + units = CalibrationEngine(partial).scan_units() + assert len(units) == 24 + assert units == tuple(u for u in CalibrationEngine(full).scan_units() if u.task_key.startswith('thumb_')) + assert set(u.cycle for u in units) == {0, 1, 3} + assert not partial.retained_joints & set(partial.zero.cad_zero_assumptions) + assert not partial.retained_joints & set(partial.urdf_authorized_fields) + path = tmp_path/'thumb.yaml' + path.write_text(dump_hand_profile(partial)) + assert load_hand_profile(path) == partial + source = Path(__file__).resolve().parents[1]/'urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf' + report = validate_executable_profile(partial, source) + require_release_geometry(report) + assert len(report['independent_measurement_coverage']['retained_uncalibrated_joints']) == 16 + + +def test_partial_raw_retry_is_immediate_bounded_and_finishes_after_return(): + _, partial = thumb_profile() + session = CalibrationSession(partial) + first, second = session.scheduled_units[:2] + bad = SweepQuality(False, ('missing_images',), (), {}) + for _ in range(2): + session.phase = Phase.EVALUATE + session.evaluation_complete(bad) + assert first.identity in session.incomplete_units + assert session.current_unit == second + assert session._retry_by_unit[first.identity] == 1 + while session.current_unit: + session.phase = Phase.EVALUATE + session.evaluation_complete(SweepQuality(True, (), (), {})) + assert session.phase == Phase.RETURN_BASELINE + session.return_complete() + assert session.phase == Phase.CAPTURE_COMPLETE diff --git a/src/linkerhand_calibration/test/test_raw_remount.py b/src/linkerhand_calibration/test/test_raw_remount.py new file mode 100644 index 0000000..9124cb2 --- /dev/null +++ b/src/linkerhand_calibration/test/test_raw_remount.py @@ -0,0 +1,195 @@ +"""A declared remount preserves good scans and cannot certify mixed mounts.""" +from copy import deepcopy +from pathlib import Path + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.raw_resume import RawCheckpoint +from linkerhand_calibration.runtime.raw_remount import REMOUNT_RECORD, remount_prefix +from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase +from linkerhand_calibration.runtime.artifacts.capture_validation import validate_capture_provenance +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel + + +@pytest.fixture +def model(): + return UrdfKinematicModel(Path(__file__).resolve().parents[1]/'urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf') + + +@pytest.fixture +def checkpoint(): + profile = load_bundled_hand_profile('o30_right_18') + units = CalibrationEngine(profile).scan_units() + rows = [dict(kind='session_start', resume_checkpoint_requested=False)] + for i, unit in enumerate(units[:78]): + base = dict(task_name=unit.task_key, cycle=unit.cycle, direction=unit.direction) + if i == 31: + rows.append(dict(kind='scan_unit_complete', **base, attempt=1, passed=False, + failures=['missing_images'])) + attempt = 2 if i == 31 else 1 + rows.append(dict(kind='raw_observation_frame', **base, attempt=attempt, + view='side', image_stamp_ns=i+1, tags={'index_pip': {'corners_xy': [[i, i]]}, + 'pinky_pip': {'corners_xy': [[1., 2.]]}})) + rows.append(dict(kind='scan_unit_complete', **base, attempt=attempt, passed=True)) + unit = units[78] + for attempt in (1, 2): + rows.append(dict(kind='scan_unit_complete', task_name=unit.task_key, + cycle=unit.cycle, direction=unit.direction, attempt=attempt, passed=False, + failures=['missing_images'])) + return profile, RawCheckpoint({}, {}, tuple(rows), frozenset(u.identity for u in units[:78])) + + +def test_remount_preserves_prefix_pixels_and_attempts_and_excludes_old_tag(checkpoint, model): + profile, source = checkpoint + original = deepcopy(source.rows) + rows, report = remount_prefix(profile, source, [10], model) + assert source.rows == original + assert len(report['retained_units']) == 78 and report['remaining_units'] == 30 + assert report['restart_unit'] == ['index_mcp_pitch_side', 0, 'increasing'] + assert report['excluded_old_tag_observations'] == 78 + assert report['excluded_old_roles'] == ['index_dip', 'index_mcp_roll', 'index_pip'] + assert [row['attempt'] for row in report['replaced_attempts']] == [1, 2] + for row in rows: + if row['kind'] == 'raw_observation_frame': + assert set(row['tags']) == {'pinky_pip'} + old = next(r for r in source.rows if r.get('image_stamp_ns') == row['image_stamp_ns']) + assert row['tags']['pinky_pip'] == old['tags']['pinky_pip'] + session = CalibrationSession(profile) + restored = RawCheckpoint({}, {}, (*rows, report), source.completed_units) + assert restored.restore(session) == () + assert list(session.current_unit.identity) == report['restart_unit'] + assert session.action().retry == 0 + unit = session.scheduled_units[31] + assert session._retry_by_unit[unit.identity] == 1 + assert session.phase == CalibrationPhase.PREPARE + with pytest.raises(ValueError, match='already_declared'): + remount_prefix(profile, restored, [10], model) + + +@pytest.mark.parametrize('ids', [[3], [999], [], [1]]) +def test_remount_does_not_accept_fixed_unknown_or_nonprefix_changes(checkpoint, model, ids): + profile, source = checkpoint + with pytest.raises(ValueError): + remount_prefix(profile, source, ids, model) + + +def test_scope_refinement_is_allowed_only_before_new_scanning(checkpoint, model): + profile, source = checkpoint + rows, report = remount_prefix(profile, source, [10], model) + old_report = {**report, 'excluded_old_roles': ['index_pip']} + prepared = RawCheckpoint({}, {}, (*rows, old_report), source.completed_units) + refined, _ = remount_prefix(profile, prepared, [10], model) + assert refined == rows + unit = CalibrationEngine(profile).scan_units()[78] + pending = dict(kind='raw_observation_frame', task_name=unit.task_key, + cycle=unit.cycle, direction=unit.direction, attempt=1) + started = RawCheckpoint({}, {}, (*prepared.rows, pending), source.completed_units) + with pytest.raises(ValueError, match='already_declared'): + remount_prefix(profile, started, [10], model) + + +def reference(profile): + from linkerhand_calibration.runtime.resume import ACQUISITION_POLICY_VERSION + pose = dict(rotation_xyzw=[0., 0., 0., 1.], translation_xyz_m=[0., 0., 1.]) + points = [[0., 0.], [10., 0.], [10., 10.], [0., 10.]] + return dict(profile_id=profile.key.profile_id, acquisition_policy_version=ACQUISITION_POLICY_VERSION, + protected_hashes={'source_urdf_sha256': 'a'*64}, + fixed_corners_by_view={view: deepcopy(points) for view in profile.vision.view_names}, + fixed_poses={view: pose for view in profile.vision.view_names}, moving_tag_poses={}, + raw_installation_reference={'side:pinky_pip': dict(corners_xy=deepcopy(points), + feedback=[0.]*profile.command.command_count)}) + + +def test_unseen_installation_is_draft_only_but_known_drift_still_rejected(checkpoint): + profile, source = checkpoint + old = reference(profile) + row = dict(kind='raw_observation_frame', view='side', tags={'pinky_pip': {}, 'ring_pip': {}}) + strict = RawCheckpoint({}, old, (row,), frozenset()) + with pytest.raises(ValueError, match='observation_missing'): + strict.verify_reference(profile, deepcopy(old)) + draft = RawCheckpoint({}, old, (row, dict(kind=REMOUNT_RECORD)), frozenset()) + result = draft.verify_reference(profile, deepcopy(old)) + assert result['unavailable_roles'] == ['side:ring_pip'] and result['draft_only'] + moved = deepcopy(old) + moved['raw_installation_reference']['side:pinky_pip']['corners_xy'][0][0] += 20 + with pytest.raises(ValueError, match='installation_changed'): + draft.verify_reference(profile, moved) + moved = deepcopy(old) + moved['fixed_corners_by_view']['front'][0][0] += 20 + with pytest.raises(ValueError, match='reference_changed'): + draft.verify_reference(profile, moved) + with pytest.raises(ValueError, match='separate_installation_certification'): + validate_capture_provenance(profile, 'serial', {}, draft.rows) + + +def test_remount_cli_rejects_missing_declaration_before_reading_configuration(): + from linkerhand_calibration.runtime.runner import main + with pytest.raises(SystemExit) as error: + main(['--config', '/nonexistent', '--prepare-remount-from', '/old']) + assert error.value.code == 2 + + +def test_raw_resume_defers_moving_baseline_difference_but_release_remains_strict(checkpoint): + profile, _ = checkpoint + old = reference(profile) + row = dict(kind='raw_observation_frame', view='side', tags={'pinky_pip': {}}) + source = RawCheckpoint({}, old, (row,), frozenset()) + current = deepcopy(old) + current['raw_installation_reference']['side:pinky_pip']['corners_xy'][0][0] += 12 + current['raw_installation_reference']['side:pinky_pip']['feedback'][5] = 7. + before = deepcopy(current) + audit = source.verify_reference(profile, current, require_installation=False) + assert current == before + assert audit['changed_roles'] == ['side:pinky_pip'] + assert audit['checked_roles'] == [] and audit['fixed_reference_verified'] + assert audit['requires_offline_review'] and not audit['installation_verified'] + assert audit['baseline_diagnostics']['side:pinky_pip'] == dict( + maximum_corner_difference_px=12., maximum_feedback_difference=7.) + with pytest.raises(ValueError, match='installation_changed'): + source.verify_reference(profile, current) + records = [dict(kind='raw_resume_verified', installation_verification=audit)] + with pytest.raises(ValueError, match='resume_installation_requires_offline_review'): + validate_capture_provenance(profile, 'serial', {}, records) + # Draft permission only defers installation certification, not provenance. + with pytest.raises(ValueError, match='requires_unique_session'): + validate_capture_provenance(profile, 'serial', {}, records, allow_installation_draft=True) + current['fixed_corners_by_view']['front'][0][0] += 20 + with pytest.raises(ValueError, match='reference_changed'): + source.verify_reference(profile, current, require_installation=False) + + +def test_missing_moving_reference_is_audited_and_invalid_feedback_still_rejected(checkpoint): + profile, _ = checkpoint + old = reference(profile) + row = dict(kind='raw_observation_frame', view='side', tags={'pinky_pip': {}, 'ring_pip': {}}) + source = RawCheckpoint({}, old, (row,), frozenset()) + audit = source.verify_reference(profile, deepcopy(old), require_installation=False) + assert audit['unavailable_roles'] == ['side:ring_pip'] and audit['requires_offline_review'] + invalid = deepcopy(old) + invalid['raw_installation_reference']['side:pinky_pip']['feedback'][0] = float('nan') + with pytest.raises(ValueError, match='installation_invalid'): + source.verify_reference(profile, invalid, require_installation=False) + + +def test_coordinator_records_unverified_moving_baseline_without_pausing(tmp_path): + from runtime_host_fixture import coordinator_fixture + from linkerhand_calibration.runtime.acquisition import load_capture + host, _ = coordinator_fixture(tmp_path, model='o30') + try: + old = reference(host.profile) + current = deepcopy(old) + current['raw_installation_reference']['side:pinky_pip']['corners_xy'][0][0] += 12 + row = dict(kind='raw_observation_frame', view='side', tags={'pinky_pip': {}}) + host._resume_checkpoint = RawCheckpoint({}, old, (row,), frozenset()) + host._current_fingerprint = current + host._verify_resume() + assert host._raw_resume_import is not None + assert host.execution.session.phase != CalibrationPhase.PAUSED + records = load_capture(host.raw_path) + audit = next(r['installation_verification'] for r in records if r.get('kind') == 'raw_resume_verified') + assert audit['requires_offline_review'] and not audit['installation_verified'] + assert '待离线复核' in host._resume_message + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_raw_resume.py b/src/linkerhand_calibration/test/test_raw_resume.py deleted file mode 100644 index ad5c41d..0000000 --- a/src/linkerhand_calibration/test/test_raw_resume.py +++ /dev/null @@ -1,112 +0,0 @@ -from dataclasses import replace - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import RAW_JOINT, configure_training -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.raw_resume import RawCheckpoint -from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase - - -def profile(): - return configure_training(load_bundled_hand_profile('o30_right_18'), RAW_JOINT) - - -def completion(unit, *, passed=True, attempt=1): - return dict(kind='scan_unit_complete', task_name=unit.task_key, - cycle=unit.cycle, direction=unit.direction, segment_key=unit.segment_key, - attempt=attempt, passed=passed, failures=[] if passed else ['missing_images']) - - -def test_restore_keeps_failed_unit_at_tail_and_does_not_reset_budget(): - session = CalibrationSession(profile()) - first, second, third = session.scheduled_units[:3] - rows = (completion(first, passed=False), completion(second)) - checkpoint = RawCheckpoint({}, {}, rows, frozenset((second.identity,))) - assert checkpoint.restore(session) == () - assert session.current_unit == third - assert session.scheduled_units[-1] == first - assert session._retry_by_unit[first.identity] == 1 - assert session.phase == Phase.PREPARE - - -def test_interrupted_second_attempt_is_missing_not_a_third_capture(): - session = CalibrationSession(profile()) - first, *others = session.scheduled_units - rows = [completion(first, passed=False), *(completion(u) for u in others)] - rows.append({**completion(first, attempt=2), 'kind': 'raw_observation_frame'}) - checkpoint = RawCheckpoint({}, {}, tuple(rows), frozenset(u.identity for u in others)) - interrupted = checkpoint.restore(session) - assert interrupted[0]['attempt'] == 2 - assert interrupted[0]['failures'] == ['interrupted_attempt'] - assert session.phase == Phase.RETURN_BASELINE - session.return_complete() - assert session.phase == Phase.FAILED - assert session.incomplete_units[first.identity] - - -def test_interrupted_first_attempt_moves_to_tail_once_across_restart(): - original = CalibrationSession(profile()) - unit = original.current_unit - row = {**completion(unit), 'kind': 'raw_observation_frame'} - checkpoint = RawCheckpoint({}, {}, (row,), frozenset()) - interrupted = checkpoint.restore(original) - restored = CalibrationSession(profile()) - checkpoint = replace(checkpoint, rows=(row, *interrupted)) - assert checkpoint.restore(restored) == () - assert restored.scheduled_units == original.scheduled_units - assert restored._retry_by_unit == original._retry_by_unit - - -def test_reordered_checkpoint_cannot_skip_work(): - session = CalibrationSession(profile()) - second = session.scheduled_units[1] - checkpoint = RawCheckpoint({}, {}, (completion(second),), frozenset((second.identity,))) - with pytest.raises(ValueError, match='schedule_order_changed'): - checkpoint.restore(session) - - -def test_reference_uses_raw_installation_pixels_not_moving_pnp_branch(): - from copy import deepcopy - from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION - selected = profile() - corners = [[0., 0.], [10., 0.], [10., 10.], [0., 10.]] - pose = dict(rotation_xyzw=[0., 0., 0., 1.], translation_xyz_m=[0., 0., 1.]) - reference = dict(profile_id=selected.key.profile_id, - acquisition_policy_version=ACQUISITION_POLICY_VERSION, - protected_hashes={'source_urdf_sha256': 'a'*64}, - fixed_corners_by_view={v: corners for v in selected.vision.view_names}, - fixed_poses={v: pose for v in selected.vision.view_names}, - moving_tag_poses={'front:thumb_mcp': pose}, - raw_installation_reference={'front:thumb_mcp': dict(corners_xy=deepcopy(corners), - feedback=list(selected.command.baseline_values))}) - current = deepcopy(reference) - current['moving_tag_poses'] = {} - checkpoint = RawCheckpoint({}, reference, (dict(kind='raw_observation_frame', - view='front', tags={'thumb_mcp': {}}),), frozenset()) - checkpoint.verify_reference(selected, current) - current['raw_installation_reference']['front:thumb_mcp']['corners_xy'][0][0] += 20. - with pytest.raises(ValueError, match='raw_resume_installation_changed'): - checkpoint.verify_reference(selected, current) - - -def test_discovery_preserves_interrupted_first_attempt_without_any_passed_unit(tmp_path): - import json - from linkerhand_calibration.core.domain.capture_plan import CapturePlan - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - from linkerhand_calibration.runtime.engine import CalibrationEngine, ACQUISITION_POLICY_VERSION - from linkerhand_calibration.runtime.resume import discover_resume_candidate - selected = profile() - hashes = {key:'a'*64 for key in selected.artifacts.protected_input_fields} - directory = tmp_path/'20260922_first_attempt'; directory.mkdir() - header = dict(kind='session_start', profile_id=selected.key.profile_id, serial_number='RAW_TEST', - acquisition_policy_version=ACQUISITION_POLICY_VERSION, - pose_tracking_policy_version=POSE_TRACKING_POLICY_VERSION, - capture_schedule_version=CalibrationEngine(selected).capture_schedule_version, - capture_plan=CapturePlan.from_profile(selected).as_dict(), resume_checkpoint_requested=False, **hashes) - row = {**completion(CalibrationEngine(selected).scan_units()[0]),'kind':'raw_observation_frame'} - records = [header,dict(kind='fixed_base_reference_locked'),row] - (directory/'raw_samples.jsonl').write_text('\n'.join(json.dumps(r) for r in records)+'\n') - found = discover_resume_candidate(tmp_path,profile_id=selected.key.profile_id,serial_number='RAW_TEST', - protected_hashes=hashes,training_policy=RAW_JOINT) - assert found == directory diff --git a/src/linkerhand_calibration/test/test_recorded_diagnostic_replay.py b/src/linkerhand_calibration/test/test_recorded_diagnostic_replay.py deleted file mode 100644 index 0cf42e6..0000000 --- a/src/linkerhand_calibration/test/test_recorded_diagnostic_replay.py +++ /dev/null @@ -1,147 +0,0 @@ -"""Real capture segments test evidence, never a fabricated complete hand run. - -175815 has old sweep images but no new pre-zero motion record. 192637 has the -actual pre-zero motion but stopped before its IP sweeps. They are replayed as -separate sessions; neither is promoted to a fourth-round accuracy certificate. -""" - -from copy import deepcopy -import gzip -import hashlib -import json -from pathlib import Path - -import numpy as np -import pytest - -from linkerhand_calibration.core.geometry.pnp import solve_square_tag_ippe -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization -from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture -from linkerhand_calibration.runtime.diagnostic_quality import evaluate_diagnostic_unit -from linkerhand_calibration.runtime.diagnostic_report import motion_initialization_summary -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 - - -SOURCE_HASHES = { - "175815": "8bdbf216bd4135f1bac16dee0535a8b72ea0e6b0f31ab2b9a2e8cc4a83be9c0d", - "192637": "757ba1d7cccae4bdb93553b2735e94862bc74f57f153198e1a9d3eff76d45c8d", -} - - -def recorded_fixture(stamp): - path = Path(__file__).with_name("fixtures") / f"o6_front_20260914_{stamp}_raw_replay.json.gz" - value = json.loads(gzip.decompress(path.read_bytes())) - assert value["source_raw_sha256"] == SOURCE_HASHES[stamp] - assert value["publication_allowed"] is value["is_accuracy_certificate"] is False - return value - - -def replay_actual_front_initialization(): - fixture = recorded_fixture("192637") - profile = load_bundled_hand_profile("o6_right_8") - recorded = fixture["recorded_initialization"] - joints = tuple(recorded["zero_joints"]) - task = next(task for task in profile.motion.tasks if task.key == recorded["task_name"]) - target = profile.motion.joint_zero_references[joints[0]].command - common = dict(task_key=task.key, command_index=task.command_index) - # Reproduce this archived v4 decision with its original 3D candidate - # policy. The new image solver has separate original-corner regressions; - # it cannot change the meaning of this historical unresolved session. - owner = BranchInitialization(profile, image_geometry=False) - owner.begin(MotionCommand("zero_approach", target, 40., reference_joints=joints, **common), - session_epoch=recorded["session_epoch"], motion_version=recorded["motion_version"]) - for row in fixture["records"]: - owner.observe(row) - zero = MotionCommand("joint_zero", target, 40., zero_joints=joints, **common) - owner.begin(zero, session_epoch=recorded["session_epoch"], motion_version=recorded["motion_version"]+1) - request = owner.request("front", zero, session_epoch=recorded["session_epoch"], - motion_version=recorded["motion_version"]+1) - result = solve_initialization(request) - model, report = owner.accept(request, result) - return fixture, owner, zero, model, report - - -def test_recorded_pre_zero_frames_reach_runtime_decision_without_forged_continuation(): - fixture, owner, zero, model, report = replay_actual_front_initialization() - recorded = fixture["recorded_initialization"] - assert len(fixture["records"]) == 120 - assert all(row["sample_phase"] == "zero_approach" for row in fixture["records"]) - assert report["source_image_stamps"] == recorded["source_image_stamps"] - assert report["source_frame_hashes"] == recorded["source_frame_hashes"] - for row in fixture["records"]: - assert source_frame_sha256(row) == report["source_frame_hashes"][str(row["image_stamp_ns"])] - # This is the actual unresolved runtime evidence, not a test claiming - # that rejecting it repaired physical accuracy or completed its IP sweep. - assert model is None and not owner.ready(zero) - assert report["status"] == recorded["status"] == "unresolved" - assert report["reason"] == recorded["reason"] == "motion_geometry_unresolved" - assert not report["evidence_ids"] and not report["constraints"] - for actual, baseline in zip(report["hypotheses"], recorded["hypotheses"], strict=True): - assert actual["accepted"] == baseline["accepted"] - for current, previous in zip(actual["relations"], baseline["relations"], strict=True): - assert current["reason"] == previous["reason"] - for key, value in previous.items(): - if isinstance(value, (int, float)): - assert current[key] == pytest.approx(value, rel=1e-9, abs=1e-10) - - -def test_report_identifies_the_ip_relation_without_promoting_local_pitch_fit(): - fixture = recorded_fixture("192637") - summaries = motion_initialization_summary([fixture["recorded_initialization"]]) - report, = summaries - assert report["status"] == "unresolved" and report["source_image_count"] == 120 - pitch = report["joint_hypotheses"]["rh_thumb_cmc_pitch"] - ip = report["joint_hypotheses"]["rh_thumb_ip"] - assert any(not item["reason"] for item in pitch) - assert not any(item["chain_accepted"] for item in (*pitch, *ip)) - supported_parent = [item for item in ip if item["other_relations_passed"]] - assert len(supported_parent) == 2 - assert all(item["reason"] == "motion_geometry_residual" for item in supported_parent) - assert min(item["pivot_rms_m"] for item in supported_parent) == pytest.approx(.005102, abs=1e-6) - - -def test_legacy_real_sweep_corners_replay_without_becoming_new_zero_or_accepted_angles(): - fixture = recorded_fixture("175815") - records = fixture["records"] - before = deepcopy(records) - profile = load_bundled_hand_profile("o6_right_8") - for direction, count in (("decreasing", 198), ("increasing", 197)): - rows = [row for row in records if row["direction"] == direction] - assert len(rows) == count - for row in rows: - for role in ("thumb_pitch", "thumb_ip"): - tag = row["tags"][role] - candidates = solve_square_tag_ippe(tag["corners_xy"], - tag_size_m=tag["tag_size_m"], camera_matrix=row["camera_matrix"]) - assert candidates - for recorded in tag["reprojection_valid_candidates"]: - assert any(np.allclose(candidate.translation_xyz_m, - recorded["translation_xyz_m"], rtol=1e-10, atol=1e-12) - for candidate in candidates) - unit = next(unit for unit in CalibrationEngine(profile).scan_units() - if unit.task_key == "thumb_pitch_ip_front" and unit.cycle == 0 and unit.direction == direction) - quality = evaluate_diagnostic_unit(profile, unit, 1, rows) - assert quality.observations.passed, quality.observations.failures - assert not quality.accepted_poses.passed - assert all(item["valid_frames"] == 0 for item in quality.accepted_poses.metrics.values()) - with pytest.raises(ValueError, match="diagnostic_capture_not_for_publication_or_resume"): - reject_diagnostic_capture([fixture["session_start"], *records]) - assert records == before - assert not any(row.get("kind") in {"joint_zero_reference", "joint_sample"} for row in records) - - -@pytest.mark.parametrize("stamp,expected", SOURCE_HASHES.items()) -def test_fixture_sources_remain_bound_when_full_local_journals_are_available(stamp, expected): - workspace = Path(__file__).resolve().parents[3] - path = workspace / "calibration_output/O6_RIGHT_001" / f"20260914_{stamp}" / "raw_samples.jsonl" - if not path.exists(): - pytest.skip("完整本地实测日志不可用；已有证据切片的回放另行执行") - assert hashlib.sha256(path.read_bytes()).hexdigest() == expected - fixture = recorded_fixture(stamp) - recorded = {(row.get("kind"), row.get("view"), row.get("image_stamp_ns")): row - for row in (json.loads(line) for line in path.read_text().splitlines())} - assert all(recorded[row["kind"], row["view"], row["image_stamp_ns"]] == row - for row in fixture["records"]) diff --git a/src/linkerhand_calibration/test/test_recorded_side_preparation.py b/src/linkerhand_calibration/test/test_recorded_side_preparation.py deleted file mode 100644 index 57d0a25..0000000 --- a/src/linkerhand_calibration/test/test_recorded_side_preparation.py +++ /dev/null @@ -1,49 +0,0 @@ -"""Use the available preparation arc without reusing validation images.""" - -import gzip -import json -from dataclasses import replace -from pathlib import Path - -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( - ImageMotionParameters, resolve_image_motion, -) - - -def test_recorded_side_preserves_holdout_and_uses_source_geometry(): - path = Path(__file__).parent / "fixtures/o6_side_preparation_132102.json.gz" - payload = json.loads(gzip.decompress(path.read_bytes())) - rows = payload["records"] - config = load_product_config(Path(__file__).resolve().parents[1]/"config/o6_right_product.yaml", - check_can=False) - profile = config.calibration_contract.typed_profile - first = rows[0] - owner = BranchInitialization(profile, source_urdf=config.source_urdf) - motion = MotionCommand("zero_approach", profile.command.baseline_values, 40., task_key=first["task_name"], - reference_joints=tuple(first["zero_joints"])) - version, epoch = first["motion_version"], first["session_epoch"] - owner.begin(motion, session_epoch=epoch, motion_version=version) - for row in rows: - owner.observe(row) - motion = replace(motion, phase="joint_zero", zero_joints=motion.reference_joints) - request = owner.request("side", motion, session_epoch=epoch, motion_version=version + 1) - assert len(request.image_frames) == 121 - old = resolve_image_motion(request.image_frames, request.relations, - parameters=ImageMotionParameters(maximum_training_frames=36)) - assert not old.resolved and old.reason == "image_motion_families_not_distinguishable" - # Current production uses the same source geometry as the final URDF. - # Adding frames to the old free-geometry fit is not sufficient evidence - # that its competing three-dimensional reconstructions are distinguishable. - assert request.geometry_constraints and request.source_urdf_sha256 - result = solve_initialization(request) - assert result.resolved, result.reason - assert result.model.source_urdf_sha256 == request.source_urdf_sha256 - assert len(result.model.training_stamps_ns) == 61 - assert len(result.model.validation_stamps_ns) == 60 - assert set(result.model.training_stamps_ns).isdisjoint(result.model.validation_stamps_ns) - probabilities = [p for h in result.hypotheses for _, p in h.comparison_p_values] - assert len(probabilities) == 3 - assert max(probabilities) <= .01 / 3 diff --git a/src/linkerhand_calibration/test/test_rectified_camera_contract.py b/src/linkerhand_calibration/test/test_rectified_camera_contract.py deleted file mode 100644 index 16d4654..0000000 --- a/src/linkerhand_calibration/test/test_rectified_camera_contract.py +++ /dev/null @@ -1,73 +0,0 @@ -from types import SimpleNamespace -import numpy as np -import pytest -import cv2 -from scipy.spatial.transform import Rotation as R - -from linkerhand_calibration.core.geometry.camera import rectified_camera_matrix -from linkerhand_calibration.pnp import solve_square_tag_ippe, square_object_points - - -def test_rectified_corners_require_projection_not_raw_intrinsics(): - k=np.array([[3816.108989,0,620.129591],[0,3796.2534,570.26288],[0,0,1.]]) - p=np.array([[3792.732422,0,606.014017,0],[0,3825.369629,568.524418,0],[0,0,1,0.]]) - expected=R.from_euler('xyz',[.2,-.5,.3]) - t=np.array([.12,.05,1.]) - corners=cv2.projectPoints(square_object_points(.016),expected.as_rotvec(),t,p[:,:3],np.zeros(4))[0].reshape(4,2) - candidates=solve_square_tag_ippe(corners,tag_size_m=.016,camera_matrix=rectified_camera_matrix(p)) - errors=[(expected.inv()*R.from_quat(c.quaternion_xyzw)).magnitude() for c in candidates] - assert min(errors)<1e-6 - wrong=solve_square_tag_ippe(corners,tag_size_m=.016,camera_matrix=k) - assert min((expected.inv()*R.from_quat(c.quaternion_xyzw)).magnitude() for c in wrong)>.001 - - -@pytest.mark.parametrize('bad',[[],[0.]*12,[float('nan')]*12]) -def test_invalid_p_cannot_silently_fall_back_to_k(bad): - with pytest.raises(ValueError):rectified_camera_matrix(bad) - - -def test_shared_camera_callback_uses_p_saves_provenance_and_invalidates(tmp_path): - import json - p=[500.,0,320,0,0,510,240,0,0,0,1,0] - msg=SimpleNamespace(p=p,k=[600.,0,300,0,620,220,0,0,1],d=[.1]*5,r=np.eye(3).ravel().tolist(),width=640,height=480) - node = camera_host(tmp_path, msg) - callback=accept_camera - callback(node,'front',msg); callback(node,'front',msg) - np.testing.assert_allclose(node.matrices['front'],np.array(p).reshape(3,4)[:,:3]) - records=[json.loads(l) for l in node.raw_path.read_text().splitlines()] - assert len(records)==1 and records[0]['raw_k']==msg.k - assert records[0]['matrix_source']=='CameraInfo.P[:3,:3]' - msg.p=[0.]*12 - callback(node,'front',msg) - assert 'front' not in node.matrices - - -def camera_host(tmp_path, message): - from linkerhand_calibration.runtime.cameras import CameraObservations - from linkerhand_calibration.core.geometry.extrinsics import camera_info_fingerprint - identity = SimpleNamespace(width=message.width, height=message.height, - intrinsics_sha256=camera_info_fingerprint(width=message.width, height=message.height, - camera_matrix=message.k, distortion=message.d, rectification=message.r, projection=message.p)) - return CameraObservations(SimpleNamespace(cameras={'front': identity}), tmp_path/'raw.jsonl', lambda: 10.) - - -def accept_camera(host, view, message, *, started=False, pause=lambda _: None): - from linkerhand_calibration.runtime.ros.io import camera_input - host.accept(view, camera_input(message), started=started, pause=pause, trackers={}) - - -@pytest.mark.parametrize('field', ['k', 'd', 'r', 'p']) -def test_live_camera_model_must_match_protected_extrinsics(tmp_path, field): - msg = SimpleNamespace(p=[500.,0,320,0,0,510,240,0,0,0,1,0], - k=[600.,0,300,0,620,220,0,0,1], d=[.1]*5, - r=np.eye(3).ravel().tolist(), width=640, height=480) - node = camera_host(tmp_path, msg) - callback = accept_camera - callback(node, 'front', msg) - assert 'front' in node.matrices - pauses = [] - getattr(msg, field)[0] += .01 - callback(node, 'front', msg, started=True, pause=pauses.append) - assert 'front' not in node.matrices - assert 'front' not in node.info_received_at - assert pauses == ['protected_camera_model_changed:front:camera_info_does_not_match_extrinsics'] diff --git a/src/linkerhand_calibration/test/test_reference_lock.py b/src/linkerhand_calibration/test/test_reference_lock.py deleted file mode 100644 index 5494277..0000000 --- a/src/linkerhand_calibration/test/test_reference_lock.py +++ /dev/null @@ -1,45 +0,0 @@ -from __future__ import annotations - -import numpy as np - -from linkerhand_calibration.runtime import ReferenceLock - - -def _corners(offset: float = 0.0): - return np.asarray( - [[10.0, 10.0], [20.0, 10.0], [20.0, 20.0], [10.0, 20.0]] - ) + offset - - -def test_preview_and_baseline_motion_are_discarded_before_lock() -> None: - lock = ReferenceLock({"camera": 7}) - for _ in range(20): - lock.observe("camera", 7, _corners(100.0)) - lock.begin_session() - for _ in range(20): - lock.observe("camera", 7, _corners(50.0)) - assert not lock.locked - - lock.start_locking() - for _ in range(9): - lock.observe("camera", 7, _corners()) - assert not lock.locked - lock.observe("camera", 7, _corners()) - assert lock.locked - assert np.allclose(lock.references["camera"], _corners()) - - -def test_locked_reference_is_not_rewritten_and_needs_ten_bad_frames() -> None: - lock = ReferenceLock({"camera": 7}) - lock.begin_session() - lock.start_locking() - for _ in range(10): - lock.observe("camera", 7, _corners()) - original = lock.fingerprint() - - for _ in range(9): - assert lock.observe("camera", 7, _corners(6.0)) is None - movement = lock.observe("camera", 7, _corners(6.0)) - assert movement is not None - assert movement.confirmation_frames == 10 - assert lock.fingerprint() == original diff --git a/src/linkerhand_calibration/test/test_reference_waypoints.py b/src/linkerhand_calibration/test/test_reference_waypoints.py deleted file mode 100644 index 445a149..0000000 --- a/src/linkerhand_calibration/test/test_reference_waypoints.py +++ /dev/null @@ -1,62 +0,0 @@ -from dataclasses import replace -import pytest - -from linkerhand_calibration.core.domain.profile import ReferenceWaypoint -from linkerhand_calibration.profiles import load_bundled_hand_profile, dump_hand_profile, load_hand_profile -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.resume import select_installation_evidence, ResumeVerifier -from test_resume_v2 import _fingerprint - - -def test_reference_waypoint_cannot_drive_disabled_channel(): - from linkerhand_calibration.core.domain.profile import validate_profile - profile = load_bundled_hand_profile("g20_right_19") - channel = next(iter(profile.command.disabled_indices)) - pose = list(profile.command.baseline_values) - pose[channel] -= 1 - view = profile.vision.views[0] - tag = next(tag for tag in view.tags if not tag.fixed_reference) - waypoint = ReferenceWaypoint("show", tuple(pose), {view.name: (tag.tag_id,)}) - profile = replace(profile, motion=replace(profile.motion, resume_verification_waypoints=(waypoint,))) - with pytest.raises(ValueError, match="disabled channels"): - validate_profile(profile) - - -def test_reference_waypoints_share_executor_and_return_before_resume(tmp_path): - profile = load_bundled_hand_profile("l6_right_8") - # Legacy v2 sessions retain their all-at-start verification waypoint API. - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2, - directional_command_joints=frozenset())) - pose = (230., 255., 255., 255., 255., 255.) - waypoint = ReferenceWaypoint("show_tip", pose, {"front": (2,)}) - profile = replace(profile, motion=replace(profile.motion, resume_verification_waypoints=(waypoint,))) - path = tmp_path/"virtual.yaml" - path.write_text(dump_hand_profile(profile)) - assert load_hand_profile(path) == profile - engine = SessionExecution(profile) - engine.session.device_ready() - engine.session.start(resume_requested=True) - current = profile.command.baseline_values - while engine.session.phase == Phase.BASELINE: - motion = engine.motion(current); current = motion.target; engine.motion_complete() - engine.session.reference_locked() - assert engine.session.phase == Phase.REFERENCE_POSES - motion = engine.motion(current) - assert motion.phase == "reference_pose" and motion.target == pose - while engine.session.phase == Phase.REFERENCE_POSES: - motion = engine.motion(current); current = motion.target; engine.motion_complete() - assert current == profile.command.baseline_values - assert engine.session.phase == Phase.RESUME_VERIFY - - -def test_resume_never_compares_mounts_from_different_waypoint_poses(): - fingerprint = _fingerprint() - pose = fingerprint.moving_tag_poses["finger"] - old = replace(fingerprint, moving_tag_poses={"show/finger": pose}) - new = replace(fingerprint, moving_tag_poses={"other/finger": pose}) - checker = ResumeVerifier(required_moving_poses=("finger",)) - assert not checker.compare(*select_installation_evidence(old, new, {"finger"})).reuse - assert checker.compare(*select_installation_evidence(old, old, {"finger"})).reuse - missing = replace(fingerprint, moving_tag_poses={}) - assert not checker.compare(*select_installation_evidence(old, missing, {"finger"})).reuse diff --git a/src/linkerhand_calibration/test/test_relative_standard_mimic.py b/src/linkerhand_calibration/test/test_relative_standard_mimic.py deleted file mode 100644 index 14caefa..0000000 --- a/src/linkerhand_calibration/test/test_relative_standard_mimic.py +++ /dev/null @@ -1,53 +0,0 @@ -import math -from dataclasses import replace - -import numpy as np -import pytest - -from linkerhand_calibration.core.fitting.coupling import ( - PairedJointObservation, fit_relative_standard_mimic, -) - - -def observations(*, nonlinear=False): - rows = [] - for cycle in range(4): - for direction in ("increasing", "decreasing"): - for index, q in enumerate(np.linspace(0, 1.4, 65)): - # Independently generated fixed mounting phases; neither is a - # physical zero. Backlash in the driver affects both joints. - q += .003 if direction == "decreasing" else 0 - child = .87*q + (.22*q*q if nonlinear else 0) - rows.append(PairedJointObservation(f"camera:{cycle}:{direction}:{index}", - cycle, direction, q+.24, child-.31)) - return rows - - -def test_unknown_mounting_phase_is_not_exported_as_passive_zero(): - result = fit_relative_standard_mimic("driver", "follower", observations(), cad_offset_rad=.012) - assert result.fit.multiplier == pytest.approx(.87, abs=1e-10) - assert result.fit.offset_rad == .012 - assert not result.fit.offset_observed - assert result.installation_phase_rad == pytest.approx(-.31-.87*.24) - assert max(abs(v) for v in result.holdout_errors_rad) < 1e-10 - - -def test_nonlinear_motion_cannot_hide_behind_json_or_endpoint_ratio(): - with pytest.raises(ValueError, match="standard_urdf_mimic_not_expressive"): - fit_relative_standard_mimic("driver", "follower", observations(nonlinear=True), cad_offset_rad=0) - - -def test_holdout_installation_slip_cannot_be_recentred(): - rows = [replace(row, target_rad=row.target_rad + (math.radians(5) if row.cycle == 3 else 0)) - for row in observations()] - with pytest.raises(ValueError, match="phase=holdout"): - fit_relative_standard_mimic("driver", "follower", rows, cad_offset_rad=0) - - -def test_both_directions_require_independent_paired_images(): - rows = observations() - with pytest.raises(ValueError, match="duplicated"): - fit_relative_standard_mimic("driver", "follower", rows + [rows[0]], cad_offset_rad=0) - with pytest.raises(ValueError, match="missing paired"): - fit_relative_standard_mimic("driver", "follower", - [row for row in rows if not (row.cycle == 3 and row.direction == "decreasing")], cad_offset_rad=0) diff --git a/src/linkerhand_calibration/test/test_resume_storage.py b/src/linkerhand_calibration/test/test_resume_storage.py deleted file mode 100644 index 636c7e7..0000000 --- a/src/linkerhand_calibration/test/test_resume_storage.py +++ /dev/null @@ -1,256 +0,0 @@ -"""Passed resume keeps scan payloads on disk until their durable import batch.""" - -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.runtime.resume_storage import ( - DeferredCaptureRecord, load_passed_capture, -) -from linkerhand_calibration.runtime.joint_resume import ResumeJournalImport, _index_motion_provenance -from linkerhand_calibration.runtime.staged_resume import _essential_indexed_provenance -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 -from linkerhand_calibration.core.domain.motion_path import record_scan_identity -from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture -from linkerhand_calibration.core.artifacts.evidence_journal import ( - REFERENCE_KEY, archive_path, compact_existing_journal, - initialize_evidence_journal, manifest_path, -) -from linkerhand_calibration.storage import append_jsonl, append_jsonl_many -from linkerhand_calibration.runtime.acquisition import load_capture -from linkerhand_calibration.runtime.capture_index import CaptureRecordIndex -from linkerhand_calibration.runtime.resume import discover_resume_candidate - - -def capture(tmp_path, rows): - path = tmp_path / "capture.jsonl" - path.write_text("\n".join(json.dumps(r, ensure_ascii=False) for r in rows)+"\n") - return path - - -def test_scalar_index_never_reads_image_payload_and_import_is_bounded(tmp_path, monkeypatch): - original = [dict(kind="joint_sample", task_name="中指", joint="middle_pip", - cycle=1, direction="increasing", image_stamp_ns=i+1, - pnp_observation_evidence={"pixels": list(range(100))}) for i in range(130)] - rows = load_passed_capture(capture(tmp_path, original)) - assert all(isinstance(r, DeferredCaptureRecord) for r in rows) - calls = [] - materialize = DeferredCaptureRecord.materialize - def observed(row): - calls.append(row.offset) - return materialize(row) - monkeypatch.setattr(DeferredCaptureRecord, "materialize", observed) - assert all(record_scan_identity(r) == ("中指", 1, "increasing") for r in rows) - reject_diagnostic_capture(rows) - assert _index_motion_provenance(rows) == {} and not calls - pending = ResumeJournalImport(tuple(rows)) - imported = [] - for size in (64, 64, 2): - before = len(calls) - batch = pending.take_batch() - assert len(batch) == size and len(calls)-before == size - assert all(type(r) is dict for r in batch) - imported.extend(batch) - assert pending.transfer_complete and not pending.complete and imported == original - assert all("pnp_observation_evidence" not in r.metadata for r in rows) - - -def test_changed_checkpoint_is_rejected_before_advancing_import_offset(tmp_path): - path = capture(tmp_path, [dict(kind="pnp_candidate_frame", image_stamp_ns=1, roles={"moving": [1, 2]})]) - pending = ResumeJournalImport(tuple(load_passed_capture(path))) - path.write_text(path.read_text().replace('[1, 2]', '[3, 4]')) - with pytest.raises(ValueError, match="resume_source_record_changed"): - pending.take_batch() - assert pending.offset == 0 - - -def test_zero_pixel_provenance_is_identical_to_eager_reading(tmp_path): - fixture = Path(__file__).parent / "fixtures/o30_shared_tag_middle_pip.json.gz" - original = json.loads(gzip.decompress(fixture.read_bytes()))["records"] - rows = load_passed_capture(capture(tmp_path, original)) - original_index = _index_motion_provenance(original) - lazy_index = _index_motion_provenance(rows) - assert set(lazy_index) == set(original_index) - for identity, row in lazy_index.items(): - assert json.loads(json.dumps(row)) == original_index[identity] - assert any(r["kind"] == "pnp_candidate_frame" for r in lazy_index.values()) - - -@pytest.mark.parametrize("text", ['{"kind":', '[]']) -def test_invalid_journal_is_not_hidden_by_deferred_storage(tmp_path, text): - path = tmp_path / "bad.jsonl" - path.write_text(text) - with pytest.raises(ValueError, match="JSONL"): - load_passed_capture(path) - - -def test_indexed_journal_keeps_resume_fields_inline_and_restores_full_evidence(tmp_path): - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=256) - header = {"kind": "session_start", "profile_id": "O30/right/layout/rev"} - samples = [dict(kind="joint_sample", task_name="食指", joint="index_dip", - cycle=cycle, direction="increasing", image_stamp_ns=cycle + 1, - relative_quaternion_xyzw=[0., 0., 0., 1.], - pnp_observation_evidence={"pixels": list(range(1000))}) for cycle in range(2)] - append_jsonl(path, header) - append_jsonl_many(path, samples) - assert load_capture(path) == [header, *samples] - stored = [json.loads(line) for line in path.read_text().splitlines()] - assert REFERENCE_KEY not in stored[0] - assert all(row["joint"] == "index_dip" and REFERENCE_KEY in row for row in stored[1:]) - assert archive_path(path).stat().st_size < sum(len(json.dumps(row)) for row in samples) - - deferred = load_passed_capture(path) - assert all(isinstance(row, DeferredCaptureRecord) for row in deferred[1:]) - assert deferred[1]["relative_quaternion_xyzw"] == [0., 0., 0., 1.] - assert "relative_quaternion_xyzw" not in deferred[1].metadata - assert deferred[1].materialize() == samples[0] - - -def test_indexed_journal_rejects_changed_compressed_evidence(tmp_path): - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=1) - append_jsonl(path, {"kind": "joint_sample", "pixels": list(range(100))}) - archive = archive_path(path) - changed = bytearray(archive.read_bytes()) - changed[len(changed)//2] ^= 1 - archive.write_bytes(changed) - with pytest.raises(ValueError, match="compressed_evidence_block_changed"): - load_capture(path) - - -def test_legacy_journal_migration_is_verified_before_atomic_replacement(tmp_path): - path = capture(tmp_path, [ - {"kind": "session_start", "serial_number": "O30_TEST"}, - {"kind": "joint_sample", "joint": "index_dip", "pixels": list(range(5000))}, - ]) - original = load_capture(path) - report = compact_existing_journal(path, replace=True) - assert report["semantic_verification"] == "passed" and report["installed"] is True - assert report["source_bytes"] > report["compact_journal_bytes"] - assert load_capture(path) == original - assert manifest_path(path).is_file() and archive_path(path).is_file() - assert not path.with_name(path.name + ".uncompressed-backup").exists() - second = compact_existing_journal(path, replace=True) - assert second["installed"] is True and load_capture(path) == original - assert not archive_path(path).with_name(archive_path(path).name + ".backup").exists() - - -def test_detailed_frames_are_externalized_even_when_smaller_than_size_threshold(tmp_path): - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path) - row = {"kind": "motion_branch_observation", "view": "front", "image_stamp_ns": 1, - "tags": {"index_distal": [[1., 2.], [3., 4.]]}} - append_jsonl(path, row) - stored = json.loads(path.read_text()) - assert REFERENCE_KEY in stored - assert load_capture(path) == [row] - - -def test_resume_import_hard_links_compressed_evidence_without_copying_records(tmp_path): - source_dir, target_dir = tmp_path / "source", tmp_path / "target" - source_dir.mkdir(); target_dir.mkdir() - source, target = source_dir / "raw_samples.jsonl", target_dir / "raw_samples.jsonl" - initialize_evidence_journal(source, threshold_bytes=1) - initialize_evidence_journal(target, threshold_bytes=1) - original = [{"kind": "motion_branch_observation", "view": "side", - "image_stamp_ns": stamp, "pixels": list(range(100))} for stamp in range(3)] - append_jsonl_many(source, original) - pending = ResumeJournalImport(tuple(load_passed_capture(source))) - batch = pending.take_batch() - assert all(isinstance(row, DeferredCaptureRecord) for row in batch) - append_jsonl_many(target, batch) - linked = list(target_dir.glob("imported_evidence_*.zlib")) - assert len(linked) == 1 and linked[0].samefile(archive_path(source)) - archive_path(source).unlink() - source.unlink() - assert load_capture(target) == original - - -def test_linked_joint_samples_use_bounded_batches_and_concrete_training_digest(tmp_path): - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=1) - leading = [dict(kind="image_observation_frame", task_name="index_dip_side", - cycle=0, direction="increasing", image_stamp_ns=-stamp-1, - pixels=list(range(100))) for stamp in range(10)] - samples = [dict(kind="joint_sample", task_name="index_dip_side", joint="index_dip", - cycle=0, direction="increasing", image_stamp_ns=stamp, - command_u8=stamp, feedback_u8=stamp, relative_quaternion_xyzw=[0., 0., 0., 1.]) - for stamp in range(100)] - append_jsonl_many(path, (*leading, *samples)) - pending = ResumeJournalImport(tuple(load_passed_capture(path))) - - assert len(pending.take_batch()) == len(leading) - first = pending.take_batch() - assert len(first) == len(samples) and all(row.get("kind") == "joint_sample" for row in first) - index = CaptureRecordIndex(retain_complete=False, retain_training_geometry=True) - index.extend(first) - assert len(index) == len(samples) - assert all(len(row["source_record_sha256"]) == 64 for row in index) - - -def test_training_index_materializes_each_deferred_sample_once(tmp_path, monkeypatch): - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=1) - sample = dict(kind="joint_sample", task_name="index_dip_side", joint="index_dip", - cycle=0, direction="increasing", image_stamp_ns=1, command_u8=20, - feedback_u8=19, relative_quaternion_xyzw=[0., 0., 0., 1.], - relative_translation_xyz_m=[0., 0., .01]) - append_jsonl(path, sample) - row = load_passed_capture(path)[0] - original = DeferredCaptureRecord.materialize_plain - calls = [] - - def observed(record): - calls.append(record.reference.record_sha256) - return original(record) - - monkeypatch.setattr(DeferredCaptureRecord, "materialize_plain", observed) - index = CaptureRecordIndex(retain_complete=False, retain_training_geometry=True) - index.append(row) - - assert len(calls) == 1 - assert index[0]["relative_translation_xyz_m"] == [0., 0., .01] - - -def test_compact_passed_resume_keeps_only_digest_referenced_motion_frames(tmp_path): - path = tmp_path / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=1) - used = {"kind": "motion_branch_observation", "task_name": "index_dip_side", - "view": "side", "image_stamp_ns": 10, "pixels": list(range(100))} - unused = {**used, "image_stamp_ns": 20, "pixels": list(range(101))} - accepted = {"kind": "motion_branch_initialization", "status": "resolved", - "task_name": "index_dip_side", "view": "side", - "source_frame_hashes": {"10": source_frame_sha256(used)}} - append_jsonl_many(path, (used, unused, accepted)) - - indexed = tuple(_index_motion_provenance(load_passed_capture(path)).values()) - essential = _essential_indexed_provenance(indexed) - - observations = [row for row in essential if row.get("kind") == "motion_branch_observation"] - assert len(observations) == 1 - assert observations[0].canonical_sha256 == source_frame_sha256(used) - - -def test_discovery_reads_large_compressed_session_header(tmp_path): - from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - session = tmp_path / "SERIAL" / "20260101_000000" - session.mkdir(parents=True) - path = session / "raw_samples.jsonl" - initialize_evidence_journal(path, threshold_bytes=128) - hashes = {"profile_config_sha256": "a" * 64} - contract = {"large": list(range(100))} - append_jsonl_many(path, [{"kind": "session_start", "profile_id": "O30/right/layout/v1", - "serial_number": "SERIAL", "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "capture_schedule_version": "unified_schedule_v13_staged_training", - "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, "protected_hashes": hashes, - "capture_plan": {"policy": "staged_2_to_3"}, "preparation_contract": contract}, - {"kind": "fixed_base_reference_locked"}, - {"kind": "scan_unit_complete", "task_name": "index_dip_side", "cycle": 0, - "direction": "increasing", "passed": True}]) - assert discover_resume_candidate(tmp_path / "SERIAL", profile_id="O30/right/layout/v1", - serial_number="SERIAL", protected_hashes=hashes, training_policy="staged_2_to_3", - preparation_contract=contract) == session diff --git a/src/linkerhand_calibration/test/test_resume_v2.py b/src/linkerhand_calibration/test/test_resume_v2.py deleted file mode 100644 index 830ca56..0000000 --- a/src/linkerhand_calibration/test/test_resume_v2.py +++ /dev/null @@ -1,48 +0,0 @@ -from __future__ import annotations - -import math - -from linkerhand_calibration.runtime import ( - ACQUISITION_POLICY_VERSION, - ResumeFingerprint, - ResumeVerifier, - TagPoseFingerprint, -) - - -def _fingerprint(*, corner_offset=0.0, hash_value="a", angle=0.0): - return ResumeFingerprint( - profile_id="TEST/right/layout/v1", - protected_hashes={"source": hash_value * 64}, - fixed_corners_by_view={ - "view": tuple( - (x + corner_offset, y + corner_offset) - for x, y in ((0, 0), (10, 0), (10, 10), (0, 10)) - ) - }, - fixed_poses={ - "base": TagPoseFingerprint( - (0.0, 0.0, math.sin(angle / 2), math.cos(angle / 2)), - (0.0, 0.0, 0.4), - ) - }, - moving_tag_poses={ - "finger": TagPoseFingerprint((0.0, 0.0, 0.0, 1.0), (0.1, 0, 0.4)) - }, - acquisition_policy_version=ACQUISITION_POLICY_VERSION, - ) - - -def test_matching_v2_reference_reuses_checkpoint() -> None: - decision = ResumeVerifier().compare(_fingerprint(), _fingerprint()) - assert decision.reuse - - -def test_reference_or_protected_input_change_starts_fresh_without_pause() -> None: - changed = ResumeVerifier().compare( - _fingerprint(), _fingerprint(corner_offset=6.0, hash_value="b") - ) - assert not changed.reuse - assert "重新采集" in changed.reason - assert "protected_hashes" in changed.incompatible_fields - assert "fixed_corners[view]" in changed.incompatible_fields diff --git a/src/linkerhand_calibration/test/test_resumed_image_reference.py b/src/linkerhand_calibration/test/test_resumed_image_reference.py deleted file mode 100644 index 776aaeb..0000000 --- a/src/linkerhand_calibration/test/test_resumed_image_reference.py +++ /dev/null @@ -1,177 +0,0 @@ -"""Recorded O30 DIP resume: retain old geometry and check only current pixels.""" - -from dataclasses import replace -import gzip -import json -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -import pytest - -from linkerhand_calibration.core.domain.reference import JointZeroReference -from linkerhand_calibration.core.geometry.extrinsics import transform_matrix -from linkerhand_calibration.core.geometry.image_motion_replay import replay_image_models, validate_image_sample -from linkerhand_calibration.core.geometry.tag_pose.image_reference import image_reference_from_dict -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import image_motion_model_from_dict -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.observations import compile_tag_feedback_channels -from linkerhand_calibration.runtime.branch_initialization import MotionBranchModel -from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture -from linkerhand_calibration.runtime.image_reference import ImageReferenceTracker -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.parent_reference import check_parent_pose -from linkerhand_calibration.runtime.pose_evidence import reference_branch_revisions -from linkerhand_calibration.runtime.reference_lock import ReferenceLock -from o30_capture_fixture import SOURCE - - -@pytest.fixture -def resumed(): - payload = json.loads(gzip.decompress((Path(__file__).parent / "fixtures" / - "o30_resumed_parent_reference.json.gz").read_bytes())) - profile = load_bundled_hand_profile("o30_right_18") - task = next(t for t in profile.motion.tasks if t.key == "pinky_dip_side") - report = payload["model_report"] - model = MotionBranchModel(report["task_name"], report["view"], (), - tuple(report["evidence_ids"].items()), image_motion_model_from_dict(report["frozen_image_model"])) - current = image_reference_from_dict(payload["current_reference"]) - assert model.image_model.reference_frames[0] != current - lock = ReferenceLock({"side": 3}) - lock.begin_session() - lock.start_locking() - for image in current.source_images: - lock.observe("side", 3, image.corners_xy) - capture = ObservationCapture(profile, reference_lock=lock, - extrinsics=SimpleNamespace(transform=lambda _: transform_matrix(**payload["reference_from_side"]))) - capture._image_references["side"] = ImageReferenceTracker(10, reference=current) - capture.locked_poses["side"] = current.pose - capture.locked_revisions["side"] = 1 - capture.activate_reference_model(model) - motion = MotionCommand("joint_zero", profile.command.baseline_values, 200, - task_key=task.key, zero_joints=(task.parent_reference.pose_joint,), - reference_only=True, reference_reuse=True) - frames = tuple(CaptureFrame("side", r["image_stamp_ns"], np.asarray(r["camera_matrix"]), - {role: np.asarray(tag["corners_xy"]) for role, tag in r["tags"].items()}, - tuple(r["feedback_vector"]), tuple(r["command_vector"]), - command_directions=tuple(r["command_direction_by_index"])) for r in payload["failed_frames"]) - return SimpleNamespace(payload=payload, profile=profile, task=task, model=model, - current=current, capture=capture, motion=motion, frames=frames) - - -def test_original_session_root_reproduces_recorded_binding_failure(resumed, monkeypatch): - rig = resumed - monkeypatch.setattr(rig.capture, "_scheduled_image_reference", - lambda view, models: rig.capture._image_references[view]) - rows, _ = rig.capture.consume(rig.frames[0], rig.motion) - assert rig.payload["original_rejection"] in {row.get("reason") for row in rows} - assert not any(row["kind"] == "joint_zero_sample" for row in rows) - - -def test_recorded_resume_captures_parent_with_original_model_and_replayable_evidence(resumed): - rig = resumed - root = rig.model.image_model.reference_frames[0] - provider = rig.capture._model_image_references[("side", root.identity)] - assert provider.motion_snapshot(root.role, rig.frames[0].stamp_ns) is None - assert not provider.reference_confirmed(root.role, 1) - samples = [] - for frame in rig.frames: - rows, movement = rig.capture.consume(frame, rig.motion) - assert movement is None - sample = next(row for row in rows if row["kind"] == "joint_zero_sample") - candidates = next(row for row in rows if row["kind"] == "pnp_candidate_frame") - persisted = json.loads(json.dumps((sample, candidates))) - validate_image_sample(persisted[0], persisted[1], replay_image_models(persisted[1])) - evidence = sample["pnp_observation_evidence"] - assert evidence["parent"]["reference_frame_sha256"] == root.identity - assert evidence["child"]["image_model_sha256"] == rig.payload["model_report"]["image_model_sha256"] - samples.append(sample) - revisions = reference_branch_revisions(samples, profile=rig.profile, require_motion_evidence=True) - assert rig.capture.freeze_reference_branches(revisions) - channels = compile_tag_feedback_channels(rig.profile, UrdfKinematicModel(SOURCE)) - reference = JointZeroReference.from_record(rig.payload["source_zero"]) - check_parent_pose(rig.profile, rig.task, reference, samples, channels["pinky_pip"]) - assert rig.capture._image_references["side"].reference == rig.current - assert rig.capture._image_references["side"].motion_snapshot(root.role, rig.frames[-1].stamp_ns) - assert provider.reference == root - - -@pytest.mark.parametrize("change", ["moved", "missing", "intrinsics"]) -def test_restored_reference_still_requires_unchanged_current_image(resumed, change): - rig = resumed - frame = rig.frames[0] - if change == "moved": - frame = replace(frame, corners={**frame.corners, "side_base": frame.corners["side_base"] + (8, 0)}) - elif change == "missing": - frame = replace(frame, corners={k: v for k, v in frame.corners.items() if k != "side_base"}) - else: - matrix = frame.camera_matrix.copy() - matrix[0, 0] += 1 - frame = replace(frame, camera_matrix=matrix) - rows, _ = rig.capture.consume(frame, rig.motion) - assert not any(row["kind"] in {"joint_zero_sample", "pnp_candidate_frame"} for row in rows) - assert not rig.capture.freeze_reference_branches({("side", "side_base"): 1}) - if change == "moved": - for i in range(1, 10): - _, movement = rig.capture.consume(replace(frame, stamp_ns=frame.stamp_ns+i), rig.motion) - assert movement is not None # New-session drift witness remains independent. - - -def test_reset_revokes_restored_tracker_snapshots(resumed): - rig = resumed - rig.capture.consume(rig.frames[0], rig.motion) - root = rig.model.image_model.reference_frames[0] - provider = rig.capture._model_image_references[("side", root.identity)] - snapshot = provider.motion_snapshot(root.role, rig.frames[0].stamp_ns) - assert provider.snapshot_current(snapshot) - rig.capture.reset() - assert not provider.snapshot_current(snapshot) - - -def test_model_restore_does_not_solve_or_replay_historical_reference_images(resumed, monkeypatch): - from linkerhand_calibration.runtime import image_reference as module - - rig = resumed - rig.capture.reset() - def historical_solve_forbidden(*args, **kwargs): - pytest.fail("Restoring passed geometry must not replay its old images") - monkeypatch.setattr(module, "solve_square_tag_ippe", historical_solve_forbidden) - monkeypatch.setattr(module, "coordinate_origin", historical_solve_forbidden) - rig.capture.activate_reference_model(rig.model) - root = rig.model.image_model.reference_frames[0] - provider = rig.capture._model_image_references[("side", root.identity)] - assert provider.reference is root - assert not provider.reference_confirmed(root.role, 1) - - -def test_switching_tasks_selects_each_models_own_reference(resumed): - rig = resumed - old = rig.model.image_model.reference_frames[0] - image_model = rig.model.image_model - offset = rig.current.source_images[-1].stamp_ns - min(image_model.source_stamps_ns) + 1 - other = replace(rig.model, task_name="another_epoch", image_model=replace( - image_model, reference_frames=(rig.current,), - source_stamps_ns=tuple(stamp + offset for stamp in image_model.source_stamps_ns), - training_stamps_ns=tuple(stamp + offset for stamp in image_model.training_stamps_ns), - validation_stamps_ns=tuple(stamp + offset for stamp in image_model.validation_stamps_ns))) - rig.capture.activate_reference_model(other) - assert rig.capture._scheduled_image_reference("side", (other,)).reference == rig.current - rig.capture.activate_reference_model(rig.model) - assert rig.capture._scheduled_image_reference("side", (rig.model,)).reference == old - assert rig.capture._scheduled_image_reference("side", (rig.model, other)) is None - - -def test_parent_timeout_reports_actual_reference_joint_and_current_branch(resumed): - from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator - - rig = resumed - host = SimpleNamespace(profile=rig.profile, _steady_rows=[], _zero_reference_error="old_pip_error", - branch_initialization=SimpleNamespace(), - _branch_observation_reasons={("side", "pinky_pip"): (0, rig.payload["original_rejection"])}, - _view=lambda name: next(v for v in rig.profile.vision.views if v.name == name)) - host._task_branch_details = lambda *a, **kw: CalibrationCoordinator._task_branch_details(host, *a, **kw) - assert not CalibrationCoordinator._confirm_model_reference(host, rig.motion) - assert host._zero_reference_error == "parent_reference_samples_missing:pinky_mcp_pitch:side:0/10" - details = CalibrationCoordinator._joint_zero_branch_details(host, rig.motion) - assert f"pinky_mcp_pitch:side:tag=4:{rig.payload['original_rejection']}" in details diff --git a/src/linkerhand_calibration/test/test_rigid_pair_observer.py b/src/linkerhand_calibration/test/test_rigid_pair_observer.py deleted file mode 100644 index 4302d8d..0000000 --- a/src/linkerhand_calibration/test/test_rigid_pair_observer.py +++ /dev/null @@ -1,186 +0,0 @@ -"""Recorded rigid-pair evidence: accept resolved motion, reject ambiguous views.""" -from copy import deepcopy -from dataclasses import replace -import gzip,json -from pathlib import Path - -import pytest - -from linkerhand_calibration.profiles import load_hand_profile -from linkerhand_calibration.profiles.export import write_hand_profile -from linkerhand_calibration.profiles.validator import validate_executable_profile -from linkerhand_calibration.profiles.preparation import preparation_contract -from linkerhand_calibration.profiles.observer_policy import TRAJECTORY_OBSERVER -from linkerhand_calibration.runtime.observer_reference import ObserverBridgeInput,build_observer_bridge,validate_observer_references -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.core.geometry.tag_pose.image_motion_model import image_motion_model_from_dict -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation -from linkerhand_calibration.core.geometry.tag_pose.source_hinge import SourceHinge -from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import image_parameters_for_sampling -from o30_capture_fixture import SOURCE -from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint - - -def recorded_source(tmp_path_factory, finger): - joint, parent_role = f'{finger}_dip', f'{finger}_pip' - data=json.loads(gzip.decompress((Path(__file__).parent/f'fixtures/o30_{finger}_rigid_pair.json.gz').read_bytes())) - path=tmp_path_factory.mktemp('rigid_pair')/'profile.yaml';path.write_text(data['profile_yaml']) - old=load_hand_profile(path) - measurements=dict(old.measurement.measurements) - measurements[joint]=replace(measurements[joint],observer_pose_policy='rigid_pair_v1') - profile=replace(old,measurement=replace(old.measurement,measurements=measurements)) - write_hand_profile(profile,path);assert load_hand_profile(path)==profile - validate_executable_profile(profile,SOURCE) - parent=next(r for r in data['parents'] if r['task_name']==f'{finger}_pip_side') - rows={r['image_stamp_ns']:r for r in data['rows']} - model=image_motion_model_from_dict(parent['frozen_image_model']) - source=ObserverBridgeInput(joint,model,parent, - tuple(rows[s] for s in parent['source_image_stamps'] if s in rows and joint in rows[s]['tags']), - tuple(rows[s] for s in data['endpoint_stamps']),(channel_for_joint(profile,joint),)) - return old, profile, source, data, rows, model, parent - - -@pytest.mark.replay -def test_independent_parent_branches_converge_on_real_bounded_probe(tmp_path): - from linkerhand_calibration.runtime.observer_preparation import ObserverProbeEvidence - root=Path(__file__).parent/'fixtures' - data=json.loads(gzip.decompress((root/'o30_index_independent_probe.json.gz').read_bytes())) - path=tmp_path/'profile.yaml';path.write_text(data['profile_yaml']) - profile=load_hand_profile(path) - old=json.loads(gzip.decompress((root/'o30_index_rigid_pair.json.gz').read_bytes())) - parent=next(r for r in old['parents'] if r['task_name']=='index_pip_side') - model=image_motion_model_from_dict(parent['frozen_image_model']) - rows=data['rows'] - decision=next(r for r in rows if r['kind']=='observer_preparation_decision') - evidence=ObserverProbeEvidence(decision, - tuple(r for r in rows if r['kind']=='image_observation_frame'), - tuple(r for r in rows if r['kind']=='observer_preparation_segment_complete'), - next(r for r in rows if r['kind']=='observer_preparation_excursion_started')) - failure=next(r for r in rows if r['kind']=='motion_branch_initialization') - target=profile.motion.joint_zero_references['index_dip'].command - endpoints=tuple(sorted((r for r in rows if r['kind']=='motion_branch_observation' - and tuple(r.get('command_vector') or ())==target - and r['image_stamp_ns'] not in failure['source_image_stamps'] - and r['image_stamp_ns']<=max(failure['source_image_stamps'])), - key=lambda r:r['image_stamp_ns']))[-10:] - bridge,record=build_observer_bridge(profile,ObserverBridgeInput( - 'index_dip',model,parent,(),endpoints,(16,),evidence)) - assert record['version']=='observer_pose_bridge_v3' - assert bridge.rotation_uncertainty_rad<.017453293 - assert bridge.translation_uncertainty_m<.001 - assert all(p==0 for _,p in record['pose']['comparison_adjusted_p_values']) - assert set(record['pose']['training_stamps']).isdisjoint(record['pose']['validation_stamps']) - measurements=dict(profile.measurement.measurements) - measurements['index_dip']=replace(measurements['index_dip'], - observer_pose_policy=TRAJECTORY_OBSERVER) - trajectory_profile=replace(profile,measurement=replace(profile.measurement, - measurements=measurements)) - trajectory_bridge,trajectory_record=build_observer_bridge(trajectory_profile, - ObserverBridgeInput('index_dip',model,parent,(),endpoints,(16,),evidence)) - assert trajectory_record['version']=='observer_pose_bridge_v4' - assert trajectory_record['trajectory_branch_decision']['holdout_role']=='veto_only' - assert trajectory_record['trajectory_branch_decision']['corner_evidence_sha256'] - assert trajectory_bridge.rotation_uncertainty_rad<.017453293 - - -@pytest.fixture(scope='module') -def rigid_source(tmp_path_factory): - finger = 'middle' - joint, parent_role = f'{finger}_dip', f'{finger}_pip' - old, profile, source, data, rows, model, parent = recorded_source(tmp_path_factory, finger) - failure = data['failure'] - bridge,record=build_observer_bridge(profile,source) - relation=MotionRelation(joint,parent_role,joint) - frames=bridge_frames(tuple(rows[s] for s in failure['source_image_stamps']),relation) - hinge=SourceHinge(model.geometry[0],parent['evidence_ids'][parent_role],parent['image_model_sha256'],*parent['geometry_uncertainty'][0][1:]) - request=BranchInitializationRequest(task_name=failure['task_name'],view='side',zero_joints=(joint,), - session_epoch=failure['session_epoch'],motion_version=failure['motion_version'],generation=1, - frames=tuple(f.evidence for f in frames),relations=(relation,),existing_evidence_ids=tuple(parent['evidence_ids'].items()), - moving_roles=(parent_role,joint),source_frame_hashes=tuple(failure['source_frame_hashes'].items()), - image_frames=frames,image_geometry=True,geometry_constraints=tuple(ParallelAxisGeometry(**v) for v in failure['image_geometry_constraints']), - source_urdf_sha256=failure['source_urdf_sha256'],source_hinges=(hinge,),parent_reference_sha256=failure['parent_reference_sha256'], - observer_sources=(source,),observer_profile=profile,image_parameters=image_parameters_for_sampling(failure.get('image_sampling_policy'))) - result=solve_initialization(request) - assert result.resolved,result.reason - _,formal=initialization_artifacts(request,result) - records=json.loads(json.dumps([data['header'],*data['rows'],*data['parents'],formal])) - return old,profile,source,bridge,record,request,result,records - - -@pytest.mark.replay -def test_recorded_index_ambiguity_cannot_be_accepted_by_switching_policy(tmp_path_factory): - _, profile, source, *_ = recorded_source(tmp_path_factory, 'index') - with pytest.raises(ValueError, match='^rigid_pair_candidates_unresolved$'): - build_observer_bridge(profile, source) - - -@pytest.mark.parametrize('complete', [False, True]) -def test_raw_parent_candidates_are_versioned_and_never_silently_discarded(tmp_path_factory, monkeypatch, complete): - from linkerhand_calibration.profiles.observer_policy import RIGID_OBSERVER, LEGACY_RIGID_OBSERVER - from linkerhand_calibration.core.geometry.tag_pose import rigid_tag_pair - _, profile, source, *_ = recorded_source(tmp_path_factory, 'middle') - specs = dict(profile.measurement.measurements) - specs[source.joint] = replace(specs[source.joint], - observer_pose_policy=RIGID_OBSERVER if complete else LEGACY_RIGID_OBSERVER) - profile = replace(profile, measurement=replace(profile.measurement, measurements=specs)) - role = specs[source.joint].parent_role - def inspect(*args, **kwargs): - if complete: - candidates = kwargs['parent_candidates'] - assert len(candidates) == len(source.rows) - assert all(len(actual) == len(row['tags'][role]['reprojection_valid_candidates']) - for actual,row in zip(candidates,source.rows)) - assert any(len(actual) == 2 for actual in candidates) - else: - assert 'parent_candidates' not in kwargs - raise ValueError('candidate_boundary_checked') - monkeypatch.setattr(rigid_tag_pair, 'fit_rigid_tag_pair', inspect) - with pytest.raises(ValueError, match='candidate_boundary_checked'): - build_observer_bridge(profile, source) - - -@pytest.mark.replay -def test_real_pair_passes_original_precision_and_dip_acceptance(rigid_source): - old,profile,source,bridge,record,_,result,_=rigid_source - with pytest.raises(ValueError,match='observer_pose_uncertain'):build_observer_bridge(old,source) - assert bridge.rotation_uncertainty_rad<.017453293 - assert bridge.translation_uncertainty_m<.001 - assert set(record['pose']['training_stamps']).isdisjoint(record['pose']['validation_stamps']) - assert all(h.pose_bridge_checks[0].status=='consistent' for h in result.hypotheses) - assert all(c.consistent for h in result.hypotheses for c in h.source_geometry_checks) - assert preparation_contract(profile)!=preparation_contract(old) - assert result.model.maximum_reprojection_error_px==1.5 - - -@pytest.mark.replay -def test_rigid_pair_and_constrained_recipient_replay(rigid_source): - _,profile,_,_,_,_,_,records=rigid_source - evidence=_initializations(records) - validate_observer_references(profile,records,evidence) - - -@pytest.mark.parametrize('fault',['held_feedback','source_pixel','current_pixel']) -def test_changed_physical_support_is_rejected(rigid_source,fault): - _,profile,source,_,_,request,_,_=rigid_source - rows,endpoints=deepcopy(source.rows),deepcopy(source.endpoint_rows) - joint = source.joint - if fault=='held_feedback':rows[10]['feedback_vector'][source.held_channels[0]]+=5 - elif fault=='source_pixel': - for r in rows[1::2]:r['tags'][joint]['corners_xy']=[[x+10,y+8] for x,y in r['tags'][joint]['corners_xy']] - else: - for r in endpoints:r['tags'][joint]['corners_xy']=[[x+10,y+8] for x,y in r['tags'][joint]['corners_xy']] - changed=replace(source,rows=tuple(rows),endpoint_rows=tuple(endpoints)) - result=solve_initialization(replace(request,observer_sources=(changed,))) - assert not result.resolved and result.model is None - - -def test_missing_rigid_evidence_cannot_fall_back(rigid_source): - _,profile,_,_,_,request,_,records=rigid_source - result=solve_initialization(replace(request,observer_sources=())) - assert not result.resolved and result.reason=='rigid_pair_source_missing' - evidence=_initializations(records) - for payload in evidence.values():payload.pop('observer_pose_bridges',None) - with pytest.raises(ValueError,match='required_evidence_missing'):validate_observer_references(profile,records,evidence) diff --git a/src/linkerhand_calibration/test/test_rotation_holdout_diagnostics.py b/src/linkerhand_calibration/test/test_rotation_holdout_diagnostics.py deleted file mode 100644 index 469890e..0000000 --- a/src/linkerhand_calibration/test/test_rotation_holdout_diagnostics.py +++ /dev/null @@ -1,143 +0,0 @@ -"""Rejected visual fits remain rejected and preserve useful raw-sample evidence.""" - -from copy import deepcopy -from dataclasses import asdict -import json -import math - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.fitting import observed_motion -from linkerhand_calibration.core.fitting.rotation_diagnostics import RotationHoldoutFailure -from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session - - -def rotation_rows(*, slip_axis=None, slip_one=False): - mount = Rotation.from_euler("xyz", (.24, -.17, .31)) - rows = [] - for cycle in range(4): - for direction in ("increasing", "decreasing"): - for index, value in enumerate(np.linspace(0, 1, 65)): - rotation = mount * Rotation.from_euler("z", .8 * value) - if cycle == 3 and slip_axis and (not slip_one or - (direction == "decreasing" and index == 12)): - rotation *= Rotation.from_euler(slip_axis, math.radians(5)) - rows.append({"sample_id": f"camera_a:{cycle}:{direction}:{index}", - "view": "camera_a", "feedback_rad": float(value), - "relative_quaternion_xyzw": rotation.as_quat().tolist(), - "cycle": cycle, "direction": direction}) - return rows, mount.as_quat() - - -def fit(rows, reference, **context): - return observed_motion.fit_native_observed_rotation(rows, input_domain="feedback_rad", - input_to_joint_direction=1, reference_xyzw=reference, **context) - - -def test_optional_diagnostic_context_preserves_existing_success_outputs(): - rows, reference = rotation_rows() - original = deepcopy(rows) - curve, errors = fit(rows, reference) - contextual_curve, contextual_errors = fit(rows, reference, joint="hinge", view="camera_a") - assert asdict(curve) == asdict(contextual_curve) - assert errors == contextual_errors - assert max(errors) < 1e-12 - assert curve.angle_rad == pytest.approx(np.linspace(0, .8, 65), abs=1e-12) - assert rows == original - - -@pytest.mark.parametrize("axis", ["x", "z"]) -def test_diagnostics_distinguish_off_axis_slip_from_angle_error_without_refitting(axis, monkeypatch): - rows, reference = rotation_rows(slip_axis=axis) - original = deepcopy(rows) - training_calls = [] - primitive = observed_motion.fit_rotation_curve - - def record_training(observations, **kwargs): - training_calls.append(tuple(observations)) - return primitive(observations, **kwargs) - - monkeypatch.setattr(observed_motion, "fit_rotation_curve", record_training) - with pytest.raises(RotationHoldoutFailure) as caught: - fit(rows, reference, joint="hinge", view="camera_a") - error = caught.value - diagnostic = error.diagnostics - assert str(error).startswith("independent_rotation_holdout_failed:joint=hinge:view=camera_a:") - assert "max_deg=5.000000" in str(error) - assert diagnostic.full_so3.count == 130 - assert diagnostic.full_so3.mae_deg == pytest.approx(5, abs=1e-10) - assert diagnostic.full_so3.p95_deg == pytest.approx(5, abs=1e-10) - assert diagnostic.full_so3.maximum_deg == pytest.approx(5, abs=1e-10) - assert diagnostic.full_so3.passed is False - if axis == "z": - assert diagnostic.axial.mae_deg == pytest.approx(5, abs=1e-10) - assert diagnostic.off_axis.maximum_deg < 1e-10 - else: - assert diagnostic.off_axis.mae_deg > 4.9 - assert diagnostic.axial.maximum_deg < .05 - assert len(training_calls) == 1 - assert {row.cycle for row in training_calls[0]} == {0, 1, 2} - assert rows == original - - -def test_one_bad_holdout_image_is_reported_and_not_discarded(): - rows, reference = rotation_rows(slip_axis="z", slip_one=True) - with pytest.raises(RotationHoldoutFailure) as caught: - fit(rows, reference) - diagnostic = caught.value.diagnostics - assert diagnostic.full_so3.mae_deg < 1 - assert diagnostic.full_so3.p95_deg < 2 - assert diagnostic.full_so3.maximum_deg == pytest.approx(5, abs=1e-10) - assert diagnostic.worst_sample.sample_id == "camera_a:3:decreasing:12" - assert diagnostic.worst_sample.cycle == 3 - assert diagnostic.worst_sample.direction == "decreasing" - assert diagnostic.worst_sample.input_value == pytest.approx(12 / 64) - assert diagnostic.worst_sample.axial_error_deg == pytest.approx(5, abs=1e-10) - - -def test_finalizer_persists_joint_and_camera_evidence_before_any_spatial_solve(tmp_path, monkeypatch): - from measured_capture_fixture import measured_capture - from linkerhand_calibration.core.fitting import session - - profile, source, records, _ = measured_capture(tmp_path, layout="o6_right_8") - failed_joint = "rh_pinky_dip" - for row in records: - if row.get("joint") == failed_joint and row.get("cycle") == 3: - slipped = Rotation.from_quat(row["relative_quaternion_xyzw"]) * Rotation.from_euler("x", .1) - row["relative_quaternion_xyzw"] = slipped.as_quat().tolist() - parent = Rotation.from_quat(row["parent_pose_common"]["quaternion_xyzw"]) - row["child_pose_common"]["quaternion_xyzw"] = (parent * slipped).as_quat().tolist() - - def forbidden_spatial_solve(*args, **kwargs): - pytest.fail("a failed visual holdout must stop before spatial fitting") - - monkeypatch.setattr(session, "solve_urdf_zero_offsets", forbidden_spatial_solve) - previous = tmp_path / "previous" - previous.mkdir() - pointer = tmp_path / profile.artifacts.publication_pointer - pointer.symlink_to(previous, target_is_directory=True) - directory = tmp_path / "failed_session" - with pytest.raises(RotationHoldoutFailure) as caught: - finalize_profile_session(profile=profile, session_dir=directory, - serial_number="SYNTHETIC_DIAGNOSTIC", source_urdf=source, - protected_inputs={}, records=records) - - report = json.loads((directory / "fit_diagnostics.json").read_text()) - assert report["passed"] is False - assert report["publication_allowed"] is False - assert report["stage"] == "relative_rotation_holdout" - assert report["reason"] == str(caught.value) - detail = report["rotation_holdout"] - assert detail["joint"] == failed_joint - assert detail["view"] == "side" - assert detail["training_cycles"] == [0, 1, 2] - assert detail["holdout_cycle"] == 3 - assert detail["full_so3"]["maximum_deg"] > 3 - assert detail["worst_sample"]["sample_id"] in { - row["sample_id"] for row in records if row.get("joint") == failed_joint - and row.get("cycle") == 3 and row.get("sample_phase", "sweep") == "sweep"} - assert pointer.resolve() == previous - assert not list(directory.glob("*.urdf")) - assert not (directory / "release_manifest.json").exists() diff --git a/src/linkerhand_calibration/test/test_runtime_gc_lifecycle.py b/src/linkerhand_calibration/test/test_runtime_gc_lifecycle.py deleted file mode 100644 index 25dc494..0000000 --- a/src/linkerhand_calibration/test/test_runtime_gc_lifecycle.py +++ /dev/null @@ -1,77 +0,0 @@ -"""Large prepared checkpoints must not be revisited by live cyclic GC.""" - -import gc -import weakref - -import pytest - -from linkerhand_calibration.runtime.ros import calibration_node - - -class Cycle: - def __init__(self): - self.back = self - - -@pytest.mark.parametrize("interrupted", [False, True]) -def test_startup_graph_is_excluded_but_new_cycles_are_collected(interrupted): - checkpoint = {"rows": [Cycle() for _ in range(128)]} - startup_ids = {id(row) for row in checkpoint["rows"]} - enabled, thresholds = gc.isenabled(), gc.get_threshold() - try: - with calibration_node._frozen_startup_graph(): - assert startup_ids.isdisjoint(id(row) for row in gc.get_objects()) - live = Cycle() - reference = weakref.ref(live) - del live - gc.collect(2) - assert reference() is None - assert gc.isenabled() == enabled - assert gc.get_threshold() == thresholds - if interrupted: - raise KeyboardInterrupt - except KeyboardInterrupt: - assert interrupted - assert startup_ids <= {id(row) for row in gc.get_objects()} - assert gc.isenabled() == enabled - assert gc.get_threshold() == thresholds - - -@pytest.mark.parametrize("failure", [None, "spin", "executor_shutdown", "node_shutdown"]) -def test_node_freezes_only_after_preparation_and_unfreezes_after_shutdown(monkeypatch, failure): - import rclpy - - events = [] - def event(name): - events.append(name) - if name == failure: - raise RuntimeError(name) - - class Node: - def __init__(self, profile): - event("prepared_checkpoint") - def destroy_node(self): - event("node_shutdown") - - class Executor: - def add_node(self, node): - event("add_node") - def spin(self): - event("spin") - def shutdown(self): - event("executor_shutdown") - - monkeypatch.setattr(rclpy, "init", lambda **kw: event("ros_init")) - monkeypatch.setattr(rclpy, "shutdown", lambda: event("ros_shutdown")) - monkeypatch.setattr(calibration_node, "UnifiedCalibrationNode", Node) - monkeypatch.setattr(calibration_node, "SingleThreadedExecutor", Executor) - monkeypatch.setattr(calibration_node.gc, "collect", lambda: event("collect")) - monkeypatch.setattr(calibration_node.gc, "freeze", lambda: event("freeze")) - monkeypatch.setattr(calibration_node.gc, "unfreeze", lambda: event("unfreeze")) - if failure: - with pytest.raises(RuntimeError, match=failure): - calibration_node.run_profile_node(object()) - else: - calibration_node.run_profile_node(object()) - assert events == ["ros_init", "prepared_checkpoint", "add_node", "collect", "freeze", - "spin", "executor_shutdown", "node_shutdown", "ros_shutdown", "unfreeze"] diff --git a/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py b/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py deleted file mode 100644 index 16ee9f0..0000000 --- a/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py +++ /dev/null @@ -1,297 +0,0 @@ -"""Frozen Tag branches guard capture, zero references and coordinator races.""" - -from concurrent.futures import ThreadPoolExecutor -from copy import deepcopy -from dataclasses import replace -from threading import Event - -import pytest - -from linkerhand_calibration.core.geometry.pnp import SquareTagPose -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 linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh -from pose_branch_fixture import IdealPoseTracker, confirm_reference_rows -from runtime_host_fixture import coordinator_fixture, ready -from test_joint_zero_runtime import reference_rows -from test_observation_capture import make_capture, frame - - -def branch_event(stamp, *, view="top", role="thumb_yaw", tag=7, reason="pose_branch_conflict"): - return {"kind": "pnp_branch_event", "view": view, "image_stamp_ns": stamp, - "tag_role": role, "tag_id": tag, "reason": reason, - "pnp_observation_evidence": {"candidate_diagnostics": { - "observation_stamp_ns": stamp, "branch_revision": 0, - "branch_status": "conflict", "branch_reason": reason, - "branch_confirmation_count": 5, "branch_frozen": True}}} - - -def begin_yaw(host, clock): - ready(host, clock) - assert host.start().success - host.execution.session.phase = Phase.SWEEP - host._motion = MotionCommand("sweep", host.profile.command.baseline_values, 40., - task_key="thumb_yaw_top", command_index=1, cycle=0, direction="decreasing") - - -@pytest.mark.parametrize("kind,paused", [ - ("required", True), ("fixed", True), ("fixed_projection", True), ("unstarted", False), - ("pending", False), ("ambiguous", False), ("fixed_unverified", False), ("recovering", False), ("stale_metadata", False), -]) -def test_branch_events_pause_only_current_required_roles_after_version_checks(tmp_path, monkeypatch, kind, paused): - host, clock = coordinator_fixture(tmp_path) - try: - begin_yaw(host, clock) - row = branch_event(host.ports.clock_ns()) - if kind in {"fixed", "fixed_projection"}: - row = branch_event(host.ports.clock_ns(), view="side", role="side_base", tag=3) - if kind == "fixed_projection": - row["pnp_observation_evidence"]["candidate_diagnostics"].update( - pose_constraint="fixed_reference", fixed_reference_verified=False, - fixed_reference_reprojection_error_px=2.1) - elif kind == "unstarted": - row = branch_event(host.ports.clock_ns(), view="front", role="thumb_pitch", tag=1) - elif kind == "pending": - row = branch_event(host.ports.clock_ns(), reason="pose_branch_unconfirmed") - elif kind == "ambiguous": - row = branch_event(host.ports.clock_ns(), reason="pose_branch_ambiguous") - elif kind == "fixed_unverified": - row = branch_event(host.ports.clock_ns(), view="front", role="front_base", tag=0, - reason="fixed_reference_pose_unverified") - row["pnp_observation_evidence"]["candidate_diagnostics"].update( - branch_status="fixed_reference_unavailable", pose_constraint="fixed_reference", - fixed_reference_verified=False, fixed_reference_reprojection_error_px=1.6233) - elif kind == "recovering": - row = branch_event(host.ports.clock_ns(), reason="pose_branch_recovery_required") - elif kind == "stale_metadata": - row["pnp_observation_evidence"]["candidate_diagnostics"]["observation_stamp_ns"] -= 1 - monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: ((deepcopy(row),), None)) - host.receive_detections(DetectionInput(row["view"], host.ports.clock_ns(), ())) - assert host.raw_records[0]["kind"] == "session_start" - assert [record for record in host.raw_records[1:] if record["kind"] != "image_observation_frame"] == [row] - assert not [record for record in host._unit_rows if record["kind"] != "image_observation_frame"] - assert host._steady_rows == [] - assert (host.execution.session.phase == Phase.PAUSED) is paused - if kind == "fixed_unverified": - assert ("front", "front_base") in host._branch_observation_reasons - yaw = next(t for t in host.profile.motion.tasks if t.key == "thumb_yaw_top") - front = next(t for t in host.profile.motion.tasks if t.view == "front") - assert host._task_branch_details(yaw) == "" - assert "fixed_reference_pose_unverified" in host._task_branch_details(front) - if paused: - assert f"view={row['view']}:tag={row['tag_id']}" in host.reason - expected_code = "OBS-BASE-POSE-110" if kind == "fixed_projection" else "OBS-POSE-BRANCH-109" - assert reason_zh({"reason": host.reason}, model_name="O6")[0] == expected_code - count = len(clock.positions) - host.tick() - assert len(clock.positions) == count - assert not host.finalization.started - finally: - host.close() - - -def test_late_branch_conflict_cannot_pause_or_append_after_abort(tmp_path, monkeypatch): - host, clock = coordinator_fixture(tmp_path) - entered, release = Event(), Event() - try: - begin_yaw(host, clock) - def consume(frame, *a, **kw): - entered.set() - assert release.wait(2) - return (branch_event(frame.stamp_ns),), None - monkeypatch.setattr(host.capture, "consume", consume) - with ThreadPoolExecutor(1) as pool: - future = pool.submit(host.receive_detections, DetectionInput("top", host.ports.clock_ns(), ())) - assert entered.wait(2) - assert host.abort().success - release.set() - future.result(timeout=2) - assert host.execution.session.phase == Phase.ABORTED - assert [row["kind"] for row in host.raw_records] == ["session_start"] - assert not host._branch_observation_reasons - finally: - release.set() - host.close() - - -@pytest.mark.parametrize("invalid", ["mixed_revision", "missing_evidence", "stale_stamp", "wrong_role", "tracker_pending"]) -def test_joint_zero_cannot_freeze_unconfirmed_or_mixed_branch_evidence(tmp_path, invalid): - host, _ = coordinator_fixture(tmp_path) - try: - name = host.profile.motion.tasks[0].joints[0] - rows = reference_rows(host.profile, name, stamp=100) - confirm_reference_rows(host, rows) - if invalid == "mixed_revision": - rows[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"]["branch_revision"] += 1 - elif invalid == "missing_evidence": - rows[-1].pop("pnp_observation_evidence") - elif invalid == "stale_stamp": - rows[-1]["pnp_observation_evidence"]["child"]["candidate_diagnostics"]["observation_stamp_ns"] -= 1 - elif invalid == "wrong_role": - rows[-1]["pnp_observation_evidence"]["child"]["tag_role"] = "top_base" - else: - host.trackers["top"]._branches.states["thumb_yaw"].status = "pending" - host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in rows] - host._observation_epoch = host._segment_number = 1 - assert not host._lock_joint_zeros(MotionCommand('joint_zero', - host.profile.command.baseline_values, 40., - task_key=host.profile.motion.joint_zero_references[name].task_key, zero_joints=(name,))) - expected = {"mixed_revision": "joint_zero_motion_evidence_changed", - "missing_evidence": "joint_zero_motion_evidence_incomplete", - "stale_stamp": "joint_zero_motion_evidence_unconfirmed", - "wrong_role": "joint_zero_motion_evidence_unconfirmed", - "tracker_pending": "joint_zero_motion_branch_not_confirmed"}[invalid] - assert expected in host._zero_reference_error - assert not host.execution.zero_references - assert all(not state.frozen for state in host.trackers["top"]._branches.states.values()) - finally: - host.close() - - -def test_joint_zero_freezes_both_physical_roles_with_one_revision(tmp_path): - host, _ = coordinator_fixture(tmp_path) - try: - name = host.profile.motion.tasks[0].joints[0] - rows = reference_rows(host.profile, name, stamp=100) - confirm_reference_rows(host, rows) - host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in rows] - host._observation_epoch = host._segment_number = 1 - assert host._lock_joint_zeros(MotionCommand('joint_zero', - host.profile.command.baseline_values, 40., - task_key=host.profile.motion.joint_zero_references[name].task_key, zero_joints=(name,))) - assert set(host.trackers["top"]._branches.states) == {"top_base", "thumb_yaw"} - assert all(state.frozen for state in host.trackers["top"]._branches.states.values()) - assert len(host.execution.zero_references[name].samples) == 10 - finally: - host.close() - - -def test_fixed_pose_buffer_discards_old_revision_before_atomic_freeze(): - profile, lock, capture = make_capture() - class Sensor(IdealPoseTracker): - revision = 0 - def estimate(self, role, corners, *, stamp_ns, **kwargs): - pose = SquareTagPose((0., 0., 0., 1.), (0., 0., .45 + self.revision*.01), 0.) - self.accept_ideal_pose(role, pose, stamp_ns) - self._branches.states[role].revision = self.revision - self.last_candidate_diagnostics_by_role[role]["branch_revision"] = self.revision - return pose, "" - sensor = capture.trackers["front"] = Sensor() - lock.begin_session() - lock.start_locking() - for stamp in range(1, 10): - capture.consume(frame(profile, stamp, omit=("thumb_pitch", "thumb_dip"))) - sensor.revision = 1 - capture.consume(frame(profile, 10, omit=("thumb_pitch", "thumb_dip"))) - assert "front" not in capture.locked_poses - for stamp in range(11, 20): - capture.consume(frame(profile, stamp, omit=("thumb_pitch", "thumb_dip"))) - assert capture.locked_revisions == {"front": 1} - assert capture.locked_poses["front"].translation_xyz_m[2] == pytest.approx(.46) - assert sensor._branches.states["front_base"].frozen - - -def test_visible_but_quality_rejected_base_cannot_be_replaced_by_cache(): - profile, lock, capture = make_capture() - lock.begin_session() - lock.start_locking() - for stamp in range(1, 15): - capture.consume(frame(profile, stamp)) - observation = replace(frame(profile, 20, omit=("front_base",)), observed_roles=frozenset({"front_base"})) - motion = MotionCommand("sweep", profile.command.baseline_values, 40., - "thumb_pitch_dip_front", 0, 0, "decreasing") - rows, _ = capture.consume(observation, motion) - assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) - - -@pytest.mark.parametrize("recovery_evidence", ["event", "sample"]) -def test_recovered_observation_clears_pending_error_and_final_quality_details(tmp_path, monkeypatch, recovery_evidence): - host, clock = coordinator_fixture(tmp_path) - try: - begin_yaw(host, clock) - row = branch_event(host.ports.clock_ns(), reason="pose_branch_recovery_required") - monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: ((deepcopy(row),), None)) - host.receive_detections(DetectionInput("top", host.ports.clock_ns(), ())) - task = host.profile.motion.tasks[0] - assert "top:tag=7:pose_branch_recovery_required" in host._task_branch_details(task) - assert host.execution.session.phase == Phase.SWEEP - clock.now += .04 - stamp = host.ports.clock_ns() - diagnostics = {"observation_stamp_ns": stamp, "branch_revision": 0, "branch_status": "tracking"} - if recovery_evidence == "event": - row = branch_event(stamp, reason="pose_branch_recovered") - row["pnp_observation_evidence"]["candidate_diagnostics"] = diagnostics - else: - row = {"kind": "joint_sample", "view": "top", "image_stamp_ns": stamp, - "pnp_observation_evidence": {"child": {"tag_role": "thumb_yaw", "candidate_diagnostics": diagnostics}}} - host.receive_detections(DetectionInput("top", stamp, ())) - assert not host._branch_observation_reasons - assert host._task_branch_details(task) == "" - assert host.execution.session.phase == Phase.SWEEP - finally: - host.close() - - -def test_pending_fixed_branch_cannot_use_cache_when_next_image_is_missing(): - profile, lock, capture = make_capture() - lock.begin_session() - lock.start_locking() - for stamp in range(1, 15): - capture.consume(frame(profile, stamp)) - tracker = capture.trackers["front"] - tracker._branches.states["front_base"].status = "pending" - motion = MotionCommand("sweep", profile.command.baseline_values, 40., - "thumb_pitch_dip_front", 0, 0, "decreasing") - rows, _ = capture.consume(frame(profile, 20, omit=("front_base",)), motion) - assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) - - -def test_occluded_fixed_corners_discard_dependent_samples_until_stable_recovery(): - profile, lock, capture = make_capture() - capture.profile = profile = replace(profile, measurement=replace(profile.measurement, - candidate_selection_tasks=frozenset({"thumb_pitch_dip_front"}))) - lock.begin_session() - lock.start_locking() - for stamp in range(1, 15): - capture.consume(frame(profile, stamp)) - original = capture.locked_reference("front") - motion = MotionCommand("sweep", profile.command.baseline_values, 40., - "thumb_pitch_dip_front", 0, 0, "decreasing") - for stamp in range(20, 30): - observation = frame(profile, stamp) - observation.corners["front_base"][0] += [0., 4.] - rows, movement = capture.consume(observation, motion) - assert movement is None # Local corner damage, below the drift gate. - assert len(rows) == 1 and rows[0]["kind"] == "pnp_branch_event" - assert rows[0]["reason"] == "fixed_reference_pose_unverified" - assert rows[0]["motion_phase"] == "sweep" - assert rows[0]["command_vector"] == list(observation.command) - assert rows[0]["feedback_vector"] == list(observation.feedback) - assert capture.locked_reference("front") == original - # Missing detection must not masquerade as a recovered visible reference. - rows, _ = capture.consume(frame(profile, 30, omit=("front_base",)), motion) - assert not any(row.get("joint") == "rh_thumb_cmc_pitch" for row in rows) - for stamp in range(31, 36): - rows, movement = capture.consume(frame(profile, stamp), motion) - samples = [r for r in rows if r.get("joint") == "rh_thumb_cmc_pitch"] - assert movement is None - assert bool(samples) is (stamp == 35) - if samples: - assert samples[0]["pnp_observation_evidence"]["parent"]["pose_source"] == "verified_fixed_reference" - assert capture.locked_reference("front") == original - - -def test_true_fixed_drift_in_unused_view_still_pauses_current_task(tmp_path, monkeypatch): - from linkerhand_calibration.runtime.reference_lock import ReferenceMovement - host, clock = coordinator_fixture(tmp_path) - try: - begin_yaw(host, clock) - movement = ReferenceMovement("front", 0, 6.0, 10) - monkeypatch.setattr(host.capture, "consume", lambda *a, **kw: ((), movement)) - host.receive_detections(DetectionInput("front", host.ports.clock_ns(), ())) - assert host.execution.session.phase == Phase.PAUSED - assert host.reason == "fixed_reference_moved:front:6.0" - assert not host.finalization.started - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_runtime_readiness.py b/src/linkerhand_calibration/test/test_runtime_readiness.py deleted file mode 100644 index 9a94bfd..0000000 --- a/src/linkerhand_calibration/test/test_runtime_readiness.py +++ /dev/null @@ -1,104 +0,0 @@ -"""Device readiness and progress must reflect evidence, for any view layout.""" - -from dataclasses import asdict, replace -from types import MethodType, SimpleNamespace -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.adapters import HardwareHealth -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator -from linkerhand_calibration.runtime.status import normalize_status, render_status_zh - - -def test_missing_stale_and_recovered_camera_pipeline_are_named_without_tag_gate(): - profile = load_bundled_hand_profile("o6_right_8") - host = SimpleNamespace(profile=profile, command_count=6, latest_feedback=(0,)*6, - required_observation_views=profile.vision.view_names, - ports=SimpleNamespace(command_publisher_count=lambda: 1), - sdk_adapter=SimpleNamespace(health=lambda: HardwareHealth(True, True)), - cameras=SimpleNamespace(matrices={"front": True, "side": True, "top": True}, - info_received_at={"front": 10., "side": 10., "top": 5.}, - detections_received_at={"side": 10.}, errors={})) - status = asdict(CalibrationCoordinator._device_status(host, 10.)) - assert not status["ready"] - assert not status["cameras"]["front"]["detections_ready"] - assert status["cameras"]["side"]["detections_ready"] - assert not status["cameras"]["top"]["camera_info_ready"] - text = render_status_zh(normalize_status({"state": "WAIT_DEVICES", "devices": status}, - profile=profile, serial_number="12345")) - assert "O6 / right / 12345" in text - assert "front 未收到" in text and "top 未收到" in text - # Readiness requires messages, not recognized Tag IDs or a rate floor. - host.cameras.info_received_at = dict.fromkeys(profile.vision.view_names, 10.) - host.cameras.detections_received_at = dict(host.cameras.info_received_at) - assert CalibrationCoordinator._device_status(host, 10.).ready - host.ports.command_publisher_count = lambda: 2 - assert "存在其他机械手指令发布者" in CalibrationCoordinator._device_status(host, 10.).waiting_for - - -def test_steady_checkpoint_does_not_erase_direction_statistics(): - from linkerhand_calibration.runtime.branch_initialization import BranchInitialization - host = SimpleNamespace(_steady_rows=[], _unit_rows=[], _capture_unit_key=None, - capture=SimpleNamespace(begin_motion=lambda motion: None), - branch_initialization=BranchInitialization(load_bundled_hand_profile("o6_right_8")), - _observation_epoch=1, _segment_number=1) - motion = MotionCommand("sweep", (0.,), 1., task_key="test", command_index=0, - cycle=0, direction="decreasing") - begin = MethodType(CalibrationCoordinator._begin_motion_capture, host) - begin(motion) - host._unit_rows.append({"sample_phase": "sweep"}) - begin(replace(motion, phase="steady", steady_index=0)) - host._steady_rows.append({"sample_phase": "steady"}) - host._unit_rows.extend(host._steady_rows) - begin(replace(motion, phase="steady", steady_index=1)) - assert len(host._unit_rows) == 2 - assert host._steady_rows == [] - begin(replace(motion, attempt=2)) - assert host._unit_rows == [] - host._unit_rows.append({"sample_phase": "sweep"}) - begin(replace(motion, attempt=2, segment_key="next_segment")) - assert host._unit_rows == [] - - -def test_ros_io_loads_profile_views_instead_of_three_camera_compatibility(tmp_path): - import json - from linkerhand_calibration.core.geometry.extrinsics import camera_info_fingerprint - from linkerhand_calibration.runtime.ros.parameters import parameter_defaults, load_runtime_parameters - profile = load_bundled_hand_profile("o6_right_8") - vision = replace(profile.vision, - views=(replace(profile.vision.views[0], name="inspection"),), - extrinsic_reference_view="inspection", extrinsics_quality_limits={}, minimum_capture_counts={}) - profile = replace(profile, vision=vision) - extrinsics = tmp_path / "extrinsics.json" - extrinsics.write_text(json.dumps({"schema_version": 1, "reference_view": "inspection", - "quality": {"passed": True}, - "cameras": {"inspection": {"serial_number": "test", "width": 640, "height": 480, - "intrinsics_sha256": camera_info_fingerprint(width=640, height=480, - camera_matrix=[500,0,320,0,500,240,0,0,1])}}, - "inspection_from_view": {"inspection": {"translation_xyz_m": [0,0,0], - "quaternion_xyzw": [0,0,0,1]}}})) - params = parameter_defaults(profile) - params.update(serial_number="test", session_dir=str(tmp_path), source_urdf_path=str(extrinsics), - camera_extrinsics_file=str(extrinsics)) - for key in profile.artifacts.protected_input_fields: - params[key.replace("_sha256", "_expected_sha256")] = "a"*64 - resolved = load_runtime_parameters(profile, params.__getitem__) - assert set(resolved.extrinsics.cameras) == {"inspection"} - assert set(resolved.detection_topics) == {"inspection"} - - -@pytest.mark.parametrize("schema", [1, 4, 6, 7]) -def test_production_cannot_fall_back_to_feedback_only_legacy_output(tmp_path, schema): - from pathlib import Path - from linkerhand_calibration.profiles.validator import validate_executable_profile - from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session - profile = load_bundled_hand_profile("o6_right_8") - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=schema)) - source = Path(__file__).resolve().parents[1] / "urdf/o6_right/linkerhand_o6_right.urdf" - with pytest.raises(ValueError, match="unified output_schema_version"): - validate_executable_profile(profile, source) - with pytest.raises(ValueError, match="unified output_schema_version"): - finalize_profile_session(profile=profile, session_dir=tmp_path / "session", serial_number="test", - source_urdf=source, protected_inputs={}, records=()) - assert not (tmp_path / "session").exists() diff --git a/src/linkerhand_calibration/test/test_safety_policy.py b/src/linkerhand_calibration/test/test_safety_policy.py deleted file mode 100644 index b50387f..0000000 --- a/src/linkerhand_calibration/test/test_safety_policy.py +++ /dev/null @@ -1,117 +0,0 @@ -from __future__ import annotations - -from dataclasses import replace - -import pytest - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry -from linkerhand_calibration.runtime import SafetyPolicy, SafetySample -from linkerhand_calibration.runtime.adapters import HardwareHealth -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh -from linkerhand_calibration.runtime.safety import MotionGoal - - -@pytest.fixture(params=["g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"]) -def profile(request): - return next(item.profile for item in get_default_registry() - if item.profile.key.layout == request.param) - - -def test_normal_lag_coupling_and_low_visual_rate_are_not_safety_inputs(profile) -> None: - decision = SafetyPolicy(profile).evaluate( - SafetySample( - now_seconds=10.0, - feedback_timestamp_seconds=9.5, - command=profile.command.baseline_values, - feedback=tuple(value + (0.01 if profile.command.unit == "rad" else -1) - for value in profile.command.baseline_values), - health=HardwareHealth(True, True), - ) - ) - assert decision.safe - - -def test_stale_feedback_hardware_fault_and_reference_motion_stop(profile) -> None: - policy = SafetyPolicy(profile) - common = dict( - now_seconds=10.0, - command=profile.command.baseline_values, - feedback=profile.command.baseline_values, - health=HardwareHealth(True, True), - ) - assert policy.evaluate( - SafetySample(feedback_timestamp_seconds=8.9, **common) - ).code == "feedback_stale" - assert policy.evaluate( - SafetySample( - feedback_timestamp_seconds=9.9, - **{**common, "health": HardwareHealth(True, True, ("overheat",))}, - ) - ).code == "hardware_fault" - assert policy.evaluate( - SafetySample( - feedback_timestamp_seconds=9.9, - **common, - reference_moved=True, - ) - ).code == "fixed_reference_moved" - - -def test_two_second_no_progress_stops_but_ordinary_lag_does_not(profile) -> None: - policy = SafetyPolicy(profile) - - def sample(now: float, feedback: float) -> SafetySample: - values = list(profile.command.baseline_values) - values[0] = feedback if profile.command.unit == "rad" else 255 - 100 * feedback - command = list(profile.command.baseline_values) - command[0] = 0.8 if profile.command.unit == "rad" else 0 - return SafetySample( - now_seconds=now, - feedback_timestamp_seconds=now, - command=tuple(command), - feedback=tuple(values), - health=HardwareHealth(True, True), - motion_expected=True, - target_channel=0, - target_value=command[0], - ) - - assert policy.evaluate(sample(0.0, 0.0)).safe - assert policy.evaluate(sample(1.0, 0.1)).safe - assert policy.evaluate(sample(2.5, 0.1)).safe - stopped = policy.evaluate(sample(3.1, 0.1)) - assert not stopped.safe - assert stopped.code == "mechanical_stall" - - -@pytest.mark.parametrize("timeout", [2., 4.]) -def test_configured_timeout_preserves_no_progress_stop_and_reports_actual_duration(timeout): - profile = load_bundled_hand_profile("o30_right_18", scope="full") - profile = replace(profile, acquisition=replace(profile.acquisition, stall_timeout_seconds=timeout)) - policy = SafetyPolicy(profile) - - def sample(now, command_value=223., feedback_value=253.): - command = list(profile.command.baseline_values) - feedback = command.copy() - command[0], feedback[0] = command_value, feedback_value - return SafetySample(now, now, tuple(command), tuple(feedback), HardwareHealth(True, True), - motion_expected=True, motion_id="endpoint_return", - motion_goals=(MotionGoal(0, 253., 3., .5, .5),)) - - assert policy.evaluate(sample(0.)).safe - # More than 30 units of tracking error does not cause the stop, and new - # streaming commands must not restart the no-progress timer. - assert policy.evaluate(sample(timeout-.01, command_value=190.)).safe - stopped = policy.evaluate(sample(timeout, command_value=180.)) - assert stopped.code == "mechanical_stall" - assert f"timeout_seconds={timeout:g}" in stopped.details - assert "command=180,feedback=253,feedback_progress_goal=3," in stopped.details - explanation = reason_zh({"reason": f"{stopped.code}:{stopped.reason}:{stopped.details}"}, model_name="O30") - assert f"连续 {timeout:g} 秒" in explanation[1] - - policy = SafetyPolicy(profile) - assert policy.evaluate(sample(0.)).safe - # Genuine late feedback progress earns a new interval; command updates do not. - assert policy.evaluate(sample(timeout-.1, feedback_value=252.)).safe - assert policy.evaluate(sample(timeout+.1, feedback_value=251.)).safe diff --git a/src/linkerhand_calibration/test/test_sample_schema.py b/src/linkerhand_calibration/test/test_sample_schema.py deleted file mode 100644 index fd26fb0..0000000 --- a/src/linkerhand_calibration/test/test_sample_schema.py +++ /dev/null @@ -1,69 +0,0 @@ -"""Tests for the explicit calibration sample command domains.""" - -from linkerhand_calibration.sample_schema import ( - SampleDataContractError, - canonical_sample_record, - fitting_sample_record, - fitting_sample_records, -) - -import pytest - - -def _sample(kind: str = 'sample') -> dict[str, object]: - return { - 'kind': kind, - 'joint': 'thumb_cmc_roll', - 'cycle': 0, - 'direction': 'decreasing', - 'requested_command_u8': 0, - 'feedback_u8': 17.6, - } - - -def test_online_and_serialized_replay_use_identical_fit_projection() -> None: - """Serialization must not change the curve fitter's input record.""" - durable = canonical_sample_record(_sample()) - online = fitting_sample_record(durable) - serialized = canonical_sample_record(online) - replay = fitting_sample_record(serialized) - - assert 'command_u8' not in durable - assert serialized == durable - assert replay == online - assert online['command_u8'] == 18 - - -def test_default_domain_is_requested_only_for_steady_checkpoints() -> None: - """Settled checkpoints and dense trajectories use declared domains.""" - steady = _sample('steady_command_sample') - - assert fitting_sample_record(steady)['command_u8'] == 0 - assert fitting_sample_record(steady, domain='feedback')['command_u8'] == 18 - - -def test_feedback_projection_can_snap_proven_requested_endpoints() -> None: - """A settled endpoint remains exact after feedback-domain projection.""" - records = fitting_sample_records( - [_sample()], - domain='feedback', - snap_requested_endpoints=True, - ) - - assert records[0]['command_u8'] == 0 - assert records[0]['feedback_u8'] == pytest.approx(17.6) - - -def test_new_sample_rejects_missing_explicit_feedback_domain() -> None: - """A new observation cannot rely on the compatibility command key.""" - ambiguous = { - 'kind': 'sample', - 'requested_command_u8': 0, - 'command_u8': 0, - } - - with pytest.raises( - SampleDataContractError, - match='DATA-CONTRACT-701.*feedback_u8', - ): - canonical_sample_record(ambiguous) diff --git a/src/linkerhand_calibration/test/test_separate_mapping_capture.py b/src/linkerhand_calibration/test/test_separate_mapping_capture.py deleted file mode 100644 index d1644e0..0000000 --- a/src/linkerhand_calibration/test/test_separate_mapping_capture.py +++ /dev/null @@ -1,139 +0,0 @@ -"""Continuous four-round motion and independent move-and-hold mapping.""" - -from collections import defaultdict -from dataclasses import replace - -import numpy as np -import pytest - -from linkerhand_calibration.core.domain.sampling import command_nodes -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit -from runtime_host_fixture import coordinator_fixture, ready -from test_diagnostic_coordinator import advance, TERMINAL_PHASES -from test_single_pass_capture import CapturedFinalization - - -def separate(profile): - return replace(profile, acquisition=replace(profile.acquisition, - command_capture_mode="separate", steady_training_nodes=9, steady_extra_training_nodes={})) - - -def test_four_continuous_rounds_and_separate_mapping_reach_finalization(tmp_path, monkeypatch): - original = CalibrationEngine.scan_units - monkeypatch.setattr(CalibrationEngine, "scan_units", lambda engine: - tuple(unit for unit in original(engine) if unit.task_key == "thumb_yaw_top")) - finalizer = CapturedFinalization() - host, clock = coordinator_fixture(tmp_path, finalization=finalizer, profile_transform=separate) - 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 - entries, seen = defaultdict(list), set() - try: - for _ in range(9000): - if finalizer.started: - break - assert host.execution.session.phase not in TERMINAL_PHASES, host.reason - advance(host, clock) - motion = host._motion - if motion is not None and motion.phase in {"sweep", "steady"} and host._segment_number not in seen: - seen.add(host._segment_number) - entries[(motion.cycle, motion.direction)].append(motion) - assert finalizer.started and host.execution.session.phase == Phase.FIT, host.reason - assert len(host.execution.completed_units) == 12 - units = host.execution.session.engine.scan_units() - records = finalizer.inputs[0].records - spans = {} - for unit in units: - motions = entries[(unit.cycle, unit.direction)] - if unit.sample_phase == "sweep": - assert len(motions) == 1 and motions[0].phase == "sweep" - assert motions[0].target[motions[0].command_index] == unit.end - else: - assert all(m.phase == "steady" for m in motions) - plan = host.execution.sampling_plans[unit.task_key] - assert len(motions) == (len(plan["training_nodes"]) if unit.cycle == 4 else 10) - quality = evaluate_capture_unit(host.profile, unit, 1, records, first_cycle_spans=spans) - assert quality.passed, quality.failures - samples = [r for r in records if r.get("kind") == "joint_sample"] - moving = [r for r in samples if r["sample_phase"] == "sweep"] - steady = [r for r in samples if r["sample_phase"] == "steady"] - assert {r["cycle"] for r in moving} == {0, 1, 2, 3} - assert {r["cycle"] for r in steady} == {4, 5} - assert {r["image_stamp_ns"] for r in moving}.isdisjoint(r["image_stamp_ns"] for r in steady) - assert all(r["command_u8"] == r["steady_target"] for r in steady) - assert len(host.execution.sampling_plans["thumb_yaw_top"]["training_nodes"]) < 9 - from linkerhand_calibration.core.fitting.command_sampling import validate_sampling_plans - validate_sampling_plans(host.profile, records) - assert any(r.get("kind") == "preparation_reused_settled_pose" for r in records) - incomplete = [r for r in records if not (r.get("sample_phase") == "steady" - and r.get("cycle") == 5 and r.get("steady_index") == 1)] - assert not evaluate_capture_unit(host.profile, units[-1], 1, incomplete, first_cycle_spans=spans).passed - assert not list(tmp_path.rglob("release_manifest.json")) - monkeypatch.setattr(host.capture, "_motion_model_schedule", - lambda *_: pytest.fail("finalization must not continue solving moving joints")) - advance(host, clock) - assert host.execution.session.phase == Phase.FIT - from test_diagnostic_coordinator import projected_frame - for _ in range(host.profile.acquisition.fixed_reference_confirmation_frames): - clock.now += .04 - frame = projected_frame(host, "top", host.last_command) - moved = tuple(replace(tag, corners=tuple((x + 10, y) for x, y in tag.corners)) - if tag.tag_id == 6 else tag for tag in frame.detections) - host.receive_detections(replace(frame, detections=moved)) - # Acquisition has closed. These later images are outside the frozen - # dataset and must not interrupt its numerical/file validation. - assert host.execution.session.phase == Phase.FIT - assert not any(row.get("kind") == "paused" for row in host.raw_records) - finally: - host.close() - - -def test_extra_endpoint_node_keeps_validation_independent(): - from linkerhand_calibration.profiles import load_bundled_hand_profile - profile = separate(load_bundled_hand_profile("o6_right_8")) - profile = replace(profile, acquisition=replace(profile.acquisition, - steady_extra_training_nodes={"pinky_pitch_dip_side": (239.,)})) - task = next(t for t in profile.motion.tasks if t.key == "pinky_pitch_dip_side") - training = set(command_nodes(profile, task)) - validation = set(command_nodes(profile, task, holdout=True)) - assert 239 in training and 247 in validation - assert training & validation == {0., 255.} - assert len(training) == 10 and len(validation) == 11 - - -@pytest.mark.parametrize("model", ["o6", "g20"]) -@pytest.mark.parametrize("changed", ["feedback", "age", "target", "command", "phase", "missing"]) -def test_preparation_cannot_reuse_changed_or_expired_settling(tmp_path, monkeypatch, model, changed): - from linkerhand_calibration.runtime.motion_execution import MotionCommand - host, clock = coordinator_fixture(tmp_path, model=model) - ready(host, clock) - target = tuple(host.profile.command.baseline_values) - feedback = tuple(host.latest_feedback) - host.last_command = target - host._settled_pose = (target, feedback, clock.now) - motion = MotionCommand("prepare", target, 1.) - delta = .002 if host.command_unit == "rad" else 2. - altered = (target[0] + delta, *target[1:]) - if changed == "feedback": - clock.now += .01 - host.receive_feedback(host.profile.command.names, - (feedback[0] + delta, *feedback[1:]), host.ports.clock_ns()) - elif changed == "age": - clock.now += 1.01 - elif changed == "target": - motion = replace(motion, target=altered) - elif changed == "command": - host.last_command = altered - elif changed == "phase": - motion = replace(motion, phase="zero_approach") - else: - host._settled_pose = None - monkeypatch.setattr(host.execution, "motion_complete", - lambda: pytest.fail("this preparation must run its normal motion and settling checks")) - try: - assert not host._reuse_settled_preparation(motion, clock.now) - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_session_execution.py b/src/linkerhand_calibration/test/test_session_execution.py deleted file mode 100644 index c40583f..0000000 --- a/src/linkerhand_calibration/test/test_session_execution.py +++ /dev/null @@ -1,185 +0,0 @@ -"""Virtual-clock tests use the same effect driver intended for the ROS host.""" - -import math - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.motion_execution import MotionExecution -from linkerhand_calibration.runtime.scan_quality import observation_streams -from linkerhand_calibration.runtime.steady import steady_targets - - -@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "o12_right_16", "g20_right_19"]) -@pytest.mark.parametrize("capture_mode", ["interleaved", "separate"]) -def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout, capture_mode): - profile = load_bundled_hand_profile(layout) - from dataclasses import replace - profile = replace(profile, acquisition=replace(profile.acquisition, command_capture_mode=capture_mode)) - profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2, - directional_command_joints=frozenset())) - driver = SessionExecution(profile) - driver.session.device_ready() - driver.session.start() - command = profile.command.baseline_values - rows, stamp, failed_once, directions = [], 0, False, {} - for _ in range(4000): - phase = driver.session.phase - if phase == Phase.FIT: - break - if phase == Phase.REFERENCE_LOCKING: - driver.session.reference_locked() - continue - if phase == Phase.EVALUATE: - driver.evaluate(rows) - continue - motion = driver.motion(command) - assert motion is not None, phase - if phase in {Phase.SWEEP, Phase.RESCAN}: - assert motion.phase != "steady_prepare" - initial = command - command = motion.target - if motion.recording: - unit = driver.action.scan_unit - task = next(t for t in profile.motion.tasks if t.key == unit.task_key) - key = (unit.task_key, unit.cycle, unit.direction, motion.attempt) - directions.setdefault(key, []).append((unit, initial, motion)) - # The first direction fails once; no speed reduction or extra retry. - # Omit its first steady point, keeping all moving observations. - if not failed_once and (capture_mode == "separate" or motion.phase == "steady"): - failed_once = True - else: - start = initial[task.command_index] - end = command[task.command_index] - count = (max(3, math.ceil(128*abs(end-start)/abs(unit.end-unit.start))+1) - if motion.phase == "sweep" else profile.acquisition.steady_minimum_samples) - for field, joint, spec in observation_streams(profile, task): - for index in range(count): - stamp += 1 - feedback = start+(end-start)*index/(count-1) if motion.phase == "sweep" else end - rows.append({field: joint, "view": spec.view, "task_name": task.key, - "cycle": unit.cycle, "direction": unit.direction, - "attempt": motion.attempt, "image_stamp_ns": stamp, - "sample_phase": motion.phase, "steady_index": motion.steady_index, - "steady_target": motion.target[task.command_index], - f"feedback_{profile.command.unit}": feedback}) - driver.motion_complete() - assert driver.session.phase == Phase.FIT - unit_count = 12 if capture_mode == "separate" else 8 - assert len(driver.completed_units) == len(profile.motion.tasks)*unit_count - assert len(directions) == len(profile.motion.tasks)*unit_count+1 - for key, trace in directions.items(): - unit = trace[0][0] - task = next(t for t in profile.motion.tasks if t.key == unit.task_key) - points = steady_targets(profile, unit) - if capture_mode == "interleaved": - assert [motion.phase for _, _, motion in trace] == ["sweep", "steady"]*len(points) - elif unit.sample_phase == "sweep": - assert len(trace) == 1 and trace[0][2].phase == "sweep" - points = () - else: - assert all(motion.phase == "steady" for _, _, motion in trace) - held = [motion for _, _, motion in trace if motion.phase == "steady"] - if capture_mode == "interleaved": - sweeps = [motion for _, _, motion in trace if motion.phase == "sweep"] - assert all(motion.defer_settling_to_hold for motion in sweeps[1:-1]) - assert not sweeps[0].defer_settling_to_hold and not sweeps[-1].defer_settling_to_hold - assert tuple(motion.target[task.command_index] for motion in held) == points - assert [motion.steady_index for motion in held] == list(range(len(points))) - assert trace[0][1][task.command_index] == pytest.approx(unit.start) - assert trace[-1][2].target[task.command_index] == pytest.approx(unit.end) - travelled = 0.0 - sign = 1.0 if unit.direction == "increasing" else -1.0 - for _, initial, motion in trace: - delta = motion.target[task.command_index]-initial[task.command_index] - if motion.phase == "sweep": - assert sign*delta > 0 or (capture_mode == "interleaved" and (initial, motion) == trace[0][1:]) - elif capture_mode == "interleaved": - assert delta == pytest.approx(0.) - else: - assert sign*delta >= 0 - assert all(motion.target[i] == initial[i] for i in range(profile.command.command_count) - if i != task.command_index) - assert motion.speed == unit.speed - travelled += abs(delta) - assert travelled == pytest.approx(abs(unit.end-unit.start)) - for task in profile.motion.tasks: - units = [trace[0][0] for key, trace in directions.items() if key[0] == task.key and key[3] == 1] - assert len(units) == unit_count - assert len([unit for unit in units if unit.cycle < 4]) == 8 - assert all({unit.direction for unit in units if unit.cycle == cycle} - == {"increasing", "decreasing"} for cycle in (0, 1, 2, 3)) - distance = sum(abs(motion.target[task.command_index]-initial[task.command_index]) - for key, trace in directions.items() if key[0] == task.key and key[3] == 1 - for _, initial, motion in trace) - assert distance == pytest.approx(unit_count*abs(task.end_value-task.start_value)) - retried = [key for key in directions if key[3] > 1] - assert len(retried) == 1 and retried[0][3] == 2 - first_key = (*retried[0][:3], 1) - assert [motion.speed for _, _, motion in directions[first_key]] == [ - motion.speed for _, _, motion in directions[retried[0]]] - - -def test_feedback_bias_does_not_turn_completed_travel_into_a_stall(): - profile = load_bundled_hand_profile("o12_right_16") - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from linkerhand_calibration.runtime.safety import SafetyPolicy, SafetySample - from linkerhand_calibration.runtime.adapters import HardwareHealth - start = list(profile.command.baseline_values) - feedback = start.copy() - feedback[1] = -0.023 - target = start.copy() - target[1] = -0.1 - segment = MotionExecution(profile, MotionCommand("prepare", tuple(target), .04), - initial_command=start, initial_feedback=feedback, now=0, identity="one-segment") - policy = SafetyPolicy(profile) - for index in range(1000): - now = index*.02 - command = segment.sample(now) - feedback[1] = -.023+command[1] - segment.observe(feedback, stamp=index, now=now) - assert policy.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), - motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)).safe - assert segment.arrived(now) - - -def test_duplicate_images_and_missing_secondary_view_fail_after_motion_only(): - profile = load_bundled_hand_profile("o12_right_16") - from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit - from linkerhand_calibration.runtime.engine import CalibrationEngine - unit = next(u for u in CalibrationEngine(profile).scan_units() if u.task_key == "middle_roll_front") - rows = [{"joint": "middle_mcp_roll", "view": "front", "task_name": unit.task_key, - "cycle": 0, "direction": unit.direction, "attempt": 1, "image_stamp_ns": 1, - "feedback_rad": unit.start}]*100 - quality = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}) - assert not quality.passed - assert any("observation_joint" in item for item in quality.failures) - assert any("frames=1" in item for item in quality.failures) - - -def test_tiny_baseline_bias_and_mapping_jog_have_different_completion_rules(): - from linkerhand_calibration.runtime.motion_execution import MotionCommand - profile = load_bundled_hand_profile("o12_right_16") - start = list(profile.command.baseline_values) - start[0], start[1] = .021, -.023 - segment = MotionExecution(profile, MotionCommand("baseline", profile.command.baseline_values, .1), - initial_command=start, initial_feedback=start, now=0, identity="baseline") - for n in range(50): - now = n*.1 - segment.sample(now) - segment.observe(start, stamp=n, now=now) - assert not segment.motion_expected - assert segment.arrived(now) - target = list(profile.command.baseline_values) - target[1] = -profile.acquisition.mapping_probe_maximum_rad - jog = MotionExecution(profile, MotionCommand("mapping_probe", tuple(target), .06), - initial_command=profile.command.baseline_values, initial_feedback=start, now=0, identity="jog") - moved = start.copy() - moved[1] -= .02 - for n in range(50): - now = n*.1 - jog.sample(now) - jog.observe(moved, stamp=n, now=now) - assert jog.arrived(now) # Basic movement proves mapping; not an 80% accuracy gate. diff --git a/src/linkerhand_calibration/test/test_shared_pose_conditioning.py b/src/linkerhand_calibration/test/test_shared_pose_conditioning.py deleted file mode 100644 index bd9b05e..0000000 --- a/src/linkerhand_calibration/test/test_shared_pose_conditioning.py +++ /dev/null @@ -1,170 +0,0 @@ -"""Measured shared posture prevents independently fitted monocular depth drift.""" - -from copy import deepcopy -from dataclasses import replace -import gzip -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.pose_bridge import ( - SHARED_ZERO_CONDITIONING, SHARED_TAG_POSE_CONDITIONED_POLICY, - check_source_pose, check_pose_bridge, -) -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ImageMotionParameters -from linkerhand_calibration.core.geometry.tag_pose.motion_image_diagnostics import _validate_frames -from linkerhand_calibration.core.geometry.tag_pose.shared_zero_solver import SharedZeroImageHingeBundle -from linkerhand_calibration.core.geometry.tag_pose.hinge_uncertainty import hinge_uncertainty -from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame -from linkerhand_calibration.core.geometry.tag_pose.relative import _relative_pose -from linkerhand_calibration.runtime.branch_initialization import solve_initialization, initialization_artifacts -from linkerhand_calibration.runtime.motion_provenance import _initializations, validate_source_geometry -from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references -from linkerhand_calibration.profiles import load_bundled_hand_profile -from test_shared_tag_pose_bridge import request_from_recording -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope="module", params=["middle", "ring"]) -def measured(request): - path = Path(__file__).parent / f"fixtures/o30_shared_tag_{request.param}_pip.json.gz" - data = json.loads(gzip.decompress(path.read_bytes())) - _, pending = request_from_recording(data) - assert not pending.invalid_reason - result = solve_initialization(pending) - assert result.resolved, result - return data, pending, result - - -def test_full_zero_is_validated_by_independent_current_images_and_new_arc(measured): - _, request, result = measured - bridge = request.pose_bridges[0] - source = check_source_pose(bridge) - assert source.status == "consistent" and source.maximum_translation_m < .0002 - geometry = result.model.geometry[0] - point = np.asarray(geometry.pivot_parent_xyz_m) + Rotation.from_quat( - geometry.reference_quaternion_xyzw).apply(geometry.mount_child_xyz_m) - assert np.allclose(point, bridge.translation_xyz_m, atol=1e-12, rtol=0) - # The new hinge's independently measured axis must not be inherited from MCP. - old_axis = np.asarray(bridge.source_model.geometry[0].axis_parent_xyz) - assert not np.allclose(old_axis, geometry.axis_parent_xyz) - assert len(bridge.frames) == 10 and len(request.image_frames) == 129 - assert not set(result.model.source_stamps_ns) & {f.evidence.stamp_ns for f in bridge.frames} - checked = check_pose_bridge(result.model, bridge) - assert checked.status == "consistent" - assert checked.maximum_translation_m < .0002 - assert np.rad2deg(checked.maximum_rotation_rad) < .4 - for hypothesis in result.hypotheses: - assert hypothesis.converged and not hypothesis.reason - assert hypothesis.pose_bridge_checks[0].status == "consistent" - assert hypothesis.validation_quality[0].rms_px < .4 - assert result.model.maximum_reprojection_error_px == 1.5 - assert all(select_image_motion_frame(result.model, f).resolved for f in request.image_frames[1::2]) - - -def test_recorded_middle_failure_is_reproduced_with_orientation_only_conditioning(): - data = json.loads(gzip.decompress((Path(__file__).parent / "fixtures/o30_shared_tag_middle_pip.json.gz").read_bytes())) - _, request = request_from_recording(data) - bridge = replace(request.pose_bridges[0], conditioning=SHARED_ZERO_CONDITIONING) - result = solve_initialization(replace(request, pose_bridges=(bridge,))) - assert not result.resolved and result.reason == "image_motion_shared_pose_unresolved" - for hypothesis in result.hypotheses: - check = hypothesis.pose_bridge_checks[0] - assert .0052 < check.maximum_translation_m < .0053 - assert np.rad2deg(check.maximum_rotation_rad) < .21 - # The recorded, independently fitted position error is almost all depth. - from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import _fit_hypothesis - limits = ImageMotionParameters() - paths, roles = _validate_frames(request.image_frames, request.relations, limits) - branches = tuple((role, 0) for role in roles) - _, model, _ = _fit_hypothesis(request.image_frames, request.relations, - {role: paths[role][0] for role in roles}, branches, - list(range(0, 129, 2)), list(range(1, 129, 2)), limits, pose_bridges=(bridge,)) - selections = select_image_motion_frame(model, bridge.frames[0]) - poses = {s.role: s.pose for s in selections.selections} - _, point = _relative_pose(poses[bridge.relation.parent_role], poses[bridge.relation.child_role]) - difference = Rotation.from_quat(poses[bridge.relation.parent_role].quaternion_xyzw).apply( - point-np.asarray(bridge.translation_xyz_m)) - assert abs(difference[2]) > .005 and np.linalg.norm(difference[:2]) < .0004 - - -def test_full_uncertainty_restores_orientation_and_translation(measured): - _, request, _ = measured - limits = ImageMotionParameters() - paths, roles = _validate_frames(request.image_frames, request.relations, limits) - bundle = SharedZeroImageHingeBundle(request.image_frames[::2], request.relations, - {role: paths[role][0][::2] for role in roles}, request.pose_bridges) - fit = bundle.solve(limits) - free, linearization = bundle.uncertainty_problem(fit) - assert len(linearization.x) == len(fit.x)+5 - assert np.allclose(bundle.residual(fit.x), free.residual(linearization.x), atol=1e-7) - uncertainty = hinge_uncertainty(bundle, fit) - assert uncertainty == hinge_uncertainty(free, linearization) - assert uncertainty[0][0][1] > .01 and uncertainty[1][0][1] > .0005 - - -@pytest.mark.parametrize("change", ["source_translation", "current_pixels"]) -def test_full_pose_requires_source_to_explain_fresh_independent_images(measured, change): - _, request, _ = measured - bridge = request.pose_bridges[0] - if change == "source_translation": - bridge = replace(bridge, translation_xyz_m=tuple(np.asarray(bridge.translation_xyz_m)+[0, 0, .011])) - else: - frames = tuple(replace(f, observations=tuple(replace(o, - corners_xy=tuple((x+20, y) for x, y in o.corners_xy)) - if o.role == bridge.relation.child_role else o for o in f.observations)) for f in bridge.frames) - bridge = replace(bridge, frames=frames) - result = solve_initialization(replace(request, pose_bridges=(bridge,))) - assert not result.resolved and result.reason.startswith("image_motion_shared_pose_source_unverified:") - - -def test_source_anchor_cannot_authorize_an_incompatible_new_arc(measured): - _, request, _ = measured - child_role = request.relations[0].child_role - frames = tuple(replace(f, observations=tuple(replace(o, - corners_xy=tuple((x+20, y) for x, y in o.corners_xy)) - if o.role == child_role else o for o in f.observations)) for f in request.image_frames) - assert check_source_pose(request.pose_bridges[0]).status == "consistent" - result = solve_initialization(replace(request, image_frames=frames)) - assert not result.resolved - assert any(h.reason in {"image_motion_reprojection_failed", "image_motion_geometry_uncertain"} - for h in result.hypotheses) - - -@pytest.fixture(scope="module") -def journal(measured): - data, request, result = measured - _, report = initialization_artifacts(request, result) - assert report["image_model_selection_policy"] == SHARED_TAG_POSE_CONDITIONED_POLICY - records = json.loads(json.dumps([*data["records"], report])) - return records, _initializations(records) - - -def test_full_pose_contract_survives_artifact_readback(journal): - records, evidence = journal - profile = load_bundled_hand_profile("o30_right_18") - validate_shared_tag_references(profile, records, evidence) - validate_source_geometry(profile, SOURCE, records) - - -def test_readback_checks_translation_even_after_model_hash_is_recomputed(journal): - from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 - records, evidence = deepcopy(journal) - payload = next(p for p in evidence.values() if p.get("shared_tag_pose_bridges")) - geometry = payload["frozen_image_model"]["geometry"][0] - geometry["mount_child_xyz_m"][0] += .001 - payload["image_model_sha256"] = image_model_sha256(payload["frozen_image_model"]) - with pytest.raises(ValueError, match="conditioned_translation_changed"): - validate_shared_tag_references(load_bundled_hand_profile("o30_right_18"), records, evidence) - - -def test_unknown_conditioning_cannot_be_read_as_legacy_unconditioned(journal): - records, _ = deepcopy(journal) - report = next(r for r in records if r.get("shared_tag_pose_bridges")) - report["image_model_selection_policy"] = "training_squared_loss_shared_pose_holm_v2" - report["shared_tag_pose_bridges"][0]["conditioning"] = "unknown" - with pytest.raises(ValueError, match="conditioning_policy_changed"): - _initializations(records) diff --git a/src/linkerhand_calibration/test/test_shared_pose_handoff.py b/src/linkerhand_calibration/test/test_shared_pose_handoff.py deleted file mode 100644 index f6a6319..0000000 --- a/src/linkerhand_calibration/test/test_shared_pose_handoff.py +++ /dev/null @@ -1,328 +0,0 @@ -"""Real MCP -> PIP evidence handoff, with immutable zeros and legacy replay.""" -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import configure_training -from linkerhand_calibration.core.domain.reference import JointZeroReference -from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import ( - check_source_pose, check_pose_bridge, SHARED_TAG_HANDOFF_POLICY, -) -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation -from linkerhand_calibration.runtime.branch_initialization import ( - BranchInitialization, solve_initialization, initialization_artifacts, -) -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import _initializations, source_frame_sha256 -from linkerhand_calibration.runtime.passed_resume import restore_parent_models -from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references -from linkerhand_calibration.runtime.shared_pose_handoff import source_window -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope='module') -def capture(): - data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_pinky_handoff.json.gz').read_bytes())) - # Replay the captured holding posture and printed sizes, not today's - # user-edited ring baseline or 16 mm production tags. Evidence is immutable. - profile = load_bundled_hand_profile('o30_right_18') - baseline = tuple(data['original_failure']['shared_tag_pose_bridges'][0] - ['source_reference']['samples'][0]['command']) - changed = {i: value for i, value in enumerate(baseline) - if value != profile.command.baseline_values[i]} - def historical_command(command): - return tuple(changed.get(i, value) if value == profile.command.baseline_values[i] else value - for i, value in enumerate(command)) - sizes = {(row['view'], role): tag['tag_size_m'] for row in data['records'] - for role, tag in row.get('tags', {}).items()} - profile = replace(profile, command=replace(profile.command, baseline_u8=tuple(map(int, baseline))), - motion=replace(profile.motion, joint_zero_references={name: replace(spec, - command=historical_command(spec.command), - approach_commands=tuple(historical_command(p) for p in spec.approach_commands)) - for name, spec in profile.motion.joint_zero_references.items()}), - vision=replace(profile.vision, views=tuple(replace(view, tags=tuple(replace(tag, - size_m=sizes.get((view.name, tag.role), tag.size_m)) for tag in view.tags)) - for view in profile.vision.views))) - profile = configure_preparation(configure_training(profile, - 'staged_2_to_3'), 'occlusion_aware_witness_v1') - owner = BranchInitialization(profile, source_urdf=SOURCE) - failure = data['original_failure'] - reference = JointZeroReference.from_record(failure['shared_tag_pose_bridges'][0]['source_reference']) - source = next(r for r in data['records'] if r['kind'] == 'motion_branch_initialization') - restore_parent_models(owner.parent_references, [source], 1) - motion = MotionCommand('zero_approach', profile.motion.joint_zero_references['pinky_pip'].command, - 200., task_key='pinky_pip_side', reference_joints=('pinky_pip',)) - owner.begin(motion, session_epoch=1, motion_version=failure['motion_version']) - for row in data['records']: - if row['kind'] == 'motion_branch_observation': - owner.observe(row) - owner.parent_references.handoff.observe(row) - request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=('pinky_pip',)), - session_epoch=1, motion_version=failure['motion_version']+1, zero_references={reference.joint:reference}) - assert not request.invalid_reason - return profile, owner, request, data, reference - - -@pytest.fixture(scope='module') -def replay(capture): - profile, owner, request, data, reference = capture - result = solve_initialization(request) - assert result.resolved, result.reason - model, report = initialization_artifacts(request, result) - return result, model, report, [*data['records'], report] - - -@pytest.mark.replay -def test_real_handoff_preserves_gates_and_formal_readback(capture, replay): - profile, _, request, _, reference = capture - result, _, report, records = replay - bridge = request.pose_bridges[0] - assert check_source_pose(replace(bridge, source_frames=())).reason.startswith( - 'shared_pose_current_image_unresolved:image_motion_frame_reprojection_or_tilt_failed') - assert check_source_pose(bridge).status == 'consistent' - assert check_pose_bridge(result.model, bridge).status == 'consistent' - assert len(bridge.source_frames) == len(bridge.frames) == 10 - assert max(f.evidence.stamp_ns for f in bridge.source_frames) < min(f.evidence.stamp_ns for f in request.image_frames) - assert report['image_model_selection_policy'] == SHARED_TAG_HANDOFF_POLICY - assert JointZeroReference.from_record(report['shared_tag_pose_bridges'][0]['source_reference']) == reference - assert result.model.maximum_reprojection_error_px == 1.5 - winner = result.hypotheses[result.selected_hypothesis] - assert winner.validation_quality[0].maximum_frame_rms_px < result.model.maximum_reprojection_error_px - validate_shared_tag_references(profile, records, _initializations(records)) - - -@pytest.mark.parametrize('fault', ['missing', 'feedback', 'command', 'camera', 'holdout', 'epoch', 'late', 'units', 'tag_id']) -def test_source_window_never_selects_around_bad_evidence(capture, fault): - profile, owner, request, _, reference = capture - bridge = request.pose_bridges[0] - rows = deepcopy(owner.parent_references.handoff.rows(1, reference, 'side')) - before = min(f.evidence.stamp_ns for f in request.image_frames) - if fault == 'missing': rows = rows[:-1] - elif fault == 'feedback': rows[-1]['feedback_vector'][14] += 10 - elif fault == 'command': rows[-1]['command_vector'][14] += 1 - elif fault == 'camera': rows[-1]['camera_matrix'][0][0] += 1 - elif fault == 'holdout': rows[-1]['cycle'] = 3 - elif fault == 'epoch': rows[-1]['session_epoch'] += 1 - elif fault == 'units': rows[-1]['command_unit'] = 'radian' - elif fault == 'tag_id': rows[-1]['tags']['pinky_pip']['tag_id'] = 99 - else: before = rows[-1]['image_stamp_ns'] - with pytest.raises(ValueError, match='shared_pose_handoff_'): - source_window(profile, rows, reference, (5, 10, 14), bridge.source_model, epoch=1, before_stamp=before) - - -@pytest.mark.parametrize('difference,accepted', [(7, True), (10, True), (11, False)]) -def test_source_window_uses_declared_repeatability_not_stability(capture, difference, accepted): - import numpy as np - from linkerhand_calibration.runtime.revisit import REVISIT_POLICY, revisit_feedback_limits - profile, owner, request, _, reference = capture - rows = deepcopy(owner.parent_references.handoff.rows(1, reference, 'side')) - baseline = np.median([s.feedback[5] for s in reference.samples]) - for row in rows: - row['feedback_vector'][5] = baseline + difference - kwargs = dict(epoch=1, before_stamp=min(f.evidence.stamp_ns for f in request.image_frames)) - def build(**policy): - return source_window(profile, rows, reference, (5, 10, 14), - request.pose_bridges[0].source_model, **kwargs, **policy) - policy = dict(verification=REVISIT_POLICY, feedback_limits=revisit_feedback_limits(profile)) - if accepted: - assert len(build(**policy)) == 10 - else: - with pytest.raises(ValueError, match='shared_pose_handoff_feedback_changed'): - build(**policy) - with pytest.raises(ValueError, match='shared_pose_handoff_feedback_changed'): - build() - rows[-1]['feedback_vector'][5] -= 3 - with pytest.raises(ValueError, match='shared_pose_handoff_feedback_changed'): - build(**policy) - - -def test_source_and_recipient_both_veto_changed_images(capture, replay): - _, _, request, _, _ = capture - bridge = request.pose_bridges[0] - def shifted(frame): - return replace(frame, observations=tuple(replace(o, - corners_xy=tuple((x+30, y) for x,y in o.corners_xy)) if o.role == 'pinky_pip' else o - for o in frame.observations)) - bad_source = replace(bridge, source_frames=tuple(map(shifted, bridge.source_frames))) - bad_target = replace(bridge, frames=tuple(map(shifted, bridge.frames))) - assert check_source_pose(bad_source).status != 'consistent' - assert check_pose_bridge(replay[0].model, bad_target).status != 'consistent' - - -def test_source_authorization_must_precede_the_entire_recipient_arc(capture): - request = capture[2] - bridge = request.pose_bridges[0] - stamp = request.image_frames[0].evidence.stamp_ns - frames = tuple(replace(f, evidence=replace(f.evidence, stamp_ns=stamp+i)) - for i, f in enumerate(bridge.source_frames)) - result = solve_initialization(replace(request, pose_bridges=(replace(bridge, source_frames=frames),))) - assert not result.resolved and result.reason.endswith('shared_pose_handoff_order_invalid') - - -@pytest.mark.parametrize('fault', ['pixels', 'remove', 'earlier_window', 'policy', 'epoch']) -def test_readback_rejects_handoff_tampering(capture, replay, fault): - profile, _, _, _, _ = capture - records = deepcopy(replay[3]) - evidence = deepcopy(_initializations(records)) - payload = next(p for p in evidence.values() if p.get('shared_tag_pose_bridges')) - bridge = payload['shared_tag_pose_bridges'][0] - stamps = bridge['source_window_hashes'] - raw = [r for r in records if r['kind'] == 'image_observation_frame'] - if fault == 'pixels': raw[-1]['tags']['pinky_pip']['corners_xy'][0][0] += 5 - elif fault == 'remove': records.remove(raw[-1]) - elif fault == 'earlier_window': - bridge['source_window_hashes'] = {str(r['image_stamp_ns']):source_frame_sha256(r) for r in raw[-11:-1]} - elif fault == 'epoch': bridge['source_window_epoch'] += 1 - else: bridge.pop('handoff_policy') - with pytest.raises(ValueError, match='shared_pose_handoff_'): - validate_shared_tag_references(profile, records, evidence) - - -def test_resume_import_retains_both_sides(capture, replay): - from linkerhand_calibration.runtime.joint_resume import _index_motion_provenance - records = list(_index_motion_provenance(replay[3]).values()) - reference = capture[4] - records.append(reference.as_record()) - assert len([r for r in records if r['kind'] == 'image_observation_frame']) == 10 - validate_shared_tag_references(capture[0], records, _initializations(records)) - - -def test_resumed_scan_and_dependency_import_share_one_exposure(capture, replay): - records = deepcopy(replay[3]) - images = [r for r in records if r['kind'] == 'image_observation_frame'] - records.extend(deepcopy(images)) - evidence = _initializations(records) - validate_shared_tag_references(capture[0], records, evidence) - records[-1]['tags']['pinky_pip']['corners_xy'][0][0] += 1 - with pytest.raises(ValueError, match='shared_pose_handoff_duplicate_image_changed'): - validate_shared_tag_references(capture[0], records, evidence) - - -def test_dip_uses_handed_off_pip_model_but_keeps_original_pose_reference(capture, replay): - profile, _, _, data, reference = capture - registry = BranchInitialization(profile, source_urdf=SOURCE).parent_references - source = next(r for r in data['records'] if r['kind'] == 'motion_branch_initialization') - restore_parent_models(registry, [source, replay[2]], 1) - task = next(t for t in profile.motion.tasks if t.key == 'pinky_dip_side') - model, report = registry.pose_source(task, 1) - assert model.image_model == replay[0].model - assert JointZeroReference.from_record(report['shared_tag_pose_bridges'][0]['source_reference']) == reference - installed = [] - from types import SimpleNamespace - registry.activate(task, 1, SimpleNamespace(activate_reference_model=installed.append)) - assert installed == [model] - altered = deepcopy(report) - altered['shared_tag_pose_bridges'][0]['source_model_sha256'] = 'f'*64 - registry.models[(1, 'side', 'pinky_pip')] = (model, altered) - with pytest.raises(ValueError, match='parent_reference_handoff_source_changed'): - registry.pose_source(task, 1) - - -@pytest.mark.parametrize('finger', ['pinky', 'ring', 'middle', 'index']) -def test_parent_model_ownership_applies_to_every_finger(capture, finger): - profile = capture[0] - registry = BranchInitialization(profile, source_urdf=SOURCE).parent_references - task = next(t for t in profile.motion.tasks if t.key == finger+'_dip_side') - original = (object(), {'image_model_sha256':'a'*64}) - latest = (object(), {'image_model_selection_policy':SHARED_TAG_HANDOFF_POLICY, - 'shared_tag_pose_bridges':[{'joint':finger+'_pip', 'source_model_sha256':'a'*64, - 'source_reference':{'joint':finger+'_mcp_pitch'}}]}) - registry.models[(1, 'side', finger+'_mcp_pitch')] = original - registry.models[(1, 'side', finger+'_pip')] = latest - assert registry.pose_source(task, 1) is latest - - -def test_recovery_preserves_raw_epoch_and_restart_drops_live_grant(capture): - from linkerhand_calibration.runtime.shared_pose_handoff import SharedPoseHandoff - profile, _, _, data, reference = capture - cache = SharedPoseHandoff(profile) - for row in data['records']: cache.observe(row) - old = cache.rows(1, reference, 'side') - assert not cache.rows(2, reference, 'side') - cache.advance_recovery_epoch(1, 2) - assert cache.rows(2, reference, 'side') == old - assert all(r['session_epoch'] == 1 for r in old) - assert not SharedPoseHandoff(profile).rows(2, reference, 'side') - - -def test_dip_parent_check_uses_formal_capture_and_roundtrip(capture, replay): - """Replay real held pixels as a simulated later DIP visit, not a real DIP run.""" - import numpy as np - from dataclasses import asdict - from types import SimpleNamespace - from scipy.spatial.transform import Rotation - from linkerhand_calibration.runtime.capture import ObservationCapture, CaptureFrame - from linkerhand_calibration.runtime.reference_lock import ReferenceLock - from linkerhand_calibration.runtime.parent_reference_evidence import validate_parent_references - from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 - from linkerhand_calibration.core.geometry.tag_pose.cad_image_model import TransferredImageMotionModel - from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry - from linkerhand_calibration.core.geometry.tag_pose.motion_image_types import ImageHingeGeometry - from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation - profile, _, request, data, reference = capture - registry = BranchInitialization(profile, source_urdf=SOURCE).parent_references - source = next(r for r in data['records'] if r['kind'] == 'motion_branch_initialization') - restore_parent_models(registry, [source, replay[2]], 1) - task = next(t for t in profile.motion.tasks if t.key == 'pinky_dip_side') - root = replay[0].model.reference_frames[0].pose - rotation = Rotation.from_quat(reference.samples[0].parent_pose_common.quaternion_xyzw) - transform = np.eye(4) - transform[:3, :3] = rotation.as_matrix() - transform[:3, 3] = np.asarray(reference.samples[0].parent_pose_common.translation_xyz_m)-rotation.apply(root.translation_xyz_m) - host = ObservationCapture(profile, reference_lock=ReferenceLock({'side':3}), - extrinsics=SimpleNamespace(transform=lambda _:transform)) - registry.activate(task, 1, host) - target = profile.motion.joint_zero_references['pinky_dip'].command - motion = MotionCommand('joint_zero', target, 200., task_key=task.key, - zero_joints=('pinky_mcp_pitch',), reference_only=True, reference_reuse=True) - now = max(replay[0].model.source_stamps_ns)+1_000_000_000 - journal, samples = list(replay[3]), [] - for i, frame in enumerate(request.pose_bridges[0].frames): - rows, _ = host.consume(CaptureFrame('side', now+i*40_000_000, - np.asarray(frame.camera_matrix), {o.role:np.asarray(o.corners_xy) for o in frame.observations}, - reference.samples[0].feedback, target), motion) - journal.extend(rows) - samples.extend(r for r in rows if r['kind'] == 'joint_zero_sample') - assert len(samples) == 10 - check = registry.confirm(task, 1, 80, reference, samples) - assert check['schema_version'] == 2 and check['pose_model_joint'] == 'pinky_pip' - assert check['source_reference'] == reference.as_record() - journal.append(check) - hinges, check_id = registry.geometry(task, 1) - # A synthetic downstream model envelope exercises only the provenance - # boundary. Its DIP geometry is not presented as measured hardware data. - parent = hinges[0].geometry - axis = Rotation.from_quat(parent.reference_quaternion_xyzw).inv().apply(parent.axis_parent_xyz) - normal = np.cross(axis, [1.,0.,0.]); normal /= np.linalg.norm(normal) - child = ImageHingeGeometry(MotionRelation('pinky_dip', 'pinky_pip', 'pinky_dip'), - tuple(axis), (0.,0.,0.,1.), tuple(.03*normal-np.asarray(parent.mount_child_xyz_m)), (.015,0.,0.)) - later = now+2_000_000_000 - transferred = TransferredImageMotionModel((child,), replay[0].model.camera_matrix, - (('pinky_pip',.0165),('pinky_dip',.0165)), (later,later+1), (later,), (later+1,), - (('pinky_pip',0),('pinky_dip',0)), - constraints=(ParallelAxisGeometry('pinky_pip','pinky_dip',1,.03),), - source_urdf_sha256='d'*64, source_hinges=hinges) - evidence = _initializations(replay[3]) - evidence['synthetic_dip'] = dict(frozen_image_model=asdict(transferred), - image_model_sha256=image_model_sha256(asdict(transferred)), view='side', tag_role='pinky_dip', - session_epoch=1, motion_version=82, task_name=task.key, - parent_reference_sha256=check_id, source_image_stamps=[later,later+1]) - validate_parent_references(profile, journal, evidence) - damaged = deepcopy(check) - damaged['pose_model_joint'] = 'pinky_mcp_pitch' - journal[-1] = damaged - evidence['synthetic_dip']['parent_reference_sha256'] = source_frame_sha256(damaged) - with pytest.raises(ValueError, match='parent_reference_handoff_source_changed'): - validate_parent_references(profile, journal, evidence) - downgraded = deepcopy(check) - downgraded['schema_version'] = 1 - downgraded.pop('pose_model_joint'); downgraded.pop('pose_model_policy') - journal[-1] = downgraded - evidence['synthetic_dip']['parent_reference_sha256'] = source_frame_sha256(downgraded) - with pytest.raises(ValueError, match='parent_reference_handoff_policy_changed'): - validate_parent_references(profile, journal, evidence) diff --git a/src/linkerhand_calibration/test/test_shared_tag_pose_bridge.py b/src/linkerhand_calibration/test/test_shared_tag_pose_bridge.py deleted file mode 100644 index 94bf5b1..0000000 --- a/src/linkerhand_calibration/test/test_shared_tag_pose_bridge.py +++ /dev/null @@ -1,296 +0,0 @@ -"""Real MCP/PIP capture: shared physical posture disambiguates the new hinge.""" -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.domain.reference import JointZeroReference -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import resolve_image_motion -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization, initialization_artifacts -from linkerhand_calibration.runtime.passed_resume import restore_parent_models -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 -from linkerhand_calibration.profiles import load_bundled_hand_profile -from o30_capture_fixture import SOURCE - - -@pytest.fixture(scope='module') -def recording(): - path = Path(__file__).parent/'fixtures/o30_shared_tag_pip.json.gz' - return json.loads(gzip.decompress(path.read_bytes())) - - -def request_from_recording(data, *, epoch=1, mutate=None): - profile = load_bundled_hand_profile('o30_right_18') - owner = BranchInitialization(profile, source_urdf=SOURCE) - rows = deepcopy(data['records']) - reference = JointZeroReference.from_record(next(r for r in rows if r['kind']=='joint_zero_reference')) - source = next(r for r in rows if r['kind']=='motion_branch_initialization') - restore_parent_models(owner.parent_references, [source], epoch) - if mutate: - rows, reference = mutate(rows, reference) - joint = data['original_failure']['zero_joints'][0] - spec = profile.motion.joint_zero_references[joint] - version = data['original_failure']['motion_version'] - motion = MotionCommand('zero_approach', spec.command, 200., task_key=spec.task_key, - reference_joints=(joint,)) - owner.begin(motion, session_epoch=epoch, motion_version=version) - for row in rows: - if row['kind']=='motion_branch_observation' and row['task_name']==spec.task_key: - owner.observe({**row, 'session_epoch': epoch}) - request = owner.request('side', replace(motion, phase='joint_zero', zero_joints=(joint,)), - session_epoch=epoch, motion_version=version+1, zero_references={reference.joint: reference}) - return owner, request - - -def test_recorded_pip_requires_shared_pose_not_lower_pixel_error(recording): - owner, request = request_from_recording(recording) - assert not request.invalid_reason - assert len(request.image_frames)==129 and len(request.pose_bridges)==1 - assert len(request.pose_bridges[0].frames)==10 - assert not {f.evidence.stamp_ns for f in request.image_frames} & { - f.evidence.stamp_ns for f in request.pose_bridges[0].frames} - failed = resolve_image_motion(request.image_frames, request.relations) - assert not failed.resolved and failed.reason=='image_motion_families_not_distinguishable' - result = solve_initialization(request) - assert result.resolved, result - winner = next(h for h in result.hypotheses if h.branches==result.model.branches) - alternative = next(h for h in result.hypotheses if h.branches!=result.model.branches) - assert winner.pose_bridge_checks[0].status=='consistent' - assert alternative.pose_bridge_checks[0].status=='consistent' - # Both original IPPE starts now converge to the same constrained physical - # family; a numerical difference in pixel score cannot select a normal. - assert abs(winner.validation_quality[0].rms_px-alternative.validation_quality[0].rms_px) < 1e-5 - assert result.model.maximum_reprojection_error_px==1.5 and result.model.reprojection_tie_px==.03 - model, report = owner.accept(request, result) - assert report['image_model_selection_policy']=='training_shared_zero_pose_holm_v4' - for role, payload in report['evidence_payloads'].items(): - assert payload['shared_tag_pose_bridges']==report['shared_tag_pose_bridges'] - assert source_frame_sha256(payload)==report['evidence_ids'][role] - - -def test_passed_resume_uses_original_source_without_old_image_replay(recording, monkeypatch): - from linkerhand_calibration.runtime import shared_tag_reference - original = shared_tag_reference.bridge_frames - monkeypatch.setattr(shared_tag_reference, 'bridge_frames', lambda rows, relation: ( - pytest.fail('old images replayed') if any(r['task_name']!='pinky_pip_side' for r in rows) - else original(rows, relation))) - _, request = request_from_recording(recording, epoch=7) - assert not request.invalid_reason - assert request.pose_bridge_records[0]['source_reference']['session_epoch']==1 - assert solve_initialization(request).resolved - - -@pytest.mark.parametrize('difference,accepted', [(7, True), (10, True), (11, False)]) -def test_shared_zero_feedback_repeatability_is_separate_from_stability(recording, difference, accepted): - from linkerhand_calibration.runtime.shared_tag_reference import build_pose_bridge - from linkerhand_calibration.runtime.revisit import REVISIT_POLICY, revisit_feedback_limits - import numpy as np - owner, request = request_from_recording(recording) - record = request.pose_bridge_records[0] - reference = JointZeroReference.from_record(record['source_reference']) - stamps = {int(s) for s in record['source_frame_hashes']} - rows = deepcopy([r for r in owner._endpoint_records['side'] if r['image_stamp_ns'] in stamps]) - baseline = np.median([s.feedback[5] for s in reference.samples]) - for row in rows: - row['feedback_vector'][5] = baseline + difference - kwargs = dict(verification=REVISIT_POLICY, feedback_limits=revisit_feedback_limits(owner.profile)) - def build(**policy): - return build_pose_bridge(owner.profile, 'pinky_pip', reference, rows, - tuple(record['held_channels']), **policy) - if accepted: - assert build(**kwargs).frames - else: - with pytest.raises(ValueError, match='shared_pose_feedback_changed'): - build(**kwargs) - # Unversioned historical evidence retains its original difference-of-four gate. - with pytest.raises(ValueError, match='shared_pose_feedback_changed'): - build() - rows[-1]['feedback_vector'][5] -= 3 - with pytest.raises(ValueError, match='shared_pose_feedback_changed'): - build(**kwargs) - with pytest.raises(ValueError, match='reference_feedback_policy_changed'): - build(verification=REVISIT_POLICY, feedback_limits={**kwargs['feedback_limits'], - 'between_visit_difference': 11.}) - - -def test_verify_resume_uses_fresh_pose_without_replacing_preserved_zero(recording): - owner, request = request_from_recording(recording) - reference = JointZeroReference.from_record(request.pose_bridge_records[0]['source_reference']) - owner.parent_references.remember_zero(reference, 1) - preserved = replace(reference, branch_references=tuple( - replace(b, motion_evidence_id='f'*64) for b in reference.branch_references)) - from linkerhand_calibration.runtime.shared_tag_reference import prepare_pose_bridges - bridges, reports = prepare_pose_bridges(owner, 'side', {reference.joint:preserved}, - owner._endpoint_records['side'], {f.evidence.stamp_ns for f in request.image_frames}, 1) - assert reports[0]['source_reference']==reference.as_record() - assert preserved.branch_references[0].motion_evidence_id=='f'*64 - assert solve_initialization(replace(request, pose_bridges=bridges)).resolved - - -@pytest.mark.parametrize('change', ['feedback', 'images', 'source_identity']) -def test_invalid_bridge_never_silently_falls_back(recording, change): - def mutate(rows, ref): - if change=='feedback': - for row in rows: - if row['kind']=='motion_branch_observation' and row['task_name']=='pinky_pip_side' and row['command_vector']: - row['feedback_vector'][14] += 12 - elif change=='images': - arc=set(recording['original_failure']['source_image_stamps']) - rows=[r for r in rows if r['kind']!='motion_branch_observation' or r['task_name']!='pinky_pip_side' or r['image_stamp_ns'] in arc] - else: - ref=replace(ref, branch_references=tuple(replace(b,motion_evidence_id='f'*64) for b in ref.branch_references)) - return rows, ref - _, request=request_from_recording(recording, mutate=mutate) - assert request.invalid_reason.startswith('shared_pose_bridge_invalid:') - assert not solve_initialization(request).resolved - - -@pytest.fixture(scope='module') -def bridged_journal(recording): - _, request = request_from_recording(recording) - result = solve_initialization(request) - _, report = initialization_artifacts(request, result) - records = json.loads(json.dumps([*recording['records'], report])) - from linkerhand_calibration.runtime.motion_provenance import _initializations - return records, _initializations(records) - - -def test_shared_pose_provenance_replays_current_and_original_zero_pixels(bridged_journal): - from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references - from linkerhand_calibration.runtime.motion_provenance import validate_source_geometry - profile = load_bundled_hand_profile('o30_right_18') - records, evidence = bridged_journal - validate_shared_tag_references(profile, records, evidence) - validate_source_geometry(profile, SOURCE, records) - - -@pytest.mark.parametrize('change', ['current_pixels', 'old_zero_pixels', 'source_reference', - 'source_model', 'overlapping_images', 'held_channels', 'target_model']) -def test_bridge_provenance_rejects_missing_or_relabelled_evidence(bridged_journal, change): - from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references - from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel - profile = load_bundled_hand_profile('o30_right_18') - records, evidence = deepcopy(bridged_journal) - payload = next(p for p in evidence.values() if p.get('shared_tag_pose_bridges')) - bridge = payload['shared_tag_pose_bridges'][0] - if change=='current_pixels': - stamp = next(iter(bridge['source_frame_hashes'])) - row = next(r for r in records if r.get('kind')=='motion_branch_observation' and str(r['image_stamp_ns'])==stamp) - row['tags']['pinky_pip']['corners_xy'][0][0] += 30 - bridge['source_frame_hashes'][stamp] = source_frame_sha256(row) - elif change=='old_zero_pixels': - records[:] = [r for r in records if r.get('kind')!='pnp_candidate_frame'] - elif change=='source_reference': - bridge['source_reference']['samples'][0]['relative_translation_xyz_m'][0] += .001 - elif change=='source_model': - bridge['source_model_sha256'] = 'f'*64 - elif change=='overlapping_images': - stamp = payload['source_image_stamps'][-1] - row = next(r for r in records if r.get('kind')=='motion_branch_observation' and r['image_stamp_ns']==stamp) - bridge['source_frame_hashes'][str(stamp)] = source_frame_sha256(row) - elif change=='held_channels': - bridge['held_channels'] = [14] - else: - payload['frozen_image_model']['geometry'][0]['pivot_parent_xyz_m'][0] += .02 - with pytest.raises(ValueError): - validate_shared_tag_references(profile, records, evidence, source_model=UrdfKinematicModel(SOURCE)) - - -def test_bridge_binding_is_part_of_initialization_hash(bridged_journal): - from linkerhand_calibration.runtime.motion_provenance import _initializations - records, _ = deepcopy(bridged_journal) - report = next(r for r in records if r.get('shared_tag_pose_bridges')) - report['shared_tag_pose_bridges'][0]['source_model_sha256'] = 'f'*64 - with pytest.raises(ValueError, match='initialization_binding_changed'): - _initializations(records) - - -def test_resume_epoch_requires_recorded_source_binding(recording): - from linkerhand_calibration.runtime.motion_provenance import _initializations - from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references - from linkerhand_calibration.runtime.passed_resume import restored_model_record - _, request = request_from_recording(recording, epoch=7) - _, report = initialization_artifacts(request, solve_initialization(request)) - records = deepcopy(recording['records']) - for row in records: - if row.get('kind')=='motion_branch_observation' and row['task_name']=='pinky_pip_side': - row['session_epoch']=7 - source=next(r for r in records if r['kind']=='motion_branch_initialization') - records.append(report) - evidence=_initializations(records) - profile=load_bundled_hand_profile('o30_right_18') - with pytest.raises(ValueError, match='source_epoch_changed'): - validate_shared_tag_references(profile, records, evidence) - records.append(restored_model_record([source], 7, - min(f.evidence.stamp_ns for f in request.image_frames)-1)) - validate_shared_tag_references(profile, records, evidence) - - -def test_inconclusive_pose_margin_does_not_force_a_candidate(recording): - import math - from scipy.spatial.transform import Rotation - from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import check_pose_bridge - from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame - from linkerhand_calibration.core.geometry.tag_pose.relative import _relative_pose - _, request = request_from_recording(recording) - model = solve_initialization(request).model - bridge = request.pose_bridges[0] - checked = select_image_motion_frame(model, bridge.frames[0]) - poses = {s.role:s.pose for s in checked.selections} - rotation, point = _relative_pose(poses[bridge.relation.parent_role], poses[bridge.relation.child_role]) - uncertain = replace(bridge, quaternion_xyzw=tuple((rotation*Rotation.from_rotvec([0,0,math.radians(3)])).as_quat()), - translation_xyz_m=tuple(point)) - result = check_pose_bridge(model, uncertain) - assert result.status=='unresolved' and result.reason=='shared_pose_tolerance_regions_overlap' - assert not solve_initialization(replace(request, pose_bridges=(uncertain,))).resolved - - -def test_bridge_cannot_use_training_images_as_independent_support(recording): - from linkerhand_calibration.core.geometry.tag_pose.pose_bridge import check_pose_bridge - _, request = request_from_recording(recording) - model = solve_initialization(request).model - bridge = replace(request.pose_bridges[0], frames=request.image_frames[-10:]) - assert check_pose_bridge(model, bridge).reason=='shared_pose_support_invalid' - - -def test_resume_preserves_bridge_and_epoch_binding_without_replaying_images(bridged_journal, monkeypatch): - from types import SimpleNamespace - from linkerhand_calibration.runtime.passed_resume import reference_import, restored_model_record - from linkerhand_calibration.runtime import shared_tag_evidence - records, _ = deepcopy(bridged_journal) - source = next(r for r in records if r.get('kind')=='motion_branch_initialization') - binding = restored_model_record([source], 1, source['source_image_stamps'][-1]+1) - records.append(binding) - reference = JointZeroReference.from_record(next(r for r in records if r.get('kind')=='joint_zero_reference')) - monkeypatch.setattr(shared_tag_evidence, 'validate_shared_tag_references', - lambda *a, **kw: pytest.fail('passed startup must not revalidate old images')) - imported = reference_import(SimpleNamespace(rows=records, references={reference.joint:reference}, units=set())) - assert binding in imported.rows - # An unfinished recipient is deliberately not imported in passed mode. - assert not any(r.get('shared_tag_pose_bridges') for r in imported.rows) - # This fixture models task ownership only, not a measured PIP zero. - recipient = replace(reference, joint='pinky_pip', task_key='pinky_pip_side') - imported = reference_import(SimpleNamespace(rows=records, - references={reference.joint:reference, recipient.joint:recipient}, units=set())) - report = next(r for r in imported.rows if r.get('shared_tag_pose_bridges')) - bridge_images = set(report['shared_tag_pose_bridges'][0]['source_frame_hashes']) - retained = {str(r['image_stamp_ns']) for r in imported.rows if r.get('kind')=='motion_branch_observation'} - assert bridge_images <= retained - - -@pytest.mark.parametrize('fault', ['limits', 'unknown_policy']) -def test_shared_feedback_contract_tampering_is_rejected(bridged_journal, fault): - from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references - records, evidence = deepcopy(bridged_journal) - payload = next(p for p in evidence.values() if p.get('shared_tag_pose_bridges')) - bridge = payload['shared_tag_pose_bridges'][0] - if fault == 'limits': - bridge['feedback_limits']['between_visit_difference'] = 11. - else: - bridge['verification'] = 'unknown' - with pytest.raises(ValueError, match='feedback_policy_changed|revisit_policy_unknown'): - validate_shared_tag_references(load_bundled_hand_profile('o30_right_18'), records, evidence) diff --git a/src/linkerhand_calibration/test/test_shared_zero_conditioning.py b/src/linkerhand_calibration/test/test_shared_zero_conditioning.py deleted file mode 100644 index 746e38e..0000000 --- a/src/linkerhand_calibration/test/test_shared_zero_conditioning.py +++ /dev/null @@ -1,169 +0,0 @@ -"""Independent real ring MCP/PIP images expose the post-fit-only bridge failure.""" - -from copy import deepcopy -from dataclasses import replace -import gzip -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.pose_bridge import ( - check_source_pose, check_pose_bridge, SHARED_ZERO_CONDITIONING, -) -from linkerhand_calibration.core.geometry.tag_pose.image_motion_projection import select_image_motion_frame -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ImageMotionParameters -from linkerhand_calibration.core.geometry.tag_pose.motion_image_diagnostics import _validate_frames -from linkerhand_calibration.core.geometry.tag_pose.shared_zero_solver import SharedZeroImageHingeBundle -from linkerhand_calibration.core.geometry.tag_pose.hinge_uncertainty import hinge_uncertainty -from linkerhand_calibration.runtime.branch_initialization import solve_initialization, initialization_artifacts -from linkerhand_calibration.runtime.shared_tag_evidence import validate_shared_tag_references -from linkerhand_calibration.runtime.motion_provenance import _initializations, validate_source_geometry -from linkerhand_calibration.profiles import load_bundled_hand_profile -from test_shared_tag_pose_bridge import request_from_recording as current_request_from_recording -from o30_capture_fixture import SOURCE - - -def request_from_recording(data): - """Exercise the preserved v3 contract explicitly after v4 becomes default.""" - owner, request = current_request_from_recording(data) - return owner, replace(request, - pose_bridges=tuple(replace(b, conditioning=SHARED_ZERO_CONDITIONING) for b in request.pose_bridges), - pose_bridge_records=tuple(dict(r, conditioning=SHARED_ZERO_CONDITIONING) for r in request.pose_bridge_records)) - - -@pytest.fixture(scope="module") -def recording(): - path = Path(__file__).parent / "fixtures/o30_shared_tag_ring_pip.json.gz" - return json.loads(gzip.decompress(path.read_bytes())) - - -@pytest.fixture(scope="module") -def solved(recording): - _, request = request_from_recording(recording) - assert not request.invalid_reason - result = solve_initialization(request) - assert result.resolved, result - return request, result - - -def test_recorded_failure_is_independent_model_tilt_not_a_changed_source_zero(recording, solved): - request, result = solved - bridge = request.pose_bridges[0] - source = check_source_pose(bridge) - assert source.status == "consistent" - assert np.rad2deg(source.maximum_rotation_rad) < .05 - assert source.maximum_translation_m < .00006 - legacy = solve_initialization(replace(request, pose_bridges=(replace(bridge, source_model=None),))) - assert not legacy.resolved and legacy.reason == "image_motion_shared_pose_unresolved" - assert 2.5 < np.rad2deg(legacy.hypotheses[0].pose_bridge_checks[0].maximum_rotation_rad) < 2.6 - checked = check_pose_bridge(result.model, bridge) - assert checked.status == "consistent" - assert np.rad2deg(checked.maximum_rotation_rad) < .2 - assert checked.maximum_translation_m < .005 - assert checked.maximum_translation_m > .001 # Translation was measured, not copied from the source. - assert len(request.image_frames) == 129 and len(bridge.frames) == 10 - assert not set(result.model.source_stamps_ns) & {f.evidence.stamp_ns for f in bridge.frames} - for hypothesis in result.hypotheses: - assert hypothesis.converged and hypothesis.pose_bridge_checks[0].status == "consistent" - assert hypothesis.validation_quality[0].rms_px < .32 - assert all(select_image_motion_frame(result.model, f).resolved for f in request.image_frames[1::2]) - assert result.model.maximum_reprojection_error_px == 1.5 - assert result.model.reprojection_tie_px == .03 - - -@pytest.mark.parametrize("change", ["zero_orientation", "fixed_tag", "child_pixels", "intrinsics"]) -def test_conditioning_requires_independently_confirmed_source_on_current_images(solved, change): - request, _ = solved - bridge = request.pose_bridges[0] - if change == "zero_orientation": - rotation = Rotation.from_quat(bridge.quaternion_xyzw)*Rotation.from_euler("x", 3, degrees=True) - bridge = replace(bridge, quaternion_xyzw=tuple(rotation.as_quat())) - else: - frames = [] - for frame in bridge.frames: - if change == "intrinsics": - matrix = np.array(frame.camera_matrix); matrix[0, 0] += 1 - frame = replace(frame, camera_matrix=tuple(map(tuple, matrix))) - else: - role = bridge.relation.parent_role if change == "fixed_tag" else bridge.relation.child_role - frame = replace(frame, observations=tuple(replace(o, - corners_xy=tuple((x+20, y) for x, y in o.corners_xy)) if o.role == role else o - for o in frame.observations)) - frames.append(frame) - bridge = replace(bridge, frames=tuple(frames)) - result = solve_initialization(replace(request, pose_bridges=(bridge,))) - assert not result.resolved and result.reason.startswith("image_motion_shared_pose_source_unverified:") - - -def test_uncertainty_keeps_full_image_orientation_sensitivity(solved): - request, _ = solved - limits = ImageMotionParameters() - paths, roles = _validate_frames(request.image_frames, request.relations, limits) - frames = request.image_frames[::2] - poses = {role: paths[role][0][::2] for role in roles} - bundle = SharedZeroImageHingeBundle(frames, request.relations, poses, request.pose_bridges) - fit = bundle.solve(limits) - free, linearization = bundle.uncertainty_problem(fit) - assert np.allclose(bundle.residual(fit.x), free.residual(linearization.x), atol=1e-7) - assert len(linearization.x) == len(fit.x)+2 # Restore the two identifiable tilt freedoms. - conditional_jac = fit.jac.toarray() if hasattr(fit.jac, "toarray") else fit.jac - assert linearization.jac.shape[1] > conditional_jac.shape[1] - uncertainty = hinge_uncertainty(bundle, fit) - assert uncertainty == hinge_uncertainty(free, linearization) - assert 1. < np.rad2deg(uncertainty[0][0][1]) < 3. - - -@pytest.fixture(scope="module") -def journal(recording, solved): - request, result = solved - _, report = initialization_artifacts(request, result) - records = json.loads(json.dumps([*recording["records"], report])) - return records, _initializations(records) - - -def test_conditioned_geometry_and_source_binding_survive_readback(journal): - profile = load_bundled_hand_profile("o30_right_18") - records, evidence = journal - validate_shared_tag_references(profile, records, evidence) - validate_source_geometry(profile, SOURCE, records) - - -@pytest.mark.parametrize("change", ["anchor", "conditioning", "policy"]) -def test_readback_rejects_a_changed_anchor_or_conditioning_contract(journal, change): - profile = load_bundled_hand_profile("o30_right_18") - records, evidence = deepcopy(journal) - payload = next(p for p in evidence.values() if p.get("shared_tag_pose_bridges")) - if change == "anchor": - from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 - geometry = payload["frozen_image_model"]["geometry"][0] - geometry["reference_quaternion_xyzw"] = (Rotation.from_quat(geometry["reference_quaternion_xyzw"]) - * Rotation.from_euler("z", .1, degrees=True)).as_quat().tolist() - payload["image_model_sha256"] = image_model_sha256(payload["frozen_image_model"]) - elif change == "conditioning": - payload["shared_tag_pose_bridges"][0]["conditioning"] = "unsupported" - else: - next(r for r in records if r.get("shared_tag_pose_bridges"))["image_model_selection_policy"] = "training_squared_loss_shared_pose_holm_v2" - with pytest.raises(ValueError, match="conditioning_policy_changed"): - _initializations(records) - return - with pytest.raises(ValueError, match="conditioned_orientation_changed|conditioning_policy_changed"): - validate_shared_tag_references(profile, records, evidence) - - -def test_legacy_shared_pose_records_still_read_back_without_rewriting_them(): - from test_shared_tag_pose_bridge import request_from_recording - data = json.loads(gzip.decompress((Path(__file__).parent / "fixtures/o30_shared_tag_pip.json.gz").read_bytes())) - _, request = request_from_recording(data) - records = tuple({k: v for k, v in record.items() if k != "conditioning"} - for record in request.pose_bridge_records) - request = replace(request, pose_bridges=tuple(replace(b, source_model=None) for b in request.pose_bridges), - pose_bridge_records=records) - result = solve_initialization(request) - assert result.resolved - _, report = initialization_artifacts(request, result) - assert report["image_model_selection_policy"] == "training_squared_loss_shared_pose_holm_v2" - journal = [*data["records"], report] - validate_shared_tag_references(load_bundled_hand_profile("o30_right_18"), journal, _initializations(journal)) diff --git a/src/linkerhand_calibration/test/test_single_pass_capture.py b/src/linkerhand_calibration/test/test_single_pass_capture.py deleted file mode 100644 index cb6b196..0000000 --- a/src/linkerhand_calibration/test/test_single_pass_capture.py +++ /dev/null @@ -1,151 +0,0 @@ -"""A formal task completes four round trips through real image capture. - -Only the task scope and finalization output are replaced. The production -coordinator, frozen image geometry, SDK adapter and sample gates run unchanged. -Synthetic observations verify the flow; they are not a hand accuracy result. -""" - -from collections import defaultdict - -import numpy as np -import pytest - -from linkerhand_calibration.core.domain.sampling import command_nodes -from linkerhand_calibration.core.fitting.observed_motion import fit_native_observed_rotation -from linkerhand_calibration.runtime.acquisition import accepted_joint_records -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from runtime_host_fixture import coordinator_fixture, ready -from test_diagnostic_coordinator import advance, TERMINAL_PHASES - - -class CapturedFinalization: - """Keep the actual finalizer input without building or publishing files.""" - - def __init__(self): - self.started = False - self.inputs = [] - - def start(self, inputs): - assert not self.started - self.inputs.append(inputs) - self.started = True - - def poll(self, *_args): - return None - - def cancel(self): - pass - - def close(self): - pass - - -def test_formal_yaw_four_round_trips_use_one_pass_and_disjoint_sample_phases(tmp_path, monkeypatch): - original_units = CalibrationEngine.scan_units - monkeypatch.setattr(CalibrationEngine, "scan_units", lambda engine: - tuple(unit for unit in original_units(engine) if unit.task_key == "thumb_yaw_top")) - finalizer = CapturedFinalization() - from dataclasses import replace - host, clock = coordinator_fixture(tmp_path, finalization=finalizer, - profile_transform=lambda p: replace(p, acquisition=replace(p.acquisition, - command_capture_mode="interleaved", steady_extra_training_nodes={}))) - 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.diagnostic_capture is None - assert host.branch_initialization.image_geometry is True - assert host.start().success - task = next(task for task in host.profile.motion.tasks if task.key == "thumb_yaw_top") - units = host.execution.session.engine.scan_units() - expected_units = [(cycle, direction) for cycle in range(4) - for direction in ("decreasing", "increasing")] - assert [(unit.cycle, unit.direction) for unit in units] == expected_units - entries, commands = defaultdict(list), defaultdict(list) - seen_segments, order, previous_key = set(), [], None - try: - for _ in range(9000): - if finalizer.started: - break - if host.execution.session.phase in TERMINAL_PHASES: - pytest.fail(f"premature capture exit: {host.state}: {host.reason}: {host.execution.last_quality}") - before, first_command = host._motion, len(clock.positions) - advance(host, clock) - motion = host._motion or before - if motion is None or motion.phase not in {"sweep", "steady"}: - continue - key = (motion.cycle, motion.direction) - if key != previous_key: - order.append(key) - previous_key = key - commands[key].extend(value[task.command_index] for value in clock.positions[first_command:]) - if host._motion is not None and host._segment_number not in seen_segments: - seen_segments.add(host._segment_number) - entries[key].append((motion.phase, motion.target[task.command_index], motion.steady_index)) - else: - pytest.fail(f"virtual formal capture timed out: {host.state}: {host.reason}") - - assert host.execution.session.phase == Phase.FIT and finalizer.started - assert order == expected_units - assert len(host.execution.completed_units) == 8 - assert len(finalizer.inputs) == 1 - records = finalizer.inputs[0].records - samples = [row for row in records if row.get("kind") == "joint_sample"] - assert {row["joint"] for row in samples} == {"rh_thumb_cmc_yaw"} - assert {row["attempt"] for row in samples} == {1} - assert {row["sample_phase"] for row in samples} == {"sweep", "steady"} - assert {row["pnp_observation_evidence"]["child"]["pose_source"] for row in samples} == { - "image_constrained_current_image"} - sweep_ids = {row["image_stamp_ns"] for row in samples if row["sample_phase"] == "sweep"} - steady_ids = {row["image_stamp_ns"] for row in samples if row["sample_phase"] == "steady"} - assert sweep_ids and steady_ids and sweep_ids.isdisjoint(steady_ids) - assert len(sweep_ids) + len(steady_ids) == len(samples) - zero_rows = [row for row in records if row.get("kind") == "joint_zero_reference"] - assert [row["joint"] for row in zero_rows] == ["rh_thumb_cmc_yaw"] - assert {row["cycle"] for row in records if row.get("kind") == "joint_zero_sample"} == {0} - - policy = host.profile.acquisition - assert policy.minimum_valid_samples == 40 and policy.minimum_bins == 32 - assert policy.maximum_unobserved_fraction == .0625 - first_spans = {} - for unit in units: - key = (unit.cycle, unit.direction) - targets = command_nodes(host.profile, task, holdout=unit.cycle == 3) - if unit.direction == "decreasing": - targets = tuple(reversed(targets)) - expected = [("sweep", targets[0], None), ("steady", targets[0], 0)] - for index, target in enumerate(targets[1:], 1): - expected.extend((("sweep", target, None), ("steady", target, index))) - assert entries[key] == expected - values = np.asarray(commands[key]) - assert values[0] == unit.start and values[-1] == unit.end - sign = 1 if unit.direction == "increasing" else -1 - assert np.all(sign * np.diff(values) >= 0), key - quality = evaluate_capture_unit(host.profile, unit, 1, records, first_cycle_spans=first_spans) - assert quality.passed, quality.failures - for metrics in quality.metrics.values(): - assert metrics["valid_frames"] >= 40 - assert metrics["feedback_bins"] >= 32 - assert metrics["maximum_bin_gap"] <= metrics["allowed_maximum_bin_gap"] == 16 - for row in samples: - if (row["cycle"], row["direction"]) == key and row["sample_phase"] == "steady": - assert row["command_u8"] == row["steady_target"] == targets[row["steady_index"]] - - train_nodes = set(command_nodes(host.profile, task)) - validation_nodes = set(command_nodes(host.profile, task, holdout=True)) - assert train_nodes & validation_nodes == {0., 255.} - rotation_rows = accepted_joint_records(host.profile, records)["rh_thumb_cmc_yaw"] - reference = host.execution.zero_references["rh_thumb_cmc_yaw"] - curve, errors = fit_native_observed_rotation(rotation_rows, - input_domain=host.profile.curve_input_domain, input_to_joint_direction=-1, - reference_xyzw=reference.quaternion("top"), joint=reference.joint, view="top") - training = {row["sample_id"] for row in rotation_rows if row["cycle"] in {0, 1, 2}} - holdout = {row["sample_id"] for row in rotation_rows if row["cycle"] == 3} - assert set(curve.circle["training_sample_ids"]) == training - assert training.isdisjoint(holdout) and len(errors) == len(holdout) - assert all(tuple(value[i] for i in (0, 2, 3, 4, 5)) == (255.,) * 5 for value in clock.positions) - assert not host.final_json and not host.final_urdf - assert not list(tmp_path.rglob("*.urdf")) and not list(tmp_path.rglob("release_manifest.json")) - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_source_mimic_export.py b/src/linkerhand_calibration/test/test_source_mimic_export.py deleted file mode 100644 index bb35c9f..0000000 --- a/src/linkerhand_calibration/test/test_source_mimic_export.py +++ /dev/null @@ -1,200 +0,0 @@ -"""Source coupling is immutable; JSON motion and preview bounds are separate.""" - -from copy import deepcopy -from dataclasses import replace -import hashlib -import json -from pathlib import Path -import xml.etree.ElementTree as ET - -import numpy as np -import pytest - -from test_independent_urdf_export import nonlinear_release, assert_robot_state_publisher_angles -from linkerhand_calibration.core.urdf.acceptance import validate_artifact_structure -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.core.urdf.limits import MotionRangeEvidence -from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan -from linkerhand_calibration.core.urdf.source_mimic import ( - compile_source_mimic_envelopes, validate_source_mimic_elements, -) -from linkerhand_calibration.runtime.artifacts.urdf_from_json import ( - rebuild_urdf_from_json, serialize_urdf_correction, -) - - -def nominal_evidence(model, name, bounds, *, delta=0.0, safety=None): - joint = model.joints[name] - return MotionRangeEvidence(name, name, True, (joint.lower, joint.upper), bounds, bounds, bounds, - delta, 0., (0, 1, 2), ("view:100", "view:200"), ("view:10",), - "calibrated_motion_envelope", "synthetic measured support", safety, - "synthetic hard safety range" if safety else "") - - -@pytest.fixture -def source_mimic_release(nonlinear_release): - source, rights, old_plan, payload, path = nonlinear_release - model = UrdfKinematicModel(source) - bounds = (model.joints["ip"].lower, model.joints["ip"].upper) - evidence = {"ip": nominal_evidence(model, "ip", bounds)} - plan = build_standard_correction_plan(source_urdf=source, - source_sha256=old_plan.source_sha256, zero_offsets_rad={}, authorized_fields=rights, - measured_ranges_output_rad={"ip": bounds}, range_evidence=evidence, preserve_source_mimic=True) - payload = deepcopy(payload) - payload["urdf_correction"] = serialize_urdf_correction(plan) - path.write_text(json.dumps(payload)) - return source, rights, plan, payload, path - - -def rebuild(release, output): - source, rights, _, payload, path = release - path.write_text(json.dumps(payload)) - return rebuild_urdf_from_json(path, source, output, authorized_fields=rights, - expected_profile_id=payload["profile_id"]) - - -def test_near_zero_extension_preserves_source_coupling_and_measured_support(source_mimic_release, tmp_path): - source, rights, plan, payload, _ = source_mimic_release - before = source.read_bytes() - output = rebuild(source_mimic_release, tmp_path/"retained.urdf") - validate_source_mimic_elements(source, output) - model = UrdfKinematicModel(output) - assert payload["urdf_correction"]["schema_version"] == 4 - assert model.joints["ip"].mimic_multiplier == 1.86 - assert model.joints["ip"].mimic_offset == 0 - assert model.joints["ip"].lower == pytest.approx(1.86*model.joints["pitch"].lower) - assert plan.range_evidence["ip"].output_limits_rad[0] == -.0020103987491093 - assert plan.mimic_envelopes["ip"].base_limits_rad == plan.range_evidence["ip"].output_limits_rad - assert output.read_bytes() == plan.write(source, tmp_path/"direct.urdf").read_bytes() - assert source.read_bytes() == before - validate_artifact_structure(source_urdf=source, corrected_urdf=output, source_sha256=plan.source_sha256, - authorized_fields=rights, payload=payload, zero_offsets_rad={}, range_evidence=plan.range_evidence) - for command in (0, 64, 128, 255): - angles = {name: row["angle_rad"][command] for name, row in payload["joints"].items()} - assert model.resolve_angles(angles, independent_mimic_angles=True) == angles - assert model.resolve_angles({"pitch": angles["pitch"]})["ip"] == pytest.approx(1.86*angles["pitch"]) - assert payload["joints"]["ip"]["angle_rad"][-1] != pytest.approx(1.86*.6) - - -@pytest.mark.parametrize("mutation", ["multiplier", "offset", "remove", "envelope", "padding", "missing", "policy"]) -def test_json_cannot_edit_source_mimic_or_forge_envelope(source_mimic_release, tmp_path, mutation): - _, _, _, payload, _ = source_mimic_release - correction = payload["urdf_correction"] - if mutation in {"multiplier", "offset", "remove"}: - field = {"multiplier": "mimic_multiplier", "offset": "mimic_offset", "remove": "remove_mimic"}[mutation] - correction["joint_patches"]["ip"][field] = True if mutation == "remove" else "0.1" - elif mutation == "envelope": - correction["mimic_envelopes"]["ip"]["mimic_reachable_limits_rad"] = [-.5, 1.2] - elif mutation == "padding": - correction["joint_patches"]["ip"]["limit_lower"] = "-0.5" - correction["mimic_envelopes"]["ip"]["output_limits_rad"] = [-.5, 1.2] - elif mutation == "missing": - del correction["mimic_envelopes"]["ip"] - else: - correction["mimic_policy"] = "unknown" - output = tmp_path/"rejected.urdf" - with pytest.raises(ValueError): - rebuild(source_mimic_release, output) - assert not output.exists() - - -def test_union_cannot_be_presented_as_measured_support(source_mimic_release, tmp_path): - source, rights, plan, payload, _ = source_mimic_release - # Coherently changing both fields still needs trusted fit evidence at final acceptance. - correction = payload["urdf_correction"] - correction["mimic_envelopes"]["ip"]["base_limits_rad"] = [-.5, 1.2] - correction["mimic_envelopes"]["ip"]["output_limits_rad"] = [-.5, 1.2] - correction["joint_patches"]["ip"]["limit_lower"] = "-0.5" - output = rebuild(source_mimic_release, tmp_path/"candidate.urdf") - with pytest.raises(ValueError, match="differs_from_trusted_ranges"): - validate_artifact_structure(source_urdf=source, corrected_urdf=output, source_sha256=plan.source_sha256, - authorized_fields=rights, payload=payload, zero_offsets_rad={}, range_evidence=plan.range_evidence) - - -def test_hard_bounds_are_not_relaxed_to_retain_mimic(source_mimic_release): - source, rights, plan, _, _ = source_mimic_release - args = dict(source_urdf=source, source_sha256=plan.source_sha256, - zero_offsets_rad={}, authorized_fields=rights, preserve_source_mimic=True) - with pytest.raises(ValueError, match="source_mimic_exceeds_safety_range:ip"): - build_standard_correction_plan(**args) - measured = plan.range_evidence["ip"] - with pytest.raises(ValueError, match="source_mimic_exceeds_safety_range:ip"): - build_standard_correction_plan(**args, measured_ranges_output_rad={"ip": measured.output_limits_rad}, - range_evidence={"ip": replace(measured, independent_safety_limits_cad_rad=measured.output_limits_rad, - safety_evidence_source="actual hard bound")}) - - -def test_standard_ros_still_drives_original_passive_joint(source_mimic_release, tmp_path): - assert_robot_state_publisher_angles(source_mimic_release, tmp_path, use_mimic=True) - - -@pytest.mark.parametrize("multiplier,offset", [(-2., .1), (0., .2), (2., -.1)]) -def test_mimic_chains_use_reachable_parent_not_its_wider_json_envelope(tmp_path, multiplier, offset): - source = tmp_path/"chain.urdf" - source.write_text(f''' - - - - - - - - - - - -''') - model = UrdfKinematicModel(source) - ranges = {"active": (-.5, .5)} - offsets = {"active": .2, "passive": .3, "tip": -.4} - envelopes = compile_source_mimic_envelopes(model, ranges, offsets, {}) - expected = tuple(sorted((multiplier*-.5+offset, multiplier*.5+offset))) - assert envelopes["passive"].mimic_reachable_limits_rad == expected - assert envelopes["tip"].mimic_reachable_limits_rad == expected - assert envelopes["tip"].mimic_reachable_limits_rad != envelopes["passive"].output_limits_rad - plan = build_standard_correction_plan(source_urdf=source, - source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), - zero_offsets_rad=offsets, measured_ranges_output_rad=ranges, preserve_source_mimic=True, - authorized_fields={name: ("origin.rpy", "limit.lower", "limit.upper") for name in model.joints}) - output = plan.write(source, tmp_path/"chain-output.urdf") - validate_source_mimic_elements(source, output) - assert ET.parse(output).find(".//joint[@name='tip']/mimic").attrib == {"joint": "passive"} - corrected = UrdfKinematicModel(output) - for angle in np.linspace(-.5, .5, 20): - assert corrected.resolve_angles({"active": angle})["tip"] == pytest.approx(multiplier*angle+offset) - - -@pytest.mark.parametrize("relative", [ - "l6_left/linkerhand_l6v3.1_left.urdf", "l6_right/linkerhand_l6v3.1_right.urdf", - "o6_left/linkerhand_o6_left.urdf", "o6_right/linkerhand_o6_right.urdf", - "o12_left/linkerhand_o12_t3_left_urdf-0703.urdf", "o12_right/linkerhand_o12_t3_right-0703.urdf", - "g20_left/linkerhand_g20_left.urdf", "g20_right/linkerhand_g20_right.urdf", -]) -def test_each_vendor_source_preserves_its_own_coefficients_and_topology(relative, tmp_path): - source = Path(__file__).resolve().parents[1]/"urdf"/relative - model = UrdfKinematicModel(source) - before = source.read_bytes() - # Synthetic subranges avoid the known inconsistencies in full CAD ranges. - # This checks both sides' export contracts, not their hardware calibration. - ranges = {name: (j.lower*.25, j.upper*.25) for name, j in model.joints.items() - if j.kind != "fixed" and j.mimic_joint is None} - plan = build_standard_correction_plan(source_urdf=source, - source_sha256=hashlib.sha256(before).hexdigest(), zero_offsets_rad={}, - measured_ranges_output_rad=ranges, preserve_source_mimic=True, - authorized_fields={name: ("limit.lower", "limit.upper") for name in model.joints}) - output = plan.write(source, tmp_path/"output.urdf") - validate_source_mimic_elements(source, output) - assert source.read_bytes() == before - - -@pytest.mark.parametrize("relative,joint", [ - ("l6_right/linkerhand_l6v3.1_right.urdf", "rh_thumb_dip"), - ("o12_right/linkerhand_o12_t3_right-0703.urdf", "ring_pip"), - ("g20_right/linkerhand_g20_right.urdf", "index_dip"), -]) -def test_known_full_source_range_conflicts_are_explicit(relative, joint): - source = Path(__file__).resolve().parents[1]/"urdf"/relative - with pytest.raises(ValueError, match=f"source_mimic_exceeds_safety_range:{joint}:"): - build_standard_correction_plan(source_urdf=source, - source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), - zero_offsets_rad={}, authorized_fields={}, preserve_source_mimic=True) diff --git a/src/linkerhand_calibration/test/test_spatial_solve_order.py b/src/linkerhand_calibration/test/test_spatial_solve_order.py deleted file mode 100644 index 9e2146b..0000000 --- a/src/linkerhand_calibration/test/test_spatial_solve_order.py +++ /dev/null @@ -1,33 +0,0 @@ -"""Recorded serial geometry must not depend on required-zero list order.""" - -from dataclasses import replace -from pathlib import Path -import gzip -import json -import math - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.core.domain.measurement import JointCurveFit -from linkerhand_calibration.core.fitting.session import compile_spatial_profile -from linkerhand_calibration.core.fitting.spatial import JointAxisMeasurement, solve_urdf_zero_offsets - - -def test_recorded_thumb_pitch_uses_the_fitted_upstream_yaw(): - package = Path(__file__).resolve().parents[1] - with gzip.open(package / "test/fixtures/o6_spatial_125641.json.gz", "rt") as stream: - values = json.load(stream) - values["measurements"] = [JointAxisMeasurement(**row) for row in values["measurements"]] - values["curves"] = {name: JointCurveFit(**row) for name, row in values["curves"].items()} - profile = load_bundled_hand_profile("o6_right_8") - spatial = compile_spatial_profile(profile) - values.update(source_urdf=package / "urdf/o6_right/linkerhand_o6_right.urdf") - wrong = solve_urdf_zero_offsets(**values, zero_profile=replace(spatial, - direct_zero_joints=("rh_pinky_mcp_pitch", "rh_thumb_cmc_pitch", "rh_thumb_cmc_yaw"))) - assert not wrong.passed - assert max(wrong.validation_errors_rad) > math.radians(4) - corrected = solve_urdf_zero_offsets(**values, zero_profile=spatial) - assert corrected.passed, corrected.failure_reasons - assert max(corrected.validation_errors_rad) < math.radians(1) - reordered = replace(profile, zero=replace(profile.zero, - direct_zero_joints=tuple(reversed(profile.zero.direct_zero_joints)))) - assert compile_spatial_profile(reordered).direct_zero_joints == spatial.direct_zero_joints diff --git a/src/linkerhand_calibration/test/test_staged_artifacts.py b/src/linkerhand_calibration/test/test_staged_artifacts.py deleted file mode 100644 index 09b49bf..0000000 --- a/src/linkerhand_calibration/test/test_staged_artifacts.py +++ /dev/null @@ -1,105 +0,0 @@ -"""One complete formal O30 path, with shared artifacts for rejection tests.""" - -import json -import xml.etree.ElementTree as ET -from copy import deepcopy - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import evidence_digest -from linkerhand_calibration.core.artifacts.storage import atomic_write_json -from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session -from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json -from linkerhand_calibration.runtime.staged_training import resolve_staged_plan - -pytestmark = pytest.mark.integration - - -@pytest.fixture(scope='module') -def staged_artifacts(o30_capture, tmp_path_factory): - from staged_capture_fixture import staged_capture - directory = tmp_path_factory.mktemp('staged_formal') - profile, source, records, hashes, cameras, truth, frozen = staged_capture(o30_capture, directory) - session = directory/'session' - session.mkdir() - atomic_write_json(session/'frozen_training.json', frozen) - result = finalize_profile_session(profile=profile, source_urdf=source, records=records, - protected_inputs=hashes, serial_number='SYNTHETIC_FORMAL_O30', session_dir=session, - standard_loader=ET.parse, require_motion_evidence=True, camera_extrinsics_file=cameras, - frozen_training_sha256=evidence_digest(frozen), publish=False) - return profile, source, records, frozen, result, session, truth - - -def test_formal_staged_json_rebuilds_identical_urdf(staged_artifacts): - profile, source, records, frozen, (result, fit, artifact), session, truth = staged_artifacts - assert json.loads((session/'capture_validation.json').read_text())['passed'] - assert result['capture_plan']['policy'] == 'staged_2_to_3' - assert all(t['training_cycles'] == [0, 1] for t in result['capture_plan']['tasks'].values()) - assert all(fit.zero_method_by_joint[n] == 'assumed_source_cad_zero' for n in profile.zero.cad_zero_assumptions) - for name, expected in truth.items(): - assert fit.zero_offsets_rad[name] == pytest.approx(expected, abs=.004) - rebuilt = rebuild_urdf_from_json(artifact.staged_release.calibration_json, source, session/'rebuilt.urdf', - authorized_fields=profile.urdf_authorized_fields, expected_profile_id=profile.key.profile_id) - assert rebuilt.read_bytes() == artifact.path.read_bytes() - accepted = json.loads(artifact.staged_release.validation_json) - assert accepted['final_file_image_holdout_verified'] and accepted['urdf_rebuilt_from_json'] - assert not (session.parent/'latest_passed.json').exists() - - -def test_offline_decision_reproduces_original_freeze(staged_artifacts): - profile, source, records, *_ = staged_artifacts - restored = resolve_staged_plan(profile, records, source_urdf=source, verify_decisions=True, require_complete=True) - assert restored == profile - - -@pytest.mark.parametrize('fault', ['early_validation', 'post_freeze_training', 'decision_hash', 'late_reference']) -def test_rejects_invalid_phase_or_decision_without_another_fit(staged_artifacts, fault): - profile, _, records, *_ = staged_artifacts - changed = list(records) - row = next(r for r in records if r.get('kind') == 'joint_sample') - if fault == 'early_validation': - changed.insert(1, {**row, 'cycle': 3}) - elif fault == 'post_freeze_training': - changed.append(row) - elif fault == 'decision_hash': - index = next(i for i, r in enumerate(changed) if r.get('kind') == 'staged_training_decision') - changed[index] = {**changed[index], 'frozen_training_sha256': '0'*64} - else: - index = next(i for i, r in enumerate(changed) if str(r.get('verification', '')).startswith('staged_revisit')) - changed[index] = deepcopy(changed[index]) - for sample in changed[index]['current_reference']['samples']: - sample['image_stamp_ns'] += 10**16 - sample['sample_id'] = f"{sample['view']}:{sample['image_stamp_ns']}" - with pytest.raises(ValueError, match='staged_'): - resolve_staged_plan(profile, changed, require_complete=True) - - -def test_resume_keeps_complete_visits_and_preserves_frozen_prefix(staged_artifacts, tmp_path): - from linkerhand_calibration.runtime.engine import CalibrationEngine - from linkerhand_calibration.runtime.joint_resume import JointResume - from linkerhand_calibration.runtime.staged_resume import stage_visits, load_frozen_checkpoint - profile, source, records, frozen, *_ = staged_artifacts - last = next(i for i, r in enumerate(records) if r.get('kind') == 'joint_sample' and r.get('cycle') == 1 - and r.get('task_name') == profile.motion.tasks[2].key) - partial = JointResume(profile) - stage_visits(partial, CalibrationEngine(profile), records[:last+1]) - assert not any(key[0] == profile.motion.tasks[2].key and key[1] == 1 for key in partial.units) - assert {u.identity for u in CalibrationEngine(profile).scan_units() if u.cycle == 0} <= partial.units - frozen_end = next(i for i, r in enumerate(records) if r.get('kind') == 'staged_training_decision')+1 - resumed = JointResume(profile) - stage_visits(resumed, CalibrationEngine(profile), records[:frozen_end]) - assert {u.identity for u in CalibrationEngine(profile).scan_units() if u.cycle != 3} == resumed.units - atomic_write_json(tmp_path/'frozen_training.json', frozen) - assert load_frozen_checkpoint(profile, records[:frozen_end], tmp_path/'raw_samples.jsonl', source) == frozen - atomic_write_json(tmp_path/'frozen_training.json', {**frozen, 'solver_version': 'tampered'}) - with pytest.raises(ValueError, match='frozen_model_changed'): - load_frozen_checkpoint(profile, records[:frozen_end], tmp_path/'raw_samples.jsonl', source) - - -def test_cancel_before_finalization_never_writes_or_publishes(staged_artifacts, tmp_path): - profile, source, records, *_ = staged_artifacts - with pytest.raises(ValueError, match='operator_abort'): - finalize_profile_session(profile=profile, source_urdf=source, records=records, - protected_inputs={}, serial_number='SYNTHETIC_FORMAL_O30', session_dir=tmp_path, - cancelled=lambda: True) - assert not tuple(tmp_path.iterdir()) diff --git a/src/linkerhand_calibration/test/test_staged_runtime.py b/src/linkerhand_calibration/test/test_staged_runtime.py deleted file mode 100644 index b33ab31..0000000 --- a/src/linkerhand_calibration/test/test_staged_runtime.py +++ /dev/null @@ -1,99 +0,0 @@ -"""Visit scheduling, bounded recovery and the persistent training service.""" - -import time - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import STAGED, CapturePlan, configure_training, evidence_digest -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.staged_training import dependency_tasks, staged_snapshot - - -def decision(profile, action, tasks=()): - row = dict(kind='staged_training_decision', decision=action, supplement_tasks=list(tasks), - input_plan_sha256=CapturePlan.from_profile(profile).sha256, failures=['persistent'] if action == 'fail' else []) - return {**row, 'decision_sha256': evidence_digest(row)} - - -@pytest.mark.parametrize('scope,final', [('local', 'freeze'), ('global', 'freeze'), ('local', 'fail')]) -def test_one_dependency_complete_supplement_then_freeze_or_fail(o30_capture, scope, final): - profile, _, records, _ = o30_capture - profile = configure_training(profile, STAGED) - execution = SessionExecution(profile) - session = execution.session - session._unit_index = max(i for i, u in enumerate(session._units) if u.cycle == 1) - session.phase = Phase.EVALUATE - tasks = dependency_tasks(profile, 'middle_pip' if scope == 'local' else 'palm_and_static_zero') - assert execution.evaluate(records, training_decision=decision(profile, 'supplement', tasks)).passed - assert session.current_unit.cycle == 2 - assert {u.task_key for u in session._units if u.cycle == 2} == set(tasks) - if scope == 'local': - assert len(tasks) < len(profile.motion.tasks) - session._unit_index = max(i for i, u in enumerate(session._units) if u.cycle == 2) - session.phase = Phase.EVALUATE - assert execution.evaluate(records, training_decision=decision(execution.profile, final)).passed - if final == 'freeze': - assert session.current_unit.cycle == 3 and session.phase == Phase.PREPARE - else: - assert session.phase == Phase.FAILED - - -def test_incremental_snapshot_equals_raw_evidence_and_invalidates_only_changed_task(o30_capture): - from linkerhand_calibration.runtime.capture_index import CaptureRecordIndex - profile, _, records, _ = o30_capture - profile = configure_training(profile, STAGED) - index = CaptureRecordIndex(retain_complete=False, retain_training_geometry=True) - index.extend(records) - first = staged_snapshot(profile, index) - assert evidence_digest(first) == evidence_digest(staged_snapshot(profile, records)) - task = profile.motion.tasks[0] - saved = index.training_task_snapshot(task.key, (0, 1)) - index.append({'kind': 'scan_unit_complete', 'task_name': profile.motion.tasks[-1].key, 'cycle': 1, 'passed': True}) - assert index.training_task_snapshot(task.key, (0, 1)) is saved - assert all(r['cycle'] in (0, 1) for r in first) - - -@pytest.mark.integration -def test_worker_reuses_one_process_and_stops_after_close(o30_capture): - from linkerhand_calibration.runtime.training import TrainingWorker - profile, source, _, _ = o30_capture - worker = TrainingWorker(timeout_seconds=10) - request = dict(profile=configure_training(profile, STAGED), source_urdf=source, rows=(), references={}) - try: - pid = None - for _ in range(2): - deadline, result = time.monotonic()+10, None - while result is None and time.monotonic() < deadline: - result = worker.poll(**request) - time.sleep(.005) - assert result['decision']['decision'] == 'supplement' - current = worker._process.pid - assert pid is None or pid == current - pid = current - finally: - worker.close() - with pytest.raises(RuntimeError, match='cancelled'): - worker.poll(**request) - - -def test_retry_allowance_survives_checkpoint_and_never_replaces_zero(o30_capture): - from linkerhand_calibration.runtime.motion_execution import MotionCommand - from linkerhand_calibration.runtime.zero_recovery import ZeroRecovery - from linkerhand_calibration.runtime.staged_resume import restore_recovery - profile = configure_training(o30_capture[0], STAGED) - execution, recovery = SessionExecution(profile), ZeroRecovery() - unit = execution.session._units[0] - name = profile.motion.tasks[0].joints[0] - motion = MotionCommand('joint_zero', profile.command.baseline_values, 200., - task_key=unit.task_key, zero_joints=(name,), reference_check=True) - assert recovery.take(motion, geometry_error='', reports={}, referenced_joints={name}) == 'retry_zero_images' - rows = [dict(kind='joint_zero_recovery', task_name=unit.task_key, joints=[name]), - dict(kind='scan_unit_complete', task_name=unit.task_key, cycle=unit.cycle, direction=unit.direction, - attempt=1, passed=False)] - restored = ZeroRecovery() - restore_recovery(execution.session, restored, rows) - assert restored.take(motion, geometry_error='', reports={}, referenced_joints={name}) is None - assert execution.session._retry_by_unit[unit.identity] == 1 - with pytest.raises(ValueError, match='recovery_exhausted'): - restore_recovery(execution.session, restored, [{**rows[-1], 'attempt': 2}]) diff --git a/src/linkerhand_calibration/test/test_staged_training.py b/src/linkerhand_calibration/test/test_staged_training.py deleted file mode 100644 index 4a781f9..0000000 --- a/src/linkerhand_calibration/test/test_staged_training.py +++ /dev/null @@ -1,210 +0,0 @@ -"""The staged policy shares evidence contracts with the fixed collector.""" - -import json -import gzip -from dataclasses import replace -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import ( - STAGED, TASKWISE_STAGED, CapturePlan, configure_training, select_task_training, -) -from linkerhand_calibration.core.domain.reference import read_joint_zero_references -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase -from linkerhand_calibration.runtime.staged_training import ( - assess_staged_training, staged_snapshot, read_frozen_training, dependency_tasks, -) - - -@pytest.fixture(scope='module') -def staged_fit(o30_capture): - profile, source, records, truth = o30_capture - profile = configure_training(profile, STAGED) - references = read_joint_zero_references(profile, records) - rows = staged_snapshot(profile, records) - result = assess_staged_training(profile=profile, source_urdf=source, rows=rows, references=references) - assert result['decision']['decision'] == 'freeze', result['decision'] - return profile, source, records, references, rows, result - - -def test_all_first_visits_precede_second_and_validation_keeps_original_motion(): - fixed = configure_training(load_bundled_hand_profile('o30_right_18'), 'fixed') - profile = configure_training(fixed, STAGED) - units = CalibrationEngine(profile).scan_units() - assert CapturePlan.from_profile(profile).as_dict()['version'] == 'capture_plan_v2' - assert [u.cycle for u in units] == sorted(u.cycle for u in units) - assert units == tuple(sorted((u for u in CalibrationEngine(fixed).scan_units() if u.cycle != 2), key=lambda u: u.cycle)) - selected = select_task_training(profile, 'fingers_mcp_roll_front', (0, 1, 2)) - assert len([u for u in CalibrationEngine(selected).scan_units() if u.cycle == 2]) == 4 - assert fixed.quality.training_policy == 'fixed' - - -def test_taskwise_schedule_keeps_first_pass_and_validation_global_but_supplements_locally(): - profile = configure_training(load_bundled_hand_profile('o30_right_18'), TASKWISE_STAGED) - plan = CapturePlan.from_profile(profile).as_dict() - assert plan['version'] == 'capture_plan_v3' - assert plan['schedule'] == ( - 'first_whole_hand_pass_then_taskwise_second_pass_with_immediate_supplement_then_validation' - ) - first_task = profile.motion.tasks[0] - profile = select_task_training(profile, first_task.key, (0, 1, 2)) - units = CalibrationEngine(profile).scan_units() - visits = [] - for unit in units: - visit = unit.task_key, unit.cycle - if not visits or visits[-1] != visit: - visits.append(visit) - task_keys = [task.key for task in profile.motion.tasks] - assert visits[:len(task_keys)] == [(key, 0) for key in task_keys] - training_visits = visits[len(task_keys):-len(task_keys)] - assert training_visits[:2] == [(first_task.key, 1), (first_task.key, 2)] - assert training_visits[2:] == [(key, 1) for key in task_keys[1:]] - assert visits[-len(task_keys):] == [(key, 3) for key in task_keys] - - -def test_taskwise_resume_carries_the_decision_that_created_optional_round(): - from linkerhand_calibration.runtime.joint_resume import JointResume - from linkerhand_calibration.runtime.staged_resume import visit_rows - - profile = configure_training(load_bundled_hand_profile('o30_right_18'), TASKWISE_STAGED) - task = profile.motion.tasks[0] - profile = select_task_training(profile, task.key, (0, 1, 2)) - units = [unit for unit in CalibrationEngine(profile).scan_units() - if unit.task_key == task.key and unit.cycle == 2] - decision = {'kind': 'training_decision', 'task_name': task.key, - 'assessed_cycles': [0, 1, 2], 'decision': 'freeze'} - rows = [dict(kind='scan_unit_complete', task_name=unit.task_key, - cycle=unit.cycle, direction=unit.direction, segment_key=unit.segment_key, - passed=True) for unit in units] - resume = JointResume(profile) - resume.rows = (*rows, decision) - resume.units = {unit.identity for unit in units} - resume.verified_visits.update((joint, 2) for joint in task.joints) - used, imported = visit_rows(resume, task.key, 2) - assert used == tuple(sorted(unit.identity for unit in units)) - assert [row for row in imported if row.get('kind') == 'training_decision'] == [decision] - - -def test_revisit_reuses_model_but_checks_zero_via_existing_path(): - profile = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) - execution = SessionExecution(profile) - task = profile.motion.tasks[0] - execution.zero_references.update({name: None for name in task.joints}) - session = execution.session - session._unit_index = next(i for i, u in enumerate(session._units) if u.task_key == task.key and u.cycle == 1) - session.phase = CalibrationPhase.PREPARE - effects = execution._make_effects(profile.command.baseline_values) - checks = [e for e in effects if e.phase == 'joint_zero' and not e.reference_only] - assert checks and all(e.reference_check for e in checks) - assert all(e.reference_check for e in effects if e.phase == 'zero_approach' and not e.reference_only) - - -@pytest.fixture(scope='module') -def recorded_revisit(): - from linkerhand_calibration.core.domain.reference import JointZeroReference - data = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_revisit_feedback.json.gz').read_bytes())) - return (load_bundled_hand_profile('o30_right_18'), - JointZeroReference.from_record(data['previous']), JointZeroReference.from_record(data['current'])) - - -def test_revisit_real_feedback_repeatability_and_legacy_contract(recorded_revisit): - from linkerhand_calibration.runtime.revisit import revisit_record, verify_revisit - profile, previous, current = recorded_revisit - with pytest.raises(ValueError, match='held_feedback_changed'): - verify_revisit(profile, previous, current) # v1 retains the old limit. - row = revisit_record(profile, previous, current, 0) - assert row['feedback_limits'] == dict(unit='u8', within_hold_range=2., between_visit_difference=10.) - assert row['preserved_reference'] == previous.as_record() - verify_revisit(profile, previous, current, verification=row['verification'], feedback_limits=row['feedback_limits']) - with pytest.raises(ValueError, match='feedback_policy_changed'): - verify_revisit(profile, previous, current, verification=row['verification'], - feedback_limits={**row['feedback_limits'], 'between_visit_difference': 11.}) - with pytest.raises(ValueError, match='policy_unknown'): - verify_revisit(profile, previous, current, verification='staged_revisit_v999') - - -@pytest.mark.parametrize('fault', ['at_limit', 'over_limit', 'unstable', 'pose', 'installation']) -def test_revisit_repeatability_does_not_replace_stability_or_geometry(recorded_revisit, fault): - from linkerhand_calibration.runtime.revisit import revisit_record - profile, previous, current = recorded_revisit - samples = [] - for i, sample in enumerate(current.samples): - feedback = list(sample.feedback) - if fault in {'at_limit', 'over_limit'}: - feedback[5] = 48. - (10. if fault == 'at_limit' else 10.01) - elif fault == 'unstable': - feedback[5] = 41. + (3. if i == 0 else 0.) - sample = replace(sample, feedback=tuple(feedback)) - if fault == 'pose': - sample = replace(sample, child_pose_common=replace(sample.child_pose_common, - translation_xyz_m=(1., 1., 1.))) - samples.append(sample) - current = replace(current, samples=tuple(samples)) - if fault == 'installation': - current = replace(current, branch_references=()) - if fault == 'at_limit': - assert revisit_record(profile, previous, current, 0)['preserved_reference'] == previous.as_record() - else: - with pytest.raises(ValueError, match='revisit_reference_changed'): - revisit_record(profile, previous, current, 0) - - -def test_revisit_new_feedback_policy_is_scoped_to_o30(): - from linkerhand_calibration.runtime.revisit import revisit_feedback_limits - for name in ('o6_right_8', 'g20_right_19'): - limits = revisit_feedback_limits(load_bundled_hand_profile(name)) - assert limits['between_visit_difference'] == 2*limits['within_hold_range'] - - -def test_dependency_closure_keeps_complete_observation_tasks(): - profile = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) - tasks = dependency_tasks(profile, 'middle_pip') - assert 'middle_dip_side' in tasks - assert 'middle_pip_side' in tasks - assert set(dependency_tasks(profile, 'shared_palm_geometry')) == {t.key for t in profile.motion.tasks} - - -@pytest.mark.integration -def test_two_training_rounds_freeze_once_and_final_validation_does_not_fit(staged_fit, monkeypatch): - from linkerhand_calibration.runtime.acquisition import accepted_joint_records - from linkerhand_calibration.core.fitting.session import fit_profile_calibration - profile, source, records, references, rows, result = staged_fit - payload = json.loads(json.dumps(result['frozen_training'])) - frozen = read_frozen_training(profile, rows, references, source, payload) - def forbidden(*args, **kwargs): - raise AssertionError('frozen training must not run another optimizer') - monkeypatch.setattr('linkerhand_calibration.core.fitting.command_motion.fit_command_training', forbidden) - monkeypatch.setattr('linkerhand_calibration.core.fitting.spatial_solver.solve.solve_selected', forbidden) - kept = [r for r in records if r.get('cycle') != 2 or r.get('kind') == 'joint_zero_reference'] - fit = fit_profile_calibration(profile, source, accepted_joint_records(profile, kept), - zero_references=references, steady_records=accepted_joint_records(profile, kept, sample_phase='steady'), - frozen_training=frozen) - assert len(fit.output_mappings) == 20 - with pytest.raises(ValueError, match='frozen_hand_training_inputs_changed'): - read_frozen_training(profile, rows[:-1], references, source, payload) - - -@pytest.mark.integration -def test_holdout_is_never_a_training_input(staged_fit): - profile, source, records, references, rows, result = staged_fit - poisoned = [dict(r, relative_quaternion_xyzw=[float('nan')]*4) if r.get('cycle') == 3 else r for r in records] - assert staged_snapshot(profile, poisoned) == rows - with pytest.raises(ValueError, match='received_holdout'): - assess_staged_training(profile=profile, source_urdf=source, rows=({**rows[0], 'cycle': 3},), references=references) - - -@pytest.mark.integration -def test_mixed_rounds_count_only_complete_real_dependencies(o30_capture): - profile, source, records, _ = o30_capture - profile = configure_training(profile, STAGED) - for key in dependency_tasks(profile, 'middle_pip'): - profile = select_task_training(profile, key, (0, 1, 2)) - result = assess_staged_training(profile=profile, source_urdf=source, rows=staged_snapshot(profile, records), - references=read_joint_zero_references(profile, records)) - assert result['decision']['decision'] == 'freeze', result['decision'] - counts = result['decision']['metrics']['spatial']['independent_cycles_by_joint'] - assert counts['middle_pip'] == 3 and counts['pinky_pip'] == 2 diff --git a/src/linkerhand_calibration/test/test_stall_retry.py b/src/linkerhand_calibration/test/test_stall_retry.py deleted file mode 100644 index 265969c..0000000 --- a/src/linkerhand_calibration/test/test_stall_retry.py +++ /dev/null @@ -1,87 +0,0 @@ -"""Exercise actual coordinator command delivery with a stationary motor.""" -import json -from dataclasses import replace - -import pytest - -from runtime_host_fixture import coordinator_fixture, ready -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase - - -def blocked_host(tmp_path, monkeypatch): - host, clock = coordinator_fixture(tmp_path, model='o30') - ready(host, clock) - host.parameters = replace(host.parameters, reuse_camera_calibration=True) - assert host.start().success - host.execution.session.phase = Phase.SWEEP - start = host.profile.command.baseline_values - target = list(start); target[0] = 255. - task = host.profile.motion.tasks[0] - motion = MotionCommand('sweep', tuple(target), 200., task_key=task.key, - command_index=0, cycle=0, direction='increasing') - host._motion = motion - host.last_command = start - host.segment = MotionExecution(host.profile, motion, initial_command=start, - initial_feedback=start, now=clock.now, identity='test_stalled_segment') - monkeypatch.setattr(host.execution, 'motion', lambda current: motion) - return host, clock, start - - -def heartbeat(host, clock): - 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={}))) - - -def advance(host, clock, feedback, seconds=.1): - clock.now += seconds - heartbeat(host, clock) - host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) - host.tick() - - -def test_two_delayed_resends_then_third_failure_stops(tmp_path, monkeypatch): - host, clock, feedback = blocked_host(tmp_path, monkeypatch) - try: - for _ in range(500): - if host.state == 'PAUSED': - break - waiting = host.stall_retry.waiting and host.stall_retry.remaining(clock.now) > .11 - sent = len(clock.positions) - advance(host, clock, feedback) - if waiting: - assert len(clock.positions) == sent - assert host.state == 'PAUSED' - assert 'consecutive_failures=3' in host.reason - rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()] - failures = [r for r in rows if r.get('kind') == 'motion_stall_retry'] - resumes = [r for r in rows if r.get('kind') == 'motion_stall_retry_resumed'] - assert [r['failure_count'] for r in failures] == [1, 2, 3] - assert [r['attempt'] for r in resumes] == [2, 3] - assert all(3. <= r['waited_seconds'] < 3.11 for r in resumes) - assert all(a['stamp_ns'] < b['stamp_ns'] for a,b in zip(failures,resumes)) - finally: - host.close() - - -@pytest.mark.parametrize('fault', ['feedback_stale', 'duplicate_controller']) -def test_other_protection_still_stops_during_retry_delay(tmp_path, monkeypatch, fault): - host, clock, feedback = blocked_host(tmp_path, monkeypatch) - try: - for _ in range(200): - advance(host, clock, feedback) - if host.stall_retry.waiting: - break - assert host.stall_retry.waiting - if fault == 'feedback_stale': - clock.now += host.profile.acquisition.feedback_stale_seconds+.1 - else: - clock.publishers = 2 - heartbeat(host, clock) - host.tick() - assert host.state == 'PAUSED' - assert fault in host.reason - assert host.stall_retry.failures == 1 - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_standard_correction_plan.py b/src/linkerhand_calibration/test/test_standard_correction_plan.py deleted file mode 100644 index c1068fb..0000000 --- a/src/linkerhand_calibration/test/test_standard_correction_plan.py +++ /dev/null @@ -1,84 +0,0 @@ -from dataclasses import replace -import hashlib - -import numpy as np -import pytest - -from linkerhand_calibration.core.urdf import build_standard_correction_plan -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel - - -XML = ''' - - - - -''' - - -@pytest.fixture -def source(tmp_path): - path = tmp_path / "original.urdf" - path.write_text(XML) - return path - - -def plan(source, offsets=None, ranges=None, rights=None): - return build_standard_correction_plan(source_urdf=source, - source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), - zero_offsets_rad=offsets or {"drive": 0.1, "follower": -0.04}, - measured_ranges_output_rad=ranges, - authorized_fields=rights if rights is not None else { - "drive": ("origin.rpy", "limit.lower", "limit.upper"), - "follower": ("origin.rpy", "limit.lower", "limit.upper", "mimic.offset"), - }) - - -def test_zero_origin_both_limits_and_mimic_shift_together(source, tmp_path): - correction = plan(source) - output = correction.write(source, tmp_path / "corrected.urdf") - old, new = UrdfKinematicModel(source), UrdfKinematicModel(output) - assert new.joints["drive"].lower == pytest.approx(-0.1) - assert new.joints["drive"].upper == pytest.approx(0.9) - assert new.joints["follower"].mimic_offset == pytest.approx(0.14) - for cad_angle in np.linspace(0, 1, 81): - for joint in ("drive", "follower"): - before = old.link_transform(joint, zero_offsets={}, joint_angles={"drive": cad_angle}) - after = new.link_transform(joint, zero_offsets={}, joint_angles={"drive": cad_angle - 0.1}) - np.testing.assert_allclose(after, before, atol=1e-12) - - -def test_range_expansion_cannot_hide_a_bad_zero(source): - with pytest.raises(ValueError, match="mechanical range"): - plan(source, ranges={"drive": (0, 1)}) - - -def test_authorization_is_independent_of_the_result(source): - with pytest.raises(ValueError, match="authorization"): - plan(source, rights={"drive": ("origin.rpy",)}) - - -def test_changed_source_cannot_be_patched(source, tmp_path): - correction = plan(source) - source.write_text(XML.replace('effort="1"', 'effort="2"')) - with pytest.raises(ValueError, match="SHA256"): - correction.write(source, tmp_path / "corrected.urdf") - assert not (tmp_path / "corrected.urdf").exists() - - -def test_invalid_mimic_never_materializes_an_output(source, tmp_path): - correction = plan(source) - from linkerhand_calibration.core.urdf import UrdfJointPatch, UrdfPatchSet - correction = replace(correction, patches=UrdfPatchSet({"follower": UrdfJointPatch(mimic_multiplier="3")}), - authorized_fields={"follower": ("mimic.multiplier",)}) - with pytest.raises(ValueError, match="mimic reachable range"): - correction.write(source, tmp_path / "corrected.urdf") - assert not (tmp_path / "corrected.urdf").exists() - - -def test_existing_output_cannot_be_overwritten_or_deleted(source, tmp_path): - output = tmp_path / "corrected.urdf" - output.write_text("user file") - with pytest.raises(ValueError, match="overwrite"): - plan(source).write(source, output) - assert output.read_text() == "user file" diff --git a/src/linkerhand_calibration/test/test_standard_loader.py b/src/linkerhand_calibration/test/test_standard_loader.py deleted file mode 100644 index 7c7152f..0000000 --- a/src/linkerhand_calibration/test/test_standard_loader.py +++ /dev/null @@ -1,26 +0,0 @@ -"""Offline ROS parser check; no SDK or command-topic publisher is started.""" -from pathlib import Path - -import pytest - -from linkerhand_calibration.runtime.artifacts.standard_loader import validate_with_robot_state_publisher - - -def test_actual_standard_parser_loads_isolated_robot_and_rejects_broken_tree(tmp_path): - pytest.importorskip("ament_index_python") - from ament_index_python.packages import get_package_prefix, PackageNotFoundError - try: - prefix = get_package_prefix("robot_state_publisher") - except PackageNotFoundError: - pytest.skip("robot_state_publisher is not installed") - if not (Path(prefix) / "lib/robot_state_publisher/robot_state_publisher").is_file(): - pytest.skip("robot_state_publisher binary is not installed") - source = tmp_path / "virtual.urdf" - source.write_text(''' - - - ''') - validate_with_robot_state_publisher(source) - source.write_text('') - with pytest.raises(ValueError, match="standard_urdf_loader_failed"): - validate_with_robot_state_publisher(source) diff --git a/src/linkerhand_calibration/test/test_standard_urdf_acceptance.py b/src/linkerhand_calibration/test/test_standard_urdf_acceptance.py deleted file mode 100644 index 986bff9..0000000 --- a/src/linkerhand_calibration/test/test_standard_urdf_acceptance.py +++ /dev/null @@ -1,124 +0,0 @@ -from dataclasses import replace -import math - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.urdf.acceptance import ( - JointHoldout, SerializedJointMapping, validate_standard_urdf_holdout, -) -from linkerhand_calibration.core.urdf.kinematics import ( - UrdfKinematicModel, corrected_mimic_offset, corrected_origin_rpy, -) -from linkerhand_calibration.core.urdf.validate import validate_structural_urdf_diff - - -XML = ''' - - - - -''' - - -@pytest.fixture -def artifacts(tmp_path): - path = tmp_path / "hand.urdf" - path.write_text(XML) - knots = np.linspace(0, 1, 65).tolist() - payload = {"schema_version": 7, "curve_input_domain": "feedback_rad", "joints": { - name: {"motor_index": 0, "curve_input_knots_rad": knots, "angle_rad": knots} - for name in ("drive", "follower")}} - return path, payload - - -def rows(nonlinear=False): - records = [] - for i, q in enumerate(np.linspace(0, 1, 64)): - for name in ("drive", "follower"): - observed = q + (0.18 * math.sin(math.pi * q) if nonlinear and name == "follower" else 0) - # Independent planar forward-kinematics oracle, not the URDF parser. - t = np.eye(4) - a = q if name == "drive" else 2*q - t[:3, :3] = ((math.cos(a), -math.sin(a), 0), (math.sin(a), math.cos(a), 0), (0, 0, 1)) - if name == "follower": - t[:3, 3] = (0.03*math.cos(q), 0.03*math.sin(q), 0) - records.append(JointHoldout(str(i), name, (float(q),), observed, "increasing", base_from_link=tuple(map(tuple, t)))) - return records - - -def test_serialized_urdf_and_independent_geometry_pass(artifacts): - path, payload = artifacts - metrics = validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, - observations=rows(), required_joints=("drive", "follower"), required_pose_joints=("drive", "follower")) - assert all(value.maximum_deg < 1e-10 for value in metrics.values()) - - -def test_perfect_nonlinear_json_cannot_compensate_wrong_urdf(artifacts): - path, payload = artifacts - payload["joints"]["follower"]["angle_rad"] = [q + 0.18*math.sin(math.pi*q) for q in payload["joints"]["follower"]["curve_input_knots_rad"]] - with pytest.raises(ValueError, match="standard_urdf_mimic_not_accurate"): - validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, - observations=rows(nonlinear=True), required_joints=("drive", "follower")) - - -def test_wrong_origin_cannot_pass_correct_angle_curves(artifacts): - path, payload = artifacts - path.write_text(XML.replace('rpy="0 0 0"', 'rpy="0.1 0 0"', 1)) - with pytest.raises(ValueError, match="spatial_holdout_failed"): - validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, - observations=rows(), required_joints=("drive", "follower"), required_pose_joints=("drive",)) - - -@pytest.mark.parametrize("corrupt", ["training", "duplicate", "missing", "nan"]) -def test_holdout_evidence_cannot_be_invented(artifacts, corrupt): - path, payload = artifacts - observations = rows() - if corrupt == "training": - observations[0] = replace(observations[0], cycle=2) - elif corrupt == "duplicate": - observations.append(observations[0]) - elif corrupt == "missing": - observations = [row for row in observations if row.joint == "drive"] - else: - observations[0] = replace(observations[0], observed_rad=math.nan) - with pytest.raises(ValueError): - validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, - observations=observations, required_joints=("drive", "follower")) - - -def test_mimic_resolves_source_on_sibling_branch(artifacts): - path, _ = artifacts - path.write_text(XML.replace('', '')) - model = UrdfKinematicModel(path) - pose = model.link_transform("follower", zero_offsets={}, joint_angles={"drive": 0.4, "follower": 0.9}) - assert Rotation.from_matrix(pose[:3, :3]).as_rotvec()[2] == pytest.approx(0.4) - - -def test_zero_composes_about_local_axis_and_mimic_coordinates(artifacts): - path, _ = artifacts - path.write_text(XML.replace('rpy="0 0 0"', 'rpy="0.2 -0.3 0.4"', 1).replace('axis xyz="0 0 1"', 'axis xyz="1 2 3"', 1)) - joint = UrdfKinematicModel(path).joints["drive"] - output = corrected_origin_rpy(joint, 0.17) - expected = Rotation.from_euler("xyz", (0.2, -0.3, 0.4)) * Rotation.from_rotvec(np.array((1, 2, 3))/math.sqrt(14)*0.17) - assert np.allclose(Rotation.from_euler("xyz", output).as_matrix(), expected.as_matrix()) - for parent in np.linspace(-0.2, 0.8, 20): - b = corrected_mimic_offset(1.2, 0.08, 0.1, -0.03) - assert 1.2*parent + b - 0.03 == pytest.approx(1.2*(parent+0.1)+0.08) - - -def test_nested_origin_is_not_authorized_joint_origin(artifacts, tmp_path): - path, _ = artifacts - source = XML.replace('', '', 1) - path.write_text(source) - changed = tmp_path / "changed.urdf" - changed.write_text(source.replace(' 15 - assert diagnostics["reference_frame_is_tag_pose"] is False - assert image_reference_from_dict(asdict(reference.reference)) == reference.reference - - -def test_coordinate_reference_preserves_independent_world_geometry(resolved_reference): - source, model, _ = resolved_reference - clean = frames() - for observed, expected in zip(source[::5], clean[::5]): - selected = select_image_motion_frame(model, observed) - assert selected.resolved, selected - child = next(item.pose for item in selected.selections if item.role == "moving") - actual = expected.evidence.roles[1].candidates[0] - assert np.linalg.norm(np.subtract(child.translation_xyz_m, actual.translation_xyz_m)) < 1e-6 - assert (Rotation.from_quat(child.quaternion_xyzw).inv() - * Rotation.from_quat(actual.quaternion_xyzw)).magnitude() < 1e-5 - assert np.allclose(model.geometry[0].axis_parent_xyz, [0, 0, 1], atol=1e-5) - assert image_motion_model_from_dict(image_model_payload(model)) == model - - -def test_stationary_reference_does_not_resolve_ambiguous_moving_tag(): - source = frames(alternatives=True) - reference = reference_from_frame(source[0]) - result = resolve_image_motion(tuple(in_reference(f, reference) for f in source), RELATIONS) - assert not result.resolved and result.model is None - assert len(result.hypotheses) == 2 - - -@pytest.mark.parametrize("change", ["moved", "missing", "matrix", "normal", "undeclared"]) -def test_current_image_and_coordinate_binding_are_mandatory(resolved_reference, change): - source, model, _ = resolved_reference - frame = source[-1] - if change == "moved": - image = replace(frame.observations[0], corners_xy=tuple((x + 2., y) - for x, y in frame.observations[0].corners_xy)) - frame = replace(frame, observations=(image, frame.observations[1])) - elif change == "missing": - frame = replace(frame, observations=frame.observations[1:]) - elif change == "matrix": - frame = replace(frame, camera_matrix=((1599., 0., 640.), *frame.camera_matrix[1:])) - elif change == "normal": - root = frame.evidence.roles[0] - pose = replace(root.candidates[0], quaternion_xyzw=tuple(Rotation.from_euler("y", 10, degrees=True).as_quat())) - frame = replace(frame, evidence=replace(frame.evidence, roles=(replace(root, candidates=(pose,)), frame.evidence.roles[1]))) - else: - frame = replace(frame, reference_frames=()) - assert not select_image_motion_frame(model, frame).resolved - - -def observed_reference(resolved_reference): - source, model, reference = resolved_reference - bank, tracker = ImageReferenceTracker(10), SquareTagPoseTracker() - for image in reference.source_images: - bank.estimate(reference.role, image.corners_xy, size=reference.tag_size_m, - matrix=reference.camera_matrix, stamp_ns=image.stamp_ns, locking=True) - frame = replace(source[-1], evidence=replace(source[-1].evidence, - stamp_ns=model.source_stamps_ns[-1] + 100_000_000)) - for observation in frame.observations: - if observation.role == reference.role: - bank.estimate(observation.role, observation.corners_xy, size=observation.tag_size_m, - matrix=frame.camera_matrix, stamp_ns=frame.evidence.stamp_ns, locking=False) - else: - tracker.estimate(observation.role, observation.corners_xy, tag_size_m=observation.tag_size_m, - camera_matrix=frame.camera_matrix, stamp_ns=frame.evidence.stamp_ns, motion_managed=True) - snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - frame = replace(frame, evidence=replace(frame.evidence, roles=(frame.evidence.roles[0], - RoleCandidates("moving", snapshot.candidates)))) - roots = (bank.motion_snapshot("root", frame.evidence.stamp_ns),) - decision = tracker.verify_image_motion_frame((snapshot,), model, frame, {"moving": "test_evidence"}, - root_snapshots=roots) - assert decision.accepted, decision - return bank, tracker, frame, decision - - -@pytest.mark.parametrize("change", ["reset", "new_image", "forged_snapshot"]) -def test_reference_change_between_solve_and_commit_is_rejected(resolved_reference, change): - bank, tracker, frame, decision = observed_reference(resolved_reference) - if change == "reset": - bank.reset() - elif change == "new_image": - bank.estimate("root", frame.observations[0].corners_xy, size=.016, matrix=frame.camera_matrix, - stamp_ns=frame.evidence.stamp_ns+1, locking=False) - else: - snapshot = decision.root_snapshots[0] - assert not bank.snapshot_current(replace(snapshot)) - assert bank.snapshot_current(snapshot) - return - assert tracker.commit_image_motion_frame(decision, establish=True).reason == "pose_observation_superseded" - - -def replayable_reference(resolved_reference): - bank, tracker, frame, decision = observed_reference(resolved_reference) - result = tracker.commit_image_motion_frame(decision, establish=True) - assert result.accepted, result - child_snapshot = tracker.motion_snapshot("moving", frame.evidence.stamp_ns) - assert tracker.freeze_roles({"moving": child_snapshot.revision}, require_motion_evidence=True) - selected = {"root": bank.reference.pose, "moving": result.selections[0].pose} - items = {} - for role, provider in (("root", bank), ("moving", tracker)): - observation = next(o for o in frame.observations if o.role == role) - tag = SimpleNamespace(tag_id=0 if role == "root" else 1, size_m=.016) - items.update(snapshot_pose_evidence({role: tag}, {role: np.asarray(observation.corners_xy)}, - {role: selected[role]}, provider, (), stamp_ns=frame.evidence.stamp_ns)) - record = dict(kind="pnp_candidate_frame", view="front", task_name="task", cycle=0, - direction="increasing", attempt=1, image_stamp_ns=frame.evidence.stamp_ns, - input_is_rectified=True, camera_matrix_source="CameraInfo.P[:3,:3]", camera_matrix=frame.camera_matrix, - roles={role: {**item, "selected": item["selected_pose"], "locked_reference": role == "root"} - for role, item in items.items()}) - parent, child = selected["root"], selected["moving"] - rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw) - row = {k: record[k] for k in ("view", "task_name", "cycle", "direction", "attempt", "image_stamp_ns")} - row.update(pnp_observation_evidence={"parent": items["root"], "child": items["moving"]}, - parent_pose_common=asdict(parent), child_pose_common=asdict(child), - relative_quaternion_xyzw=(rp.inv()*rc).as_quat().tolist(), - relative_translation_xyz_m=rp.inv().apply(np.subtract(child.translation_xyz_m, parent.translation_xyz_m)).tolist()) - return record, row - - -def test_reference_capture_and_serialized_readback_preserve_raw_candidates(resolved_reference): - from linkerhand_calibration.core.geometry.frozen_evidence import EvidenceDecoder - frame, row = replayable_reference(resolved_reference) - parent = row["pnp_observation_evidence"]["parent"] - assert parent["pose_source"] == IMAGE_REFERENCE_SOURCE - assert parent["reprojection_valid_candidates"] - assert reference_from_evidence(parent, stamp_ns=row["image_stamp_ns"]) == resolved_reference[2] - validate_image_sample(row, frame, replay_image_models(frame)) - decoder = EvidenceDecoder() - saved_frame, saved_row = decoder.loads(json.dumps((frame, row))) - assert (saved_frame["roles"]["root"]["reference_frame"] - is saved_row["pnp_observation_evidence"]["parent"]["reference_frame"]) - with pytest.raises(TypeError): - saved_frame["roles"]["root"]["reference_frame"]["role"] = "another_tag" - validate_image_sample(saved_row, saved_frame, replay_image_models(saved_frame)) - - -@pytest.mark.parametrize("change", ["origin", "identity", "normal_claim", "pose_source", "corners", "model_marker"]) -def test_readback_rejects_changed_reference_evidence(resolved_reference, change): - frame, row = json.loads(json.dumps(replayable_reference(resolved_reference))) - item = frame["roles"]["root"] - if change == "origin": - item["reference_frame"]["origin_xyz_m"][0] += .001 - elif change == "identity": - item["reference_frame_sha256"] = "a"*64 - elif change == "normal_claim": - item["candidate_diagnostics"]["reference_frame_is_tag_pose"] = True - elif change == "pose_source": - item["pose_source"] = "verified_fixed_reference" - elif change == "corners": - row["pnp_observation_evidence"]["parent"]["corners_xy"][0][0] += .1 - else: - child = frame["roles"]["moving"] - child["frozen_image_model"].pop("reference_frames") - child["image_model_sha256"] = image_model_sha256(child["frozen_image_model"]) - with pytest.raises(ValueError): - validate_image_sample(row, frame, replay_image_models(frame)) - - -def test_legacy_model_payload_keeps_original_hash(resolved_reference): - legacy = replace(resolved_reference[1], reference_frames=()) - old = asdict(legacy) - del old["reference_frames"] - assert image_model_payload(legacy) == old - assert image_model_sha256(image_model_payload(image_motion_model_from_dict(old))) == image_model_sha256(old) - - -def test_new_coordinate_mode_cannot_resume_old_pose_based_schedule(): - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.engine import CalibrationEngine, ACQUISITION_POLICY_VERSION - from linkerhand_calibration.core.geometry.tag_pose.parameters import POSE_TRACKING_POLICY_VERSION - from linkerhand_calibration.core.urdf.partial_scope import profile_scope - profile = load_bundled_hand_profile("o30_right_18", scope="without_finger_roll") - current = CalibrationEngine(profile) - old = CalibrationEngine(replace(profile, acquisition=replace(profile.acquisition, - fixed_reference_mode="tag_pose"))) - header = dict(profile_id=profile.key.profile_id, calibration_scope=profile_scope(profile), - acquisition_policy_version=ACQUISITION_POLICY_VERSION, - pose_tracking_policy_version=POSE_TRACKING_POLICY_VERSION, - capture_schedule_version=old.capture_schedule_version) - assert old.resume_compatible(header) - assert not current.resume_compatible(header) - header["capture_schedule_version"] = current.capture_schedule_version - assert current.resume_compatible(header) - assert not old.resume_compatible(header) diff --git a/src/linkerhand_calibration/test/test_steady_feedback_window.py b/src/linkerhand_calibration/test/test_steady_feedback_window.py deleted file mode 100644 index 131182d..0000000 --- a/src/linkerhand_calibration/test/test_steady_feedback_window.py +++ /dev/null @@ -1,143 +0,0 @@ -"""Feedback stability must survive scheduler jitter without hiding motion.""" - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution - - -@pytest.fixture(params=["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"]) -def profile(request): - return load_bundled_hand_profile(request.param) - - -@pytest.fixture(params=["steady", "joint_zero"]) -def phase(request): - return request.param - - -def segment_at_baseline(profile, phase, *, target=None, started_at=0.): - task = profile.motion.tasks[0] - baseline = profile.command.baseline_values - speed = task.formal_speed_u8 if profile.command.unit == "u8" else task.formal_speed - return MotionExecution(profile, MotionCommand( - phase, baseline if target is None else target, speed, - task_key=task.key, command_index=task.command_index), - initial_command=baseline, initial_feedback=baseline, now=started_at, identity="test") - - -def observe(segment, now, *, stamp=None, feedback=None): - segment.sample(now) - segment.observe(segment.profile.command.baseline_values if feedback is None else feedback, - stamp=now if stamp is None else stamp, now=now) - return segment.steady_ready(now) - - -@pytest.mark.parametrize("started_at", [0., 10., 1_000_000.]) -def test_constant_feedback_with_90ms_timer_becomes_and_remains_stable(profile, phase, started_at): - segment = segment_at_baseline(profile, phase, started_at=started_at) - for index in range(21): - now = started_at + index * .09 - stable = observe(segment, now) - if index < 3: - assert not stable - else: - assert stable, f"Constant feedback lost stability at {now:.2f} s" - - -@pytest.mark.parametrize("started_at", [0., 10., 1_000_000.]) -def test_mixed_timer_intervals_do_not_revoke_constant_feedback_stability(profile, phase, started_at): - segment = segment_at_baseline(profile, phase, started_at=started_at) - times = [index * .02 for index in range(26)] - times += [.59, .68, .77, .79, .81, .83, .85, .87, .89, .91] - for elapsed in times: - now = started_at + elapsed - stable = observe(segment, now) - if elapsed >= .24: - assert stable, f"Scheduler jitter cleared stable feedback at {now:.2f} s" - - -@pytest.mark.parametrize("started_at", [0., 10., 1_000_000.]) -def test_100hz_timer_preserves_stability_between_60hz_feedback_samples(profile, phase, started_at): - segment = segment_at_baseline(profile, phase, started_at=started_at) - stamps = set() - for tick in range(200): - # Feedback identities advance at 60 Hz. The 100 Hz timer often sees - # the same independent observation and must retain its stable window. - stamp = tick * 60 // 100 - stamps.add(stamp) - now = started_at + tick / 100 - stable = observe(segment, now, stamp=stamp) - if tick >= 24: - assert stable, f"A tick without new feedback revoked stability at {now:.2f} s" - assert len(segment.stamped_history) == len(stamps) < 200 - - -def test_actual_feedback_change_requires_a_new_stable_window(profile, phase): - segment = segment_at_baseline(profile, phase) - for now in (0., .09, .18, .27): - stable = observe(segment, now) - assert stable - - index = segment.command.command_index - changed = list(profile.command.baseline_values) - lower, upper = (profile.command.minimum_feedback_values[index], - profile.command.maximum_feedback_values[index]) - # A real encoder change well above the existing stability threshold. - delta = .05 * (upper - lower) - changed[index] += delta if changed[index] <= (lower + upper) / 2 else -delta - assert not observe(segment, .30, feedback=changed) - for now in (.39, .48, .57): - stable = observe(segment, now, feedback=changed) - assert stable - - -def test_repeated_feedback_stamp_cannot_supply_three_independent_samples(profile, phase): - segment = segment_at_baseline(profile, phase) - for now in (0., .10, .20): - assert not observe(segment, now, stamp=1) - for now in (.21, .24, .27): - assert not observe(segment, now, stamp=2) - assert len(segment.stamped_history) == 2 - - -def test_feedback_before_trajectory_end_cannot_establish_the_hold_window(profile, phase): - task = profile.motion.tasks[0] - index = task.command_index - target = list(profile.command.baseline_values) - target[index] = (profile.command.minimum_values[index] + profile.command.maximum_values[index]) / 2 - segment = segment_at_baseline(profile, phase, target=tuple(target)) - for now in (0., .01, .02): - assert not observe(segment, now) - assert segment.finished_at is None - arrival = segment.duration + .1 - for delay in (0., .02, .04): - assert not observe(segment, arrival + delay, feedback=target) - assert segment.finished_at == arrival - for delay in (.10, .20, .30): - stable = observe(segment, arrival + delay, feedback=target) - assert stable - - -def test_stale_feedback_history_cannot_remain_stable_indefinitely(profile, phase): - segment = segment_at_baseline(profile, phase) - for now in (0., .09, .18, .27): - stable = observe(segment, now) - assert stable - stale_time = .27 + profile.acquisition.steady_window_seconds + .01 - assert not segment.steady_ready(stale_time) - - -def test_joint_zero_checks_held_channels_in_addition_to_the_current_joint(profile): - segment = segment_at_baseline(profile, "joint_zero") - for now in (0., .09, .18, .27): - stable = observe(segment, now) - assert stable - index = next(index for index in range(profile.command.command_count) - if index != segment.command.command_index and index not in profile.command.disabled_indices) - changed = list(profile.command.baseline_values) - lower, upper = (profile.command.minimum_feedback_values[index], - profile.command.maximum_feedback_values[index]) - delta = .05 * (upper - lower) - changed[index] += delta if changed[index] <= (lower + upper) / 2 else -delta - assert not observe(segment, .30, feedback=changed) diff --git a/src/linkerhand_calibration/test/test_storage.py b/src/linkerhand_calibration/test/test_storage.py deleted file mode 100644 index 03419ad..0000000 --- a/src/linkerhand_calibration/test/test_storage.py +++ /dev/null @@ -1,69 +0,0 @@ -from linkerhand_calibration.storage import ( - append_jsonl, - append_jsonl_many, - atomic_write_json, - completed_scan_keys, - load_json, - load_jsonl, - sync_jsonl, -) - - -def test_jsonl_checkpoint_and_resume_keys(tmp_path) -> None: - raw_path = tmp_path / "raw_samples.jsonl" - record = { - "kind": "sample", - "phase": "root", - "cycle": 0, - "direction": "decreasing", - "command_u8": 255, - } - append_jsonl(raw_path, record) - append_jsonl(raw_path, {"kind": "validation", "command_u8": 10}) - loaded = load_jsonl(raw_path) - assert loaded[0] == record - assert completed_scan_keys(loaded) == {("root", 0, "decreasing", 255)} - - checkpoint_path = tmp_path / "checkpoint.json" - atomic_write_json(checkpoint_path, {"state": "PAUSED", "records": 1}) - assert load_json(checkpoint_path) == {"state": "PAUSED", "records": 1} - - -def test_resume_ignores_only_a_truncated_final_jsonl_record(tmp_path) -> None: - raw_path = tmp_path / "raw_samples.jsonl" - append_jsonl(raw_path, {"kind": "sample", "phase": "root"}) - with raw_path.open("a", encoding="utf-8") as stream: - stream.write('{"kind":"sample"\n\n') - assert load_jsonl(raw_path) == [{"kind": "sample", "phase": "root"}] - - -def test_continuous_batches_flush_in_order_and_checkpoint_syncs_all_prior_rows(tmp_path, monkeypatch): - from linkerhand_calibration.core.artifacts import storage - path = tmp_path / "raw_samples.jsonl" - persisted = [] - monkeypatch.setattr(storage.os, "fsync", lambda _fd: persisted.append(load_jsonl(path))) - rows = [{"kind": "joint_sample", "image_stamp_ns": index} for index in range(3)] - append_jsonl_many(path, rows, durable=False) - assert load_jsonl(path) == rows - assert persisted == [] - checkpoint = {"kind": "scan_unit_complete", "passed": True} - append_jsonl(path, checkpoint) - assert persisted == [rows + [checkpoint]] - more = {"kind": "pnp_branch_event"} - append_jsonl_many(path, [more]) # Existing callers remain durable by default. - assert persisted[-1] == rows + [checkpoint, more] - sync_jsonl(path) - assert len(persisted) == 3 - assert persisted[-1] == load_jsonl(path) - - -def test_explicit_sync_failure_is_not_swallowed(tmp_path, monkeypatch): - import pytest - from linkerhand_calibration.core.artifacts import storage - path = tmp_path / "raw_samples.jsonl" - append_jsonl_many(path, [{"kind": "joint_sample"}], durable=False) - def failed(_fd): - raise OSError("simulated disk sync failure") - monkeypatch.setattr(storage.os, "fsync", failed) - with pytest.raises(OSError, match="disk sync failure"): - sync_jsonl(path) diff --git a/src/linkerhand_calibration/test/test_tag_border.py b/src/linkerhand_calibration/test/test_tag_border.py deleted file mode 100644 index a0999b7..0000000 --- a/src/linkerhand_calibration/test/test_tag_border.py +++ /dev/null @@ -1,110 +0,0 @@ -"""Occluded decoded quads must not become fixed-reference movement evidence.""" - -from pathlib import Path - -import cv2 -import numpy as np - -from linkerhand_calibration.core.geometry.tag_border import inspect_tag_border -from linkerhand_calibration.runtime.image_pairing import ExactImagePairing -from linkerhand_calibration.runtime.reference_lock import ReferenceLock - - -def test_recorded_front_tag_border_distinguishes_clear_and_thumb_occlusion(): - fixture = np.load(Path(__file__).parent/"fixtures/o6_front_border_121857.npz") - for image, corners, expected in zip(fixture["images"], fixture["corners"], fixture["expected"]): - assert inspect_tag_border(image, corners)["passed"] == bool(expected) - - -def test_visible_translation_still_triggers_original_five_pixel_movement_guard(): - fixture = np.load(Path(__file__).parent/"fixtures/o6_front_border_121857.npz") - image, corners = fixture["images"][0], fixture["corners"][0] - shifted = cv2.warpAffine(image, np.float32([[1, 0, 12], [0, 1, 8]]), - (image.shape[1], image.shape[0])) - moved = corners + [12, 8] - assert inspect_tag_border(shifted, moved)["passed"] - lock = ReferenceLock({"front": 0}) - lock.begin_session(); lock.start_locking() - for _ in range(10): lock.observe("front", 0, corners) - for _ in range(9): assert lock.observe("front", 0, moved) is None - assert lock.observe("front", 0, moved).drift_px > 5 - - -def test_blank_and_outside_image_have_no_border_evidence(): - corners = np.float32([[30, 30], [90, 30], [90, 90], [30, 90]]) - assert not inspect_tag_border(np.full((120, 120), 100, np.uint8), corners)["passed"] - assert not inspect_tag_border(np.full((120, 120), 100, np.uint8), corners-40)["passed"] - - -def test_pairing_requires_same_stamp_in_either_order_and_never_reuses_old_images(): - pair = ExactImagePairing(capacity=2) - assert pair.add("detections", 10, "d10") is None - assert pair.add("image", 11, "i11") is None - assert pair.add("image", 10, "i10") == ("i10", "d10") - assert pair.add("detections", 11, "d11") == ("i11", "d11") - assert pair.add("image", 10, "old") is None - assert pair.add("detections", 11, "duplicate") is None - for stamp in (12, 13, 14): pair.add("image", stamp, f"i{stamp}") - assert len(pair.images) == 2 - assert pair.add("detections", 12, "expired") is None - assert pair.add("detections", 14, "d14") == ("i14", "d14") - - -def test_coordinator_discards_occluded_quad_but_pauses_on_visible_motion(tmp_path): - from runtime_host_fixture import coordinator_fixture, ready - from linkerhand_calibration.runtime.inputs import DetectionInput, TagDetection - from linkerhand_calibration.runtime.session import CalibrationPhase as Phase - host, clock = coordinator_fixture(tmp_path) - try: - ready(host, clock) - assert host.start().success - corners = np.float32([[100, 160], [160, 160], [160, 100], [100, 100]]) - host.reference_lock.start_locking() - for view, tag_id in host.reference_lock.fixed_tag_by_view.items(): - for _ in range(10): host.reference_lock.observe(view, tag_id, corners) - for passed in (False, True): - for _ in range(10): - clock.now += .04 - detection = TagDetection(0, tuple(map(tuple, corners+[12, 8])), 0, 100., - {"passed": passed, "reason": "" if passed else "tag_border_incomplete"}) - host.receive_detections(DetectionInput("front", host.ports.clock_ns(), (detection,))) - if not passed: - assert host.execution.session.phase != Phase.PAUSED - assert host.reference_lock.latest_drift_px["front"] == 0 - assert sum(r["kind"] == "detection_quality_rejected" for r in host.raw_records) == 10 - assert not any(r["kind"] in {"joint_sample", "joint_zero_sample"} for r in host.raw_records) - assert host.execution.session.phase == Phase.PAUSED - assert host.reason.startswith("fixed_reference_moved:front:") - finally: - host.close() - - -def test_image_process_publishes_checked_pixels_with_exact_identity(monkeypatch, tmp_path): - import rclpy - from types import SimpleNamespace - from linkerhand_calibration.profiles import load_bundled_hand_profile - from linkerhand_calibration.runtime.ros.tag_border_filter import TagBorderFilter - from linkerhand_calibration.runtime.ros.io import checked_detection_input - monkeypatch.setenv("ROS_DOMAIN_ID", "221") - monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path/"logs")) - fixture = np.load(Path(__file__).parent/"fixtures/o6_front_border_121857.npz") - rclpy.init() - node = TagBorderFilter(load_bundled_hand_profile("o6_right_8"), ("front",)) - sent = [] - monkeypatch.setattr(node.publishers_by_view["front"], "publish", sent.append) - try: - for index in (0, 5): - image = node.bridge.cv2_to_imgmsg(fixture["images"][index], encoding="mono8") - image.header.stamp.sec = index+1 - detection = SimpleNamespace(header=image.header, detections=[SimpleNamespace( - id=0, hamming=0, decision_margin=100, - corners=[SimpleNamespace(x=float(x), y=float(y)) for x, y in fixture["corners"][index]])]) - node.receive("front", "detections", detection) - assert len(sent) == (0 if index == 0 else 1) - node.receive("front", "image", image) - observed = checked_detection_input("front", sent[-1], fixed_ids={0}) - assert observed.stamp_ns == (index+1)*1_000_000_000 - assert observed.detections[0].border_quality["passed"] == (index == 0) - finally: - node.destroy_node() - rclpy.shutdown() diff --git a/src/linkerhand_calibration/test/test_task_input_support.py b/src/linkerhand_calibration/test/test_task_input_support.py deleted file mode 100644 index 6da33d8..0000000 --- a/src/linkerhand_calibration/test/test_task_input_support.py +++ /dev/null @@ -1,97 +0,0 @@ -"""A variable hardware endpoint must not escape into a later whole-hand fit.""" - -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.runtime.engine import CalibrationEngine -from linkerhand_calibration.runtime.execution import SessionExecution -from linkerhand_calibration.runtime.session import CalibrationPhase as Phase -from linkerhand_calibration.runtime.steady import steady_targets -from linkerhand_calibration.runtime.task_quality import evaluate_task_input_support - - -def capture(*, held_upper=253., attempt=1): - profile = load_bundled_hand_profile("o30_right_18", scope="full") - profile = replace(profile, acquisition=replace(profile.acquisition, motion_observation="feedback"), - measurement=replace(profile.measurement, release_basis="feedback_and_command")) - task = profile.motion.tasks[0] - units = [unit for unit in CalibrationEngine(profile).scan_units() if unit.task_key == task.key] - rows = [] - for unit in units: - upper = 250. if unit.cycle < 3 else held_upper - common = dict(joint="thumb_cmc_roll", view="front", task_name=task.key, - cycle=unit.cycle, direction=unit.direction, attempt=attempt) - for command in np.linspace(unit.start, unit.end, 129): - rows.append(dict(common, image_stamp_ns=len(rows)+1, sample_phase="sweep", - command_u8=float(command), feedback_u8=3.+(upper-3.)*command/255.)) - for index, command in enumerate(steady_targets(profile, unit)): - for _ in range(profile.acquisition.steady_minimum_samples): - rows.append(dict(common, image_stamp_ns=len(rows)+1, sample_phase="steady", - command_u8=float(command), feedback_u8=3.+(upper-3.)*command/255., - steady_index=index, steady_target=command)) - return profile, task, units, rows - - -def test_completed_direction_cannot_hide_unseen_holdout_endpoint(): - profile, task, units, rows = capture() - original = deepcopy(rows) - driver = SessionExecution(profile) - driver.session.phase = Phase.EVALUATE - driver.session._unit_index = len(units)-1 - driver.completed_units.update(unit.identity for unit in units[:-1]) - quality = driver.evaluate(rows) - assert quality.passed # Direction coverage and command endpoints are complete. - assert driver.session.phase == Phase.PAUSED - assert not driver.last_task_quality.passed - assert driver.session.status().pause.code == "task_input_support_failed" - assert not driver.completed_units - assert rows == original # No clipping, relabelling or held-out training. - metrics = driver.last_task_quality.metrics["joint:thumb_cmc_roll:front"] - assert metrics["training_support"] == [3., 250.] - assert metrics["holdout_span"] == [3., 253.] - assert metrics["outside_training_support"] > 0 - - -def test_complete_supported_task_advances_without_extra_travel(): - profile, task, units, rows = capture(held_upper=250.) - driver = SessionExecution(profile) - driver.session.phase = Phase.EVALUATE - driver.session._unit_index = len(units)-1 - driver.completed_units.update(unit.identity for unit in units[:-1]) - assert driver.evaluate(rows).passed - assert driver.last_task_quality.passed - assert driver.session.phase == Phase.PREPARE - assert driver.session.current_unit.task_key == profile.motion.tasks[1].key - - -def test_old_attempt_or_another_joint_cannot_extend_training_support(): - profile, task, _, rows = capture(attempt=2) - distractors = [dict(row, attempt=1, feedback_u8=255.) for row in rows if row["cycle"] < 3] - distractors += [dict(row, joint="thumb_mcp", feedback_u8=255.) for row in rows if row["cycle"] < 3] - quality = evaluate_task_input_support(profile, task, distractors+rows) - assert not quality.passed - assert quality.metrics["joint:thumb_cmc_roll:front"]["training_support"] == [3., 250.] - - -@pytest.mark.parametrize("missing", ["training", "holdout"]) -def test_missing_native_input_evidence_is_rejected(missing): - profile, task, _, rows = capture(held_upper=250.) - rows = [row for row in rows if (row["cycle"] == 3) == (missing == "training")] - assert not evaluate_task_input_support(profile, task, rows).passed - - -def test_command_release_does_not_gate_on_feedback_support(): - profile, task, _, rows = capture() - profile = replace(profile, measurement=replace(profile.measurement, release_basis="steady_command")) - quality = evaluate_task_input_support(profile, task, rows) - assert quality.passed - metrics = quality.metrics["joint:thumb_cmc_roll:front"] - assert metrics["input_domain"] == "command_u8" - assert metrics["training_support"] == [0., 255.] - # Missing commanded endpoint coverage still blocks the same release. - reduced = [r for r in rows if r["cycle"] == 3 or r["command_u8"] < 255] - assert not evaluate_task_input_support(profile, task, reduced).passed diff --git a/src/linkerhand_calibration/test/test_taskwise_recovery.py b/src/linkerhand_calibration/test/test_taskwise_recovery.py deleted file mode 100644 index 9f956ef..0000000 --- a/src/linkerhand_calibration/test/test_taskwise_recovery.py +++ /dev/null @@ -1,192 +0,0 @@ -"""Bounded whole-task recovery preserves acquired data and release requirements.""" -from itertools import groupby -import json - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.session import CalibrationSession, CalibrationPhase as Phase -from linkerhand_calibration.runtime.task_deferral import ( - TaskDeferralQueue, TaskObservationState, recoverable_task_observation, -) - -REASON = 'image_motion_families_not_distinguishable' -TASK = 'pinky_mcp_pitch_side' - - -@pytest.fixture -def profile(): - return load_bundled_hand_profile('o30_right_18') - - -def at_task(session, key): - session._unit_index = next(i for i, u in enumerate(session.scheduled_units) if u.task_key == key) - session.phase = Phase.PREPARE - - -def test_entire_chain_moves_once_without_reordering_rounds_or_completed_prefix(profile): - session = CalibrationSession(profile) - at_task(session, TASK) - before = session.scheduled_units - prefix = before[:session._unit_index] - decision = session.defer_current_task(REASON) - assert decision.affected_tasks == (TASK, 'pinky_pip_side', 'pinky_dip_side') - assert decision.moved_units == 18 - assert session.current_unit.task_key == 'ring_mcp_pitch_side' - assert session.scheduled_units[:session._unit_index] == prefix - assert set(session.scheduled_units) == set(before) - for key, units in groupby(session.scheduled_units, lambda u: u.task_key): - assert [c for c, _ in groupby(units, lambda u: u.cycle)] == [0, 1, 3] - assert session.scheduled_units[-1].task_key == 'pinky_dip_side' - - -def test_second_failure_finishes_independent_tasks_returns_home_and_never_fits(profile): - session = CalibrationSession(profile) - at_task(session, TASK) - session.defer_current_task(REASON) - at_task(session, TASK) # All independent tasks have now been visited. - decision = session.defer_current_task(REASON) - assert decision.state == TaskObservationState.UNOBSERVABLE - assert session.phase == Phase.RETURN_BASELINE - session.return_complete() - assert session.phase == Phase.FAILED - assert session.status().pause.code == 'unobservable_tasks' - - -def test_recovery_budget_and_pending_order_survive_restart(profile): - session = CalibrationSession(profile) - at_task(session, TASK) - first = session.defer_current_task(REASON).as_record(stamp_ns=1) - restored = CalibrationSession(profile) - restored.restore_task_deferrals([first]) - assert restored.scheduled_units == session.scheduled_units - at_task(restored, TASK) - second = restored.defer_current_task(REASON).as_record(stamp_ns=2) - again = CalibrationSession(profile) - again.restore_task_deferrals([first, second]) - assert not {u.task_key for u in again.scheduled_units} & set(first['affected_tasks']) - assert again.task_deferrals.attempts[(TASK, 0)] == 2 - with pytest.raises(ValueError, match='budget_exhausted'): - again.task_deferrals.decide(TASK, 0, REASON, 0) - - -def test_resolved_parent_audit_keeps_dependency_contract(profile): - queue = TaskDeferralQueue(profile) - pending = queue.decide(TASK, 0, REASON, 18).as_record(stamp_ns=1) - resolved = queue.resolve(TASK, 0).as_record(stamp_ns=2) - recovered = TaskDeferralQueue(profile) - recovered.restore([pending, resolved]) - assert (TASK, 0) in recovered.resolved - assert not recovered.unobservable - - -def test_incomplete_recovery_cannot_publish_even_if_other_tasks_pass(profile): - from linkerhand_calibration.runtime.artifacts.capture_validation import validate_capture_units - queue = TaskDeferralQueue(profile) - rows = [queue.decide(TASK, 0, REASON, 18).as_record(stamp_ns=1)] - with pytest.raises(ValueError, match='capture_unobservable_tasks'): - validate_capture_units(profile, rows) - - -def test_started_task_cannot_be_reordered(profile): - session = CalibrationSession(profile) - at_task(session, TASK) - session._unit_index += 1 - before = session.scheduled_units - with pytest.raises(ValueError, match='requires_unstarted_task'): - session.defer_current_task(REASON) - assert session.scheduled_units == before - assert not session.task_deferrals.attempts - - -def test_live_coordinator_continues_independent_task_and_logs_budget(tmp_path): - from runtime_host_fixture import coordinator_fixture - from linkerhand_calibration.runtime.motion_execution import MotionCommand - host, clock = coordinator_fixture(tmp_path, model='o30') - try: - session = host.execution.session - at_task(session, TASK) - motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 200, - task_key=TASK, cycle=0, zero_joints=('pinky_mcp_pitch',)) - host.branch_initialization.zero_joints = motion.zero_joints - host.branch_initialization.reports['side'] = dict(status='unresolved', reason=REASON) - assert host._defer_unobservable_task(motion, host.branch_initialization.error()) - assert session.phase == Phase.PREPARE - assert session.current_unit.task_key == 'ring_mcp_pitch_side' - assert not host._pause_effects_applied - rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()] - audit = [r for r in rows if r['kind'] == 'task_observation_state'] - assert len(audit) == 1 and audit[0]['moved_units'] == 18 - assert host.execution.motion(host.profile.command.baseline_values).task_key == 'ring_mcp_pitch_side' - finally: - host.close() - - -def test_one_ambiguous_view_cannot_hide_another_changed_reference(): - reports = {'side': dict(status='unresolved', reason=REASON), - 'front': dict(status='unresolved', reason='motion_held_feedback_changed')} - assert not recoverable_task_observation('view=side:joints=pinky_dip:'+REASON, reports) - - -@pytest.mark.parametrize('reason', [ - 'image_reference_current_image_moved', 'feedback_stalled', 'sdk_disconnected', - 'over_temperature', 'joint_zero_unavailable', 'image_motion_solver_timeout', - 'feedback_stalled:image_motion_families_not_distinguishable', -]) -def test_live_reference_or_hardware_failures_cannot_be_deferred(tmp_path, reason): - from runtime_host_fixture import coordinator_fixture - from linkerhand_calibration.runtime.motion_execution import MotionCommand - host, clock = coordinator_fixture(tmp_path, model='o30') - try: - at_task(host.execution.session, TASK) - motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 200, - task_key=TASK, cycle=0, zero_joints=('pinky_mcp_pitch',)) - before = host.execution.session.scheduled_units - assert not host._defer_unobservable_task(motion, reason) - assert host.execution.session.scheduled_units == before - finally: - host.close() - - -def test_resume_persists_budget_without_any_reusable_zero(tmp_path): - from types import SimpleNamespace - from runtime_host_fixture import coordinator_fixture - from linkerhand_calibration.runtime.passed_resume import reference_import - from linkerhand_calibration.runtime.motion_execution import MotionCommand - host, clock = coordinator_fixture(tmp_path, model='o30') - try: - queue = TaskDeferralQueue(host.profile) - first = queue.decide(TASK, 0, REASON, 18).as_record(stamp_ns=1) - host.joint_resume.rows = (first,) - pending_import = reference_import(host.joint_resume) - assert any(row['kind'] == 'task_observation_state' for row in pending_import.rows) - checkpoint = SimpleNamespace(rows=(first,), passed_import=pending_import) - host._restore_taskwise_recovery(checkpoint) - assert not any(row['kind'] == 'task_observation_state' for row in pending_import.rows) - at_task(host.execution.session, TASK) - motion = MotionCommand('joint_zero', host.profile.command.baseline_values, 200, - task_key=TASK, cycle=0, zero_joints=('pinky_mcp_pitch',)) - assert host._defer_unobservable_task(motion, REASON) - rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()] - audit = [r for r in rows if r['kind'] == 'task_observation_state'] - assert [r['attempt'] for r in audit] == [1, 2] - restored = TaskDeferralQueue(host.profile) - restored.restore(audit) - assert (TASK, 0) in restored.unobservable - finally: - host.close() - - -def test_incomplete_task_restart_cannot_reset_a_previously_resolved_budget(profile): - queue = TaskDeferralQueue(profile) - first = queue.decide(TASK, 0, REASON, 18).as_record(stamp_ns=1) - resolved = queue.resolve(TASK, 0).as_record(stamp_ns=2) - # The first training visit passed, but the process stopped before a full task commit. - restored = TaskDeferralQueue(profile) - restored.restore([first, resolved]) - second = restored.decide(TASK, 0, REASON, 18).as_record(stamp_ns=3) - assert second['state'] == 'unobservable' - replay = TaskDeferralQueue(profile) - replay.restore([first, resolved, second]) - assert (TASK, 0) in replay.unobservable - assert (TASK, 0) not in replay.resolved diff --git a/src/linkerhand_calibration/test/test_taskwise_two_round.py b/src/linkerhand_calibration/test/test_taskwise_two_round.py deleted file mode 100644 index cd6de0c..0000000 --- a/src/linkerhand_calibration/test/test_taskwise_two_round.py +++ /dev/null @@ -1,131 +0,0 @@ -"""Fixed task-contiguous collection preserves the existing release boundary.""" - -from itertools import groupby -import json -import xml.etree.ElementTree as ET - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import ( - CapturePlan, TASKWISE_TWO_ROUND, configure_training, select_task_training, -) -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.training import resolve_capture_plan - - -@pytest.fixture -def profile(): - return load_bundled_hand_profile('o30_right_18') - - -def test_every_task_finishes_two_training_rounds_and_validation_before_next(profile): - assert profile.quality.training_policy == TASKWISE_TWO_ROUND - assert not profile.motion.observer_preparations - assert not profile.motion.preparation_witnesses - engine = CalibrationEngine(profile) - units = engine.scan_units() - visits = [key for key, _ in groupby(units, key=lambda u: (u.task_key, u.cycle))] - assert visits == [(t.key, cycle) for t in profile.motion.tasks for cycle in (0, 1, 3)] - assert len(units) == 108 - assert CapturePlan.from_profile(profile).as_dict()['version'] == 'capture_plan_v5' - assert all(t.training == (0, 1) and t.holdout == 3 for t in CapturePlan.from_profile(profile).tasks) - assert engine.capture_schedule_version != CalibrationEngine(configure_training(profile, 'fixed')).capture_schedule_version - - -def test_round_transition_does_not_repeat_task_exit_or_reference_preparation(profile): - execution = SessionExecution(profile) - task = profile.motion.tasks[0] - execution._entered_task = task.key - execution._entered_visit = (task.key, 0) - execution.zero_references.update({name: None for name in task.joints}) - execution.session._unit_index = next(i for i, u in enumerate(execution.session._units) - if u.task_key == task.key and u.cycle == 1) - execution.session.phase = CalibrationPhase.PREPARE - effects = execution._make_effects(profile.command.baseline_values) - assert not any(e.phase in {'task_exit', 'stage_return', 'joint_zero', 'zero_approach'} for e in effects) - assert execution.session.status().task.cycle == 2 - assert execution.session.status().task.cycle_count == 3 - - -def test_partition_replay_rejects_added_round_or_changed_header(profile): - header = dict(kind='session_start', capture_plan=CapturePlan.from_profile(profile).as_dict()) - assert resolve_capture_plan(profile, [header]) == profile - with pytest.raises(ValueError, match='cannot_add_training'): - select_task_training(profile, profile.motion.tasks[0].key, (0, 1, 2)) - with pytest.raises(ValueError, match='unexpected_cycle'): - resolve_capture_plan(profile, [header, dict(kind='joint_sample', cycle=2)]) - with pytest.raises(ValueError, match='header_changed'): - resolve_capture_plan(profile, [dict(header, capture_plan={})]) - - -def test_existing_preparation_policy_remains_available_without_stage_reordering(profile, tmp_path): - from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_image_budget - from linkerhand_calibration.profiles.observer_policy import TRAJECTORY_OBSERVER - from linkerhand_calibration.profiles.observer_preparation import configure_observer_preparations, observer_actions - from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel - from linkerhand_calibration.profiles.validator import validate_executable_profile - from o30_capture_fixture import SOURCE - prepared = configure_preparation(profile, 'occlusion_aware_trajectory_v3') - prepared = configure_observer_preparations(prepared, UrdfKinematicModel(SOURCE)) - assert prepared.measurement.measurements['pinky_dip'].observer_pose_policy == TRAJECTORY_OBSERVER - assert set(prepared.motion.observer_preparations) == { - 'thumb_ip', 'pinky_dip', 'ring_dip', 'middle_dip', 'index_dip'} - assert observer_actions(prepared, 'pinky_dip')[0].joint == 'pinky_mcp_roll' - validate_executable_profile(prepared, SOURCE) - assert preparation_image_budget(prepared)[0] == 256 - assert CalibrationEngine(prepared).scan_units() == CalibrationEngine(profile).scan_units() - from linkerhand_calibration.profiles.export import write_hand_profile - from linkerhand_calibration.profiles import load_hand_profile - path = tmp_path/'effective_profile.yaml' - write_hand_profile(prepared, path) - assert load_hand_profile(path) == prepared - - -def test_deferral_requires_current_preparation_motion(profile): - from types import SimpleNamespace - from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator - host = SimpleNamespace(profile=profile) - assert CalibrationCoordinator._defer_unobservable_task( - host, None, 'image_motion_families_not_distinguishable') is False - - -@pytest.mark.integration -def test_two_round_formal_json_rebuilds_identical_urdf(tmp_path, o30_capture): - from o30_formal_fixture import formal_capture - from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session - from linkerhand_calibration.runtime.artifacts.urdf_from_json import rebuild_urdf_from_json - from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper - - profile, source, records, hashes, cameras, truth = formal_capture( - o30_capture, tmp_path, training_policy=TASKWISE_TWO_ROUND) - result, fit, artifact = finalize_profile_session(profile=profile, source_urdf=source, - records=records, protected_inputs=hashes, serial_number='SYNTHETIC_FORMAL_O30', - session_dir=tmp_path/'session', standard_loader=ET.parse, - require_motion_evidence=True, camera_extrinsics_file=cameras) - assert result['capture_plan']['policy'] == TASKWISE_TWO_ROUND - assert all(t['training_cycles'] == [0, 1] and t['holdout_cycle'] == 3 - for t in result['capture_plan']['tasks'].values()) - assert len(fit.command_mappings) == 20 - for name, value in truth.items(): - assert fit.zero_offsets_rad[name] == pytest.approx(value, abs=.004) - rebuilt = rebuild_urdf_from_json(artifact.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() == artifact.path.read_bytes() - from linkerhand_calibration.core.urdf.validate import validate_structural_urdf_diff - validate_structural_urdf_diff(source, artifact.path, - authorized_fields=profile.urdf_authorized_fields) - original_joints = {j.get('name'): j for j in ET.parse(source).findall('joint')} - corrected_joints = {j.get('name'): j for j in ET.parse(artifact.path).findall('joint')} - for name in profile.zero.cad_zero_assumptions: - assert fit.zero_offsets_rad[name] == 0.0 - assert fit.zero_method_by_joint[name] == 'assumed_source_cad_zero' - assert corrected_joints[name].find('origin').attrib == original_joints[name].find('origin').attrib - assert set(profile.urdf_authorized_fields[name]) == {'limit.lower', 'limit.upper'} - mapper = load_unified_mapper(artifact.staged_release.calibration_json) - assert len(mapper.map_positions(profile.command.baseline_values)) == 20 - validation = json.loads(artifact.staged_release.validation_json) - assert validation['final_file_image_holdout_verified'] and validation['urdf_rebuilt_from_json'] diff --git a/src/linkerhand_calibration/test/test_three_camera_diagnostics.py b/src/linkerhand_calibration/test/test_three_camera_diagnostics.py deleted file mode 100644 index c72ef46..0000000 --- a/src/linkerhand_calibration/test/test_three_camera_diagnostics.py +++ /dev/null @@ -1,697 +0,0 @@ -import json - -import numpy as np - -from linkerhand_calibration.three_camera_diagnostics import ( - _task_text, - render_three_camera_status_text_zh, - three_camera_reason_zh, -) - - -def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None: - payload = { - "state": "PAUSED", - "reason": "sweep_missing_endpoint_bin", - "progress": 0.1429, - "completed_sweeps": 6, - "total_sweeps": 42, - "active": { - "kind": "sweep", - "view": "front", - "motor_index": 5, - "joints": ["thumb_cmc_roll"], - "cycle": 1, - "repetitions": 3, - "start_u8": 255, - "target_u8": 0, - "actual_u8": 0.4, - "motion_progress": 0.998, - "valid_frames": 239, - "sample": { - "minimum_u8": 0.4, - "maximum_u8": 248.2, - "bin_count": 180, - "minimum_bin_count": 32, - "maximum_bin_gap": 3, - "allowed_maximum_bin_gap": 16, - "missing_endpoint_u8": [255], - "endpoint_tolerance_u8": 2.0, - }, - }, - "views": { - "front": { - "ready": False, - "detection_hz": 30.04, - "valid_rate": 0.0, - "missing_tag_ids": [2], - "pnp_rejections": {}, - "group_pnp_reason": "group_pose_jump", - "pnp_invalid_seconds": 0.8, - "pnp_reset_count": 2, - }, - "side": { - "ready": True, - "detection_hz": 30.02, - "valid_rate": 1.0, - "missing_tag_ids": [], - }, - }, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "标定已暂停（PAUSED）" in text - assert "缺少电机端点255" in text - assert "实际电机范围为0.4～248.2" in text - assert "第1/3轮，255→0" in text - assert "计划扫描6/42个方向，扫描进度14.3%" in text - assert "当前缺失Tag=2" in text - assert "group_pose_jump" not in text - assert "PnP拒绝" not in text - assert "自动重置" not in text - assert "/g20_calibration/resume" in text - - -def test_endpoint_zero_lifecycle_failure_is_not_reported_as_quality() -> None: - payload = { - "state": "PAUSED", - "reason": ( - "validated_endpoint_zero_state_incomplete:" - "missing=index_pip;extra=-" - ), - "active": {}, - "progress": 1.0, - "completed_sweeps": 16, - "total_sweeps": 16, - "views": {}, - } - - text = render_three_camera_status_text_zh(payload) - - assert "程序内部状态生命周期错误" in text - assert "不要移动相机、Tag或机械手底座" in text - - -def test_artifact_failure_explains_that_collection_need_not_restart() -> None: - payload = { - "state": "PAUSED", - "reason": ( - "PUB-ARTIFACT-601:runtime curve exceeds runtime URDF limit " - "for index_mcp_pitch" - ), - "active": {}, - "progress": 1.0, - "completed_sweeps": 16, - "total_sweeps": 16, - "views": {}, - } - - text = render_three_camera_status_text_zh(payload) - - assert "正式发布前" in text - assert "原始URDF未被覆盖" in text - assert "不要重新标定相机或调整Tag" in text - - -def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None: - explanation, action = three_camera_reason_zh( - "PAUSED", "combination_pose_prediction_failed", {} - ) - - assert "多关节组合姿态" in explanation - assert "未分类原因码" not in explanation - assert "combination_validation_failure" in action - assert "不要重新采集16个单关节任务" in action - - -def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None: - explanation, action = three_camera_reason_zh( - "PAUSED", - "sweep_start_tag_timeout", - { - "group_pnp_reasons": { - "side": "group_initializing:5/8", - }, - "pnp_initialization_progress": { - "side": {"accepted": 5, "required": 8}, - }, - "pnp_rejection_counts": { - "side": { - "ring_pip:no_pose_within_reprojection_or_tilt_limit": 12, - }, - }, - }, - ) - - assert "所需Tag也可见" in explanation - assert "初始化5/8" in explanation - assert "no_pose_within_reprojection_or_tilt_limit×12" in explanation - assert "不要根据可见性重复粘贴" in action - - -def test_mid_sweep_pnp_failure_names_missing_candidate_role() -> None: - explanation, action = three_camera_reason_zh( - "PAUSED", - "synchronised_tag_state_timeout:side", - { - "valid_frames": 150, - "group_pnp_reasons": { - "side": "group_missing_pose_candidates", - }, - "group_missing_candidate_roles": { - "side": ["index_dip"], - }, - "pnp_rejection_counts": { - "side": { - "index_dip:no_pose_within_reprojection_or_tilt_limit": 90, - }, - }, - "pnp_candidate_diagnostics": { - "side": { - "index_dip": { - "solved_candidate_count": 2, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - }, - }, - }, - }, - ) - - assert "已经取得部分有效轨迹" in explanation - assert "缺候选=index_dip" in explanation - assert "solve=2,reproj=0,tilt=0" in explanation - assert "group_pnp_candidate_event" in action - - -def test_multiview_failure_names_the_camera_specific_joint() -> None: - explanation, action = three_camera_reason_zh( - "PAUSED", - "sweep_bin_gap_too_large:pinky_mcp_roll_side", - { - "sample": { - "maximum_bin_gap": 20, - "maximum_bin_gap_start_u8": 100, - "maximum_bin_gap_end_u8": 120, - "allowed_maximum_bin_gap": 16, - } - }, - ) - - assert "小指MCP侧摆（侧面校验）" in explanation - assert "100→120" in explanation - assert "resume" in action - - -def test_preflight_lists_missing_tags_in_chinese() -> None: - payload = { - "state": "PREFLIGHT", - "reason": "waiting_for_three_cameras_tags_and_sdk", - "progress": 0.0, - "completed_sweeps": 0, - "total_sweeps": 0, - "active": {}, - "views": { - "top": { - "ready": False, - "detection_hz": 30.0, - "valid_rate": 0.0, - "missing_tag_ids": [9], - } - }, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "设备和标签预检（PREFLIGHT）" in text - assert "当前缺失Tag=9" in text - assert "全部必需Tag同时有效率0.0%" in text - - -def test_joint_fit_failure_names_metric_and_selective_retry() -> None: - payload = { - "state": "PAUSED", - "reason": "joint_fit_check_failed", - "progress": 18 / 42, - "completed_sweeps": 18, - "total_sweeps": 42, - "active": { - "kind": "fit_failure", - "view": "front", - "motor_index": 6, - "joints": ["index_mcp_roll"], - "attempt": 1, - "directions_to_rescan": 6, - "failures": [ - { - "joint": "index_mcp_roll", - "metric": "arc_deg", - "actual": 3.98, - "limit": 15.0, - "comparison": "minimum", - } - ], - }, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "食指MCP侧摆的实测圆弧为3.98°" in text - assert "要求至少15.00°" in text - assert "只清除当前失败关节的数据并重扫6个方向" in text - assert "第1次尝试" in text - assert "运动采样：" not in text - - -def test_palm_orientation_failure_explains_thumb_top_coverage() -> None: - explanation, suggestion = three_camera_reason_zh( - "PAUSED", - "palm_orientation_quality_failed", - { - "failures": [ - { - "reason": ( - "palm orientation cycle 2 has 1/2 usable sources: " - "thumb_cmc_roll_top_axis:cycle2=too few samples" - ) - } - ] - }, - ) - - assert "顶部Tag 8/9" in explanation - assert "1/2 usable sources" in explanation - assert "CMC pitch和roll" in suggestion - - -def test_cross_view_side_line_rms_names_side_source() -> None: - payload = { - "state": "PAUSED", - "reason": "joint_fit_check_failed", - "progress": 0.1, - "completed_sweeps": 8, - "total_sweeps": 80, - "active": { - "kind": "fit_failure", - "view": "front", - "motor_index": 9, - "joints": ["pinky_mcp_roll", "pinky_mcp_roll_side"], - "attempt": 1, - "directions_to_rescan": 8, - "failures": [ - { - "joint": "pinky_mcp_roll_side", - "model_joint": "pinky_mcp_roll", - "quality_source_joints": ["pinky_mcp_roll_side"], - "metric": "axis_line_cycle_rms_mm", - "actual": 1.2, - "limit": 1.0, - "comparison": "maximum", - } - ], - }, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "小指MCP侧摆（侧面校验）的四轮轴线位置RMS为1.20mm" in text - assert "要求不超过1.00mm" in text - - -def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None: - payload = { - "state": "PAUSED", - "reason": "joint_fit_check_failed", - "progress": 0.15, - "completed_sweeps": 24, - "total_sweeps": 160, - "active": { - "kind": "fit_failure", - "view": "front", - "motor_index": 15, - "joints": ["thumb_ip"], - "attempt": 3, - "directions_to_rescan": 6, - "failures": [ - { - "joint": "thumb_ip", - "metric": "baseline_hysteresis_deg", - "actual": 1.34, - "limit": 0.5, - "comparison": "maximum", - "cycle_values_deg": [1.34, 0.04, 0.14], - } - ], - }, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "baseline正反程关节角差为1.34°" in text - assert "要求不超过0.50°" in text - assert "各轮=1.34°/0.04°/0.14°" in text - assert "未知原因" not in text - - -def test_thumb_yaw_repeatability_failure_names_real_source_retry() -> None: - explanation, suggestion = three_camera_reason_zh( - "PAUSED", - "joint_fit_check_failed", - { - "source_task_names": [ - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - ], - "directions_to_rescan": 16, - "failures": [ - { - "joint": "thumb_cmc_yaw", - "metric": "zero_cycle_offset_range_deg", - "actual": 0.555836, - "limit": 0.5, - "comparison": "maximum", - "cycle": 3, - "cycle_offset_deg": [-2.650924, -2.615277, -2.095088], - } - ], - }, - ) - - assert "训练轮零位极差为0.56°" in explanation - assert "各轮=-2.65°/-2.62°/-2.10°" in explanation - assert "未知原因" not in explanation - assert "CMC pitch/roll顶部轴观测" in suggestion - assert "重扫16个方向" in suggestion - assert "不会无效重扫yaw侧摆" in suggestion - - -def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None: - payload = { - "state": "PAUSED", - "reason": "sweep_bin_gap_too_large", - "progress": 0.1, - "completed_sweeps": 1, - "total_sweeps": 10, - "active": { - "kind": "sweep", - "view": "top", - "motor_index": 10, - "joints": ["thumb_cmc_yaw"], - "cycle": 1, - "repetitions": 3, - "start_u8": 255, - "target_u8": 0, - "actual_u8": 3.0, - "valid_frames": 610, - "detection_frames": 644, - "detection_valid_frames": 610, - "detection_rate": 610 / 644, - "sample": { - "minimum_u8": 3.0, - "maximum_u8": 252.0, - "maximum_bin_gap": 17, - "maximum_bin_gap_start_u8": 99, - "maximum_bin_gap_end_u8": 116, - "allowed_maximum_bin_gap": 16, - }, - }, - "views": {}, - } - - text = render_three_camera_status_text_zh(payload) - - assert "最大空缺为17，（99→116）" in text - assert "本方向Tag检出：94.7%（610/644帧）" in text - - -def test_zero_model_failure_explains_that_rescan_will_not_help() -> None: - payload = { - "state": "PAUSED", - "reason": "zero_model_validation_failed", - "progress": 0.92, - "scan_progress": 1.0, - "completed_sweeps": 42, - "total_sweeps": 42, - "active": { - "kind": "zero_model_failure", - "view": "front", - "motor_index": 15, - "joints": ["thumb_mcp", "thumb_ip"], - "directions_to_rescan": 0, - "failures": [ - { - "joint": "thumb_mcp", - "metric": "zero_guard", - "reason": "zero_offset_exceeds_configured_limit", - "actual_deg": -39.81, - "limit_deg": 20.0, - } - ], - }, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "拇指MCP：零位估计超过安全范围" in text - assert "估计-39.81°，允许±20.00°" in text - assert "不会自动重扫" in text - assert "总体进度：92.0%" in text - assert "扫描进度100.0%" in text - assert "运动采样：" not in text - - -def test_sweep_status_shows_localized_fit_retry_cycle() -> None: - active = { - "kind": "sweep", - "view": "front", - "motor_index": 15, - "joints": ["thumb_mcp", "thumb_ip"], - "cycle": 1, - "repetitions": 3, - "start_u8": 255, - "target_u8": 0, - "fit_attempt": 2, - "fit_attempt_limit": 3, - "fit_retry_cycles": [2], - } - - assert "补采异常轮2" in _task_text(active) - - -def test_motor_stall_reason_is_explained_in_chinese() -> None: - reason, action = three_camera_reason_zh( - "PAUSED", - "motor_state_stalled:sweep_motor_19:error_u8=5.000", - {}, - ) - - assert "距目标5.000个u8" in reason - assert "机械端点" in action - assert "不要反复调用resume" in action - - -def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None: - payload = { - "state": "PAUSED", - "reason": ( - "motor_state_stalled:return_baseline:motor_index=10:" - "target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:" - "timeout_seconds=2.000:" - "error_u8=5.000" - ), - "progress": 0.0, - "scan_progress": 0.0, - "completed_sweeps": 0, - "total_sweeps": 42, - "active": { - "kind": "motion_stall", - "stage": "return_baseline", - "motor_index": 10, - "target_u8": 255.0, - "actual_u8": 250.0, - "error_u8": 5.0, - "tolerance_u8": 4.0, - "timeout_seconds": 2.0, - }, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "电机10反馈连续2秒" in text - assert "目标255.0、实际250.0、误差5.000 u8" in text - assert "允许容差±4.0 u8" in text - assert "当前任务：电机10运动停滞，目标255.0、实际250.0" in text - assert "运动采样：" not in text - - -def test_return_baseline_prints_the_exact_command() -> None: - baseline = [255] * 20 - baseline[6:10] = [127, 127, 127, 127] - payload = { - "state": "RETURN_BASELINE", - "reason": "return_baseline_before_next_sweep", - "progress": 0.0, - "completed_sweeps": 0, - "total_sweeps": 42, - "baseline_command_u8": baseline, - "active": {}, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "正在恢复目标姿态（RETURN_BASELINE）" in text - assert f"正在确认基准姿态：{baseline}" in text - - -def test_return_recovery_prints_thumb_yaw_start_and_clearance_pose() -> None: - baseline = [255] * 20 - baseline[6:10] = [127, 127, 127, 127] - recovery = list(baseline) - recovery[5] = 145 - recovery[10] = 0 - payload = { - "state": "RETURN_BASELINE", - "reason": "return_baseline_before_retry_sweep", - "progress": 0.5, - "completed_sweeps": 36, - "total_sweeps": 42, - "baseline_command_u8": baseline, - "return_command_u8": recovery, - "active": {}, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert f"正在确认恢复姿态：{recovery}" in text - - -def test_status_numeric_diagnostics_are_json_serializable() -> None: - bins = [0, 16, 255] - payload = { - "state": "SWEEP", - "reason": "collecting_timestamp_synchronised_tag_centres", - "active": { - "sample": { - # np.diff返回NumPy标量；节点必须在放入状态前转成原生int。 - "maximum_bin_gap": int(max(np.diff(bins), default=0)), - } - }, - } - - encoded = json.dumps(payload, ensure_ascii=False) - - assert '"maximum_bin_gap": 239' in encoded - - -def test_urdf_zero_bound_has_specific_chinese_scale_guidance() -> None: - payload = { - "state": "PAUSED", - "reason": ( - "URDF zero offset reached the configured 20.000 degree bound: " - "thumb_cmc_roll=+20.000deg, index_pip=-20.000deg; " - "all_offsets: thumb_cmc_roll=+20.000deg" - ), - "progress": 1.0, - "completed_sweeps": 42, - "total_sweeps": 42, - "active": {}, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "触及±20.000°安全边界" in text - assert "拇指CMC滚转=+20.000deg" in text - assert "食指PIP=-20.000deg" in text - assert "不要调用resume" in text - assert "Tag有效黑框边长" in text - assert "未分类原因码" not in text - - -def test_index_roll_status_prints_clearance_motor_feedback() -> None: - payload = { - "state": "SWEEP", - "reason": "collecting_timestamp_synchronised_tag_centres", - "progress": 0.43, - "completed_sweeps": 18, - "total_sweeps": 42, - "active": { - "kind": "sweep", - "view": "front", - "motor_index": 6, - "joints": ["index_mcp_roll"], - "cycle": 1, - "repetitions": 3, - "automatic_retry_count": 1, - "automatic_retry_limit": 2, - "start_u8": 255, - "target_u8": 0, - "actual_u8": 44.0, - "motion_progress": 0.827, - "valid_frames": 971, - "sample": {"minimum_u8": 40.0, "maximum_u8": 253.0}, - "auxiliary_motors": [ - {"motor_index": 7, "command_u8": 0, "actual_u8": 0.0}, - {"motor_index": 8, "command_u8": 0, "actual_u8": 1.0}, - {"motor_index": 9, "command_u8": 0, "actual_u8": 0.0}, - ], - "speed": { - "commanded_finger_speed": [15, 5, 15, 15, 15], - "reported_finger_speed": [15, 5, 15, 15, 15], - }, - }, - "views": {}, - "result_path": "", - } - - text = render_three_camera_status_text_zh(payload) - - assert "避挡姿态：电机7目标0、实际0.0" in text - assert "电机8目标0、实际1.0" in text - assert "电机9目标0、实际0.0" in text - assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text - assert "SDK报告[15, 5, 15, 15, 15]" in text - assert "自动重试：当前方向已自动重扫1/2次" in text - - -def test_same_finger_transition_explains_that_clearance_stays_parked() -> None: - explanation, action = three_camera_reason_zh( - "RETURN_BASELINE", - "holding_same_finger_clearance_before_next_task", - {}, - ) - - assert "继续保持当前避让姿态" in explanation - assert "只调整被测关节" in explanation - assert "不要手动展开" in action - - -def test_fixed_base_reference_movement_requires_a_new_session() -> None: - explanation, action = three_camera_reason_zh( - "PAUSED", - "fixed_base_reference_moved", - {}, - ) - - assert "Tag 8" in explanation - assert "基准锁定后" in explanation - assert "下一次标定预检前重新摆放" in action - assert "重新启动新会话" in action diff --git a/src/linkerhand_calibration/test/test_trajectory.py b/src/linkerhand_calibration/test/test_trajectory.py deleted file mode 100644 index ad40ed3..0000000 --- a/src/linkerhand_calibration/test/test_trajectory.py +++ /dev/null @@ -1,297 +0,0 @@ -from __future__ import annotations - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core import ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - PHASE_ROOT, - PHASE_TIP, -) -from linkerhand_calibration.compat.legacy.thumb_core import ( - PAIR_IP, - PAIR_MCP, - PAIR_ROOT, - create_final_payload, -) -from linkerhand_calibration.trajectory import ( - _regularize_coupled_zero_tail, - fit_center_trajectory_curves, - maximum_center_non_target_drift_rad, - measure_center_trajectory_angles, -) - - -def _quat(angle: float) -> list[float]: - return [ - float(value) - for value in Rotation.from_rotvec([0.0, 0.0, angle]).as_quat() - ] - - -def _records( - *, - camera_rotation: Rotation = Rotation.identity(), - camera_translation: np.ndarray = np.zeros(3), - tag_shift: float = 0.0, -) -> list[dict]: - commands = list(range(0, 256, 8)) - if commands[-1] != 255: - commands.append(255) - directions = ( - (DIRECTION_DECREASING, list(reversed(commands))), - (DIRECTION_INCREASING, commands), - ) - t0 = np.asarray([0.0, 0.0, 0.55]) - root_centre = np.asarray([0.025, -0.010, 0.55]) - root_points = { - "t3": np.asarray([0.060 + tag_shift, -0.005, 0.55]), - "t4": np.asarray([0.090, 0.002 + tag_shift, 0.55]), - "t5": np.asarray([0.120, 0.009, 0.55 + tag_shift]), - } - mcp_centre = np.asarray([0.055, -0.004, 0.0]) - t3_tip = np.asarray([0.0, 0.0, 0.55]) - t4_reference = t3_tip + np.asarray([0.080, 0.006 + tag_shift, 0.0]) - ip_centre = t3_tip + np.asarray([0.095, 0.006, 0.0]) - t5_reference = t3_tip + np.asarray([0.125, 0.008 + tag_shift, 0.0]) - - def camera(point: np.ndarray) -> np.ndarray: - return camera_rotation.apply(point) + camera_translation - - records: list[dict] = [] - for phase in (PHASE_ROOT, PHASE_TIP): - for direction, ordered_commands in directions: - for command in ordered_commands: - progress = (255.0 - command) / 255.0 - root_angle = 0.80 * progress if phase == PHASE_ROOT else 0.0 - mcp_angle = 1.15 * progress if phase == PHASE_TIP else 0.0 - ip_angle = 1.02 * mcp_angle if phase == PHASE_TIP else 0.0 - if phase == PHASE_ROOT: - root_rotation = Rotation.from_rotvec( - [0.0, 0.0, root_angle] - ) - positions = { - "t0": t0, - **{ - role: root_centre - + root_rotation.apply(point - root_centre) - for role, point in root_points.items() - }, - } - else: - mcp_rotation = Rotation.from_rotvec( - [0.0, 0.0, mcp_angle] - ) - ip_rotation = Rotation.from_rotvec( - [0.0, 0.0, ip_angle] - ) - # The MCP centre below is expressed relative to T3. - mcp_world = t3_tip + mcp_centre - t4 = mcp_world + mcp_rotation.apply( - t4_reference - mcp_world - ) - ip_at_zero = ip_centre - t5_inside_parent = ip_at_zero + ip_rotation.apply( - t5_reference - ip_at_zero - ) - t5 = mcp_world + mcp_rotation.apply( - t5_inside_parent - mcp_world - ) - positions = { - "t0": t0, - "t3": t3_tip, - "t4": t4, - "t5": t5, - } - records.append( - { - "kind": "sample", - "phase": phase, - "cycle": 0, - "direction": direction, - "command_u8": command, - "relative_quaternion_xyzw": { - PAIR_ROOT: _quat(root_angle), - PAIR_MCP: _quat(mcp_angle), - PAIR_IP: _quat(ip_angle), - }, - "tag_translation_xyz_m": { - role: [ - float(value) for value in camera(point) - ] - for role, point in positions.items() - }, - } - ) - return records - - -def test_centre_trajectory_recovers_three_joint_angles_and_zero() -> None: - records = _records() - fit = fit_center_trajectory_curves( - records, - maximum_plane_rms_m=0.001, - maximum_radial_rms_m=0.001, - maximum_anchor_drift_m=0.001, - ) - assert fit.measurement_mode == "trajectory_center_3d" - assert fit.joints["thumb_cmc_pitch"]["angle_rad"][0] == pytest.approx( - 0.80, abs=2.0e-3 - ) - assert fit.joints["thumb_mcp"]["angle_rad"][0] == pytest.approx( - 1.15, abs=2.0e-3 - ) - assert fit.joints["thumb_ip"]["angle_rad"][0] == pytest.approx( - 1.173, abs=3.0e-3 - ) - for joint in fit.joints.values(): - assert joint["angle_rad"][255] == 0.0 - payload = create_final_payload( - serial_number="G20_LEFT_TRAJECTORY_TEST", - fit=fit, - validation_errors_rad=[0.01, -0.01], - passed=True, - ) - assert payload["zero_command_u8"] == 255 - for joint in payload["joints"].values(): - assert len(joint["angle_rad"]) == 256 - assert joint["angle_rad"][255] == 0.0 - - -def test_centre_trajectory_is_invariant_to_camera_and_tag_offset() -> None: - reference = fit_center_trajectory_curves(_records()) - changed = fit_center_trajectory_curves( - _records( - camera_rotation=Rotation.from_euler( - "xyz", [0.35, -0.25, 0.20] - ), - camera_translation=np.asarray([0.12, -0.04, 0.08]), - tag_shift=0.004, - ) - ) - for joint_name in ("thumb_cmc_pitch", "thumb_mcp", "thumb_ip"): - assert changed.joints[joint_name]["angle_rad"] == pytest.approx( - reference.joints[joint_name]["angle_rad"], - abs=6.0e-3, - ) - - -def test_passive_ip_uses_mimic_constraint_despite_distal_pnp_bias() -> None: - reference = fit_center_trajectory_curves(_records()) - biased_records = _records( - camera_rotation=Rotation.from_euler( - "xyz", [-0.28, 0.31, -0.16] - ), - camera_translation=np.asarray([-0.08, 0.03, 0.11]), - ) - for record in biased_records: - if record["phase"] != PHASE_TIP: - continue - progress = (255.0 - float(record["command_u8"])) / 255.0 - bias = np.asarray( - [ - 0.0012 * np.sin(1.7 * progress), - 0.0008 * progress * progress, - -0.0006 * np.sin(2.3 * progress), - ] - ) - record["tag_translation_xyz_m"]["t5"] = [ - float(value) - for value in ( - np.asarray( - record["tag_translation_xyz_m"]["t5"], dtype=float - ) - + bias - ) - ] - - biased = fit_center_trajectory_curves(biased_records) - for fit in (reference, biased): - mcp = np.asarray(fit.joints["thumb_mcp"]["angle_rad"]) - ip = np.asarray(fit.joints["thumb_ip"]["angle_rad"]) - assert ip == pytest.approx(1.02 * mcp, abs=1.1e-8) - assert fit.ip_coupling["multiplier"] == pytest.approx(1.02) - assert fit.ip_coupling["offset_rad"] == 0.0 - assert fit.ip_coupling["constrained_r_squared"] == 1.0 - assert fit.ip_coupling["r_squared"] > 0.98 - - assert biased.joints["thumb_ip"]["angle_rad"] == pytest.approx( - reference.joints["thumb_ip"]["angle_rad"], - abs=6.0e-3, - ) - assert ( - biased.trajectory_quality["tip"][ - "ip_observed_vs_constrained_max_rad" - ] - > 0.0 - ) - - -def test_passive_ip_multiplier_is_configurable() -> None: - fit = fit_center_trajectory_curves( - _records(), - passive_ip_multiplier=0.97, - ) - mcp = np.asarray(fit.joints["thumb_mcp"]["angle_rad"]) - ip = np.asarray(fit.joints["thumb_ip"]["angle_rad"]) - assert ip == pytest.approx(0.97 * mcp, abs=1.1e-8) - - -def test_static_measurement_uses_fitted_serial_tip_model() -> None: - records = _records() - fit = fit_center_trajectory_curves(records) - command = 128 - record = next( - item - for item in records - if item["phase"] == PHASE_TIP - and item["direction"] == DIRECTION_DECREASING - and item["command_u8"] == command - ) - measured = measure_center_trajectory_angles( - fit.trajectory_models, - record["tag_translation_xyz_m"], - ) - progress = (255.0 - command) / 255.0 - assert measured["thumb_mcp"] == pytest.approx( - 1.15 * progress, abs=2.0e-3 - ) - assert measured["thumb_ip"] == pytest.approx( - 1.02 * 1.15 * progress, abs=3.0e-3 - ) - - -def test_non_target_drift_is_measured_without_tag_orientations() -> None: - records = _records() - fit = fit_center_trajectory_curves(records) - assert maximum_center_non_target_drift_rad( - records, fit.trajectory_models - ) == pytest.approx(0.0, abs=3.0e-3) - - -def test_short_ip_zero_tail_uses_coupled_mcp_shape() -> None: - mcp = np.linspace(1.0, 0.0, 256) - ip = 0.6 * mcp - ip[248:] = 0.0 - - regularized = _regularize_coupled_zero_tail(ip, mcp) - - assert regularized[:248] == pytest.approx(ip[:248]) - assert np.all(regularized[248:255] > 0.0) - assert np.all(np.diff(regularized) <= 1.0e-12) - assert regularized[255] == 0.0 - assert regularized[248:255] == pytest.approx( - 0.6 * mcp[248:255] - ) - - -def test_long_or_unresolved_ip_zero_tail_is_not_invented() -> None: - mcp = np.linspace(1.0, 0.0, 256) - ip = 0.6 * mcp - ip[220:] = 0.0 - - regularized = _regularize_coupled_zero_tail(ip, mcp) - - assert regularized == pytest.approx(ip) diff --git a/src/linkerhand_calibration/test/test_trajectory_observation_v3.py b/src/linkerhand_calibration/test/test_trajectory_observation_v3.py deleted file mode 100644 index 3dea0f9..0000000 --- a/src/linkerhand_calibration/test/test_trajectory_observation_v3.py +++ /dev/null @@ -1,207 +0,0 @@ -"""v3 trajectory observation, bounded deferral and publication refusal.""" - -import hashlib -from types import SimpleNamespace - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import ( - STAGED, TASKWISE_STAGED, configure_training, select_task_training, -) -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.observer_policy import TRAJECTORY_OBSERVER -from linkerhand_calibration.profiles.observer_policy import configure_observer_policy -from linkerhand_calibration.profiles.observer_preparation import configure_observer_preparations -from linkerhand_calibration.profiles.export import dump_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation, preparation_contract -from linkerhand_calibration.runtime.artifacts.capture_validation import validate_capture_units -from linkerhand_calibration.runtime.session import CalibrationPhase, CalibrationSession -from linkerhand_calibration.runtime.checkpoint_observer_upgrade import ObserverPolicyUpgrade -from linkerhand_calibration.runtime.task_deferral import ( - POLICY, TaskDeferralQueue, TaskObservationState, deferred_visit_keys, - dependency_closure, recoverable_observation_reason, -) -from o30_capture_fixture import SOURCE - - -@pytest.fixture -def trajectory_profile(): - profile = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) - profile = configure_preparation(profile, 'occlusion_aware_trajectory_v3') - return configure_observer_preparations(profile, UrdfKinematicModel(SOURCE)) - - -def test_v3_is_a_versioned_profile_contract_with_two_action_bound(trajectory_profile): - policy = preparation_contract(trajectory_profile)['observer_preparations'] - assert policy['policy'] == 'occlusion_aware_trajectory_v3' - assert policy['trajectory_solver']['holdout_selects_model'] is False - assert policy['trajectory_solver']['maximum_actions_per_task'] == 2 - assert all(1 <= len(plan.actions) <= 2 - for plan in trajectory_profile.motion.observer_preparations.values()) - assert trajectory_profile.measurement.measurements['index_dip'].observer_pose_policy \ - == TRAJECTORY_OBSERVER - - -def test_visual_ambiguity_is_deferred_but_hardware_failure_is_not(): - assert recoverable_observation_reason('image_motion_frame_angles_ambiguous') - assert recoverable_observation_reason('motion_candidates_missing') - assert not recoverable_observation_reason('sdk_disconnected') - assert not recoverable_observation_reason('feedback_stalled') - assert not recoverable_observation_reason('over_temperature') - - -def test_parent_deferral_moves_dependency_closure_to_stage_end(trajectory_profile): - assert 'index_dip_side' in dependency_closure(trajectory_profile, 'index_pip_side') - session = CalibrationSession(trajectory_profile) - session.phase = CalibrationPhase.PREPARE - session._unit_index = next(i for i, unit in enumerate(session._units) - if unit.task_key == 'index_pip_side' and unit.cycle == 0) - original_prefix = tuple(unit.identity for unit in session._units[:session._unit_index]) - decision = session.defer_current_task('rigid_pair_candidates_unresolved') - assert decision.state == TaskObservationState.PENDING - assert {'index_pip_side', 'index_dip_side'} <= set(decision.affected_tasks) - record = decision.as_record(stamp_ns=1) - assert record['dependency_states']['index_dip_side'] == 'dependency_pending' - assert tuple(unit.identity for unit in session._units[:session._unit_index]) == original_prefix - same_cycle = [unit.task_key for unit in session._units if unit.cycle == 0] - assert same_cycle[-1] == 'index_dip_side' - - -def test_staged_assessment_waits_for_deferred_visit(trajectory_profile): - from linkerhand_calibration.runtime.execution import SessionExecution - execution = SessionExecution(trajectory_profile) - session = execution.session - session.phase = CalibrationPhase.PREPARE - session._unit_index = next(i for i, unit in enumerate(session._units) - if unit.task_key == 'index_pip_side' and unit.cycle == 1) - session.defer_current_task('rigid_pair_candidates_unresolved') - cycle = [unit for unit in session.scheduled_units if unit.cycle == 1] - assert cycle[-1].task_key == 'index_dip_side' - session._unit_index = session._units.index(cycle[-2]) - assert not execution.training_check_due() - session._unit_index = session._units.index(cycle[-1]) - assert execution.training_check_due() - - -def test_second_failure_is_terminal_and_budget_survives_restart(trajectory_profile): - queue = TaskDeferralQueue(trajectory_profile) - first = queue.decide('index_dip_side', 0, 'rigid_pair_candidates_unresolved', 2) - records = [first.as_record(stamp_ns=1, corner_evidence_sha256='a'*64)] - restored = TaskDeferralQueue(trajectory_profile) - restored.restore(records) - second = restored.decide('index_dip_side', 0, - 'rigid_pair_candidates_unresolved', 2) - assert second.state == TaskObservationState.UNOBSERVABLE - records.append(second.as_record(stamp_ns=2, corner_evidence_sha256='b'*64)) - again = TaskDeferralQueue(trajectory_profile) - again.restore(records) - assert again.attempts[('index_dip_side', 0)] == 2 - assert ('index_dip_side', 0) in again.unobservable - with pytest.raises(ValueError, match='budget_exhausted'): - again.decide('index_dip_side', 0, 'rigid_pair_candidates_unresolved', 1) - with pytest.raises(ValueError, match='budget_changed'): - again.restore([first.as_record(stamp_ns=3)]) - - -def test_restart_restores_pending_visit_at_end_of_taskwise_stage(trajectory_profile): - profile = configure_training(trajectory_profile, TASKWISE_STAGED) - profile = select_task_training(profile, profile.motion.tasks[0].key, (0, 1, 2)) - queue = TaskDeferralQueue(profile) - pending = queue.decide('index_dip_side', 0, - 'image_motion_no_observable_image_model', 2).as_record(stamp_ns=1) - session = CalibrationSession(profile) - session.restore_task_deferrals([pending]) - first_stage = [unit for unit in session.scheduled_units if unit.cycle == 0] - assert first_stage[-1].task_key == 'index_dip_side' - assert session.task_deferrals.attempts[('index_dip_side', 0)] == 1 - - # The same rule treats cycle 1 and optional cycle 2 as one training stage, - # so a restored revisit cannot jump ahead of later training or into holdout. - queue = TaskDeferralQueue(profile) - pending = queue.decide('index_dip_side', 1, - 'image_motion_no_observable_image_model', 2).as_record(stamp_ns=2) - session = CalibrationSession(profile) - session.restore_task_deferrals([pending]) - training_stage = [unit for unit in session.scheduled_units if unit.cycle in {1, 2}] - assert training_stage[-1].task_key == 'index_dip_side' - assert session.scheduled_units[session.scheduled_units.index(training_stage[-1]) + 1].cycle == 3 - - -def test_deferral_audit_allows_only_declared_noncontiguous_resume_gap(trajectory_profile): - queue = TaskDeferralQueue(trajectory_profile) - pending = queue.decide('index_pip_side', 1, - 'rigid_pair_candidates_unresolved', 4).as_record(stamp_ns=1) - keys = deferred_visit_keys(trajectory_profile, [pending]) - assert ('index_pip_side', 1) in keys - assert ('index_dip_side', 1) in keys - changed = {**pending, 'affected_tasks': ['index_pip_side']} - with pytest.raises(ValueError, match='dependencies_changed'): - deferred_visit_keys(trajectory_profile, [changed]) - - -def test_full_visit_commit_closes_crash_window_but_partial_visit_does_not(trajectory_profile): - from linkerhand_calibration.runtime.engine import CalibrationEngine - queue = TaskDeferralQueue(trajectory_profile) - pending = queue.decide('index_dip_side', 1, - 'rigid_pair_candidates_unresolved', 2).as_record(stamp_ns=1) - units = [unit for unit in CalibrationEngine(trajectory_profile).scan_units() - if (unit.task_key, unit.cycle) == ('index_dip_side', 1)] - completed = [dict(kind='scan_unit_complete', task_name=unit.task_key, - cycle=unit.cycle, direction=unit.direction, segment_key=unit.segment_key, - passed=True) for unit in units] - partial = TaskDeferralQueue(trajectory_profile) - partial.restore([pending, *completed[:-1]]) - assert ('index_dip_side', 1) not in partial.resolved - restored = TaskDeferralQueue(trajectory_profile) - restored.restore([pending, *completed]) - assert ('index_dip_side', 1) in restored.inferred_resolutions - - -def test_unobservable_record_blocks_release_before_fitting(trajectory_profile): - queue = TaskDeferralQueue(trajectory_profile) - first = queue.decide('index_dip_side', 0, - 'rigid_pair_candidates_unresolved', 2).as_record(stamp_ns=1) - row = queue.decide('index_dip_side', 0, - 'rigid_pair_candidates_unresolved', 2).as_record(stamp_ns=2) - with pytest.raises(ValueError, match='capture_unobservable_tasks:index_dip_side:0:unobservable'): - validate_capture_units(trajectory_profile, [first, row]) - - -def test_resolved_revisit_clears_release_blocker_state(trajectory_profile): - queue = TaskDeferralQueue(trajectory_profile) - first = queue.decide('index_dip_side', 0, 'rigid_pair_candidates_unresolved', 2) - passed = queue.resolve('index_dip_side', 0) - assert passed is not None and passed.state == TaskObservationState.PASSED - restored = TaskDeferralQueue(trajectory_profile) - restored.restore([first.as_record(stamp_ns=1), passed.as_record(stamp_ns=2)]) - assert ('index_dip_side', 0) in restored.resolved - assert not restored.unobservable - - -def test_v3_checkpoint_upgrade_preserves_accepted_joint_semantics(): - base = configure_training(load_bundled_hand_profile('o30_right_18'), STAGED) - source = configure_preparation(base, 'occlusion_aware_witness_v2') - source = configure_observer_preparations(source, UrdfKinematicModel(SOURCE)) - preserved = set(source.measurement.measurements)-{'index_dip'} - target = configure_observer_policy(source, preserved_joints=preserved, - policy=TRAJECTORY_OBSERVER) - source_yaml, target_yaml = (dump_hand_profile(profile).encode() - for profile in (source, target)) - upgrade = ObserverPolicyUpgrade(source, target, source_yaml, target_yaml, - source_urdf=SOURCE) - inputs = {'profile_config_sha256': hashlib.sha256(target_yaml).hexdigest(), - 'source_urdf_sha256': 'a'*64} - old_inputs = {**inputs, 'profile_config_sha256': hashlib.sha256(source_yaml).hexdigest()} - header = {'kind': 'session_start', **old_inputs, - 'preparation_contract': preparation_contract(source)} - reference = {'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=set())) - _, _, audit = upgrade.metadata(checkpoint, inputs, 'source.jsonl', 'c'*64) - assert audit['policy'] == 'unmeasured_observer_upgrade_v3' - assert audit['preserved_joints'] == sorted(preserved) - assert audit['observer_policy_changes'] == { - 'index_dip': {'previous': 'rigid_pair_candidates_v2', 'current': TRAJECTORY_OBSERVER}} diff --git a/src/linkerhand_calibration/test/test_trajectory_policy.py b/src/linkerhand_calibration/test/test_trajectory_policy.py deleted file mode 100644 index 9d67457..0000000 --- a/src/linkerhand_calibration/test/test_trajectory_policy.py +++ /dev/null @@ -1,39 +0,0 @@ -from linkerhand_calibration.runtime import avoidance_arrival, smoothstep_position - - -def test_return_sweep_prepares_its_own_start_not_the_forward_start(): - from linkerhand_calibration.profiles.loader import load_bundled_hand_profile - from linkerhand_calibration.runtime.engine import CalibrationEngine - from linkerhand_calibration.runtime.trajectory import build_calibration_preparation_waypoints - profile = load_bundled_hand_profile("o12_right_16") - task = profile.motion.tasks[0] - reverse = CalibrationEngine(profile).scan_units()[1] - waypoints = build_calibration_preparation_waypoints(task, profile=profile, start_value=reverse.start) - assert waypoints[-1][task.command_index] == task.end_value - - -def test_cosine_trajectory_has_exact_endpoints() -> None: - assert smoothstep_position(0.0, 1.0, 0.0) == 0.0 - assert smoothstep_position(0.0, 1.0, 1.0) == 1.0 - - -def test_avoidance_accepts_stable_eighty_percent_without_exact_tracking() -> None: - result = avoidance_arrival( - start_feedback=0.0, - requested_target=1.5, - feedback_history=(1.20, 1.205, 1.21, 1.205), - trajectory_complete=True, - ) - assert result.arrived - assert result.progress_01 >= 0.8 - - -def test_avoidance_does_not_accept_wrong_direction() -> None: - result = avoidance_arrival( - start_feedback=0.0, - requested_target=1.5, - feedback_history=(-0.3, -0.31, -0.30), - trajectory_complete=True, - ) - assert not result.arrived - assert not result.direction_correct diff --git a/src/linkerhand_calibration/test/test_transfer_artifact_contract.py b/src/linkerhand_calibration/test/test_transfer_artifact_contract.py deleted file mode 100644 index 2524fea..0000000 --- a/src/linkerhand_calibration/test/test_transfer_artifact_contract.py +++ /dev/null @@ -1,239 +0,0 @@ -"""Synthetic transfer artifacts retain provenance without recipient evidence.""" - -from copy import deepcopy -from dataclasses import replace -import hashlib -import json -from types import SimpleNamespace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.domain.measurement import JointCurveFit -from linkerhand_calibration.core.domain.reference import JointZeroPose, JointZeroReference, JointZeroSample -from linkerhand_calibration.core.domain.result import JointMapping -from linkerhand_calibration.core.fitting.coupling import LinearMimicFit -from linkerhand_calibration.core.fitting.mimic_approximation import MimicApproximation -from linkerhand_calibration.core.fitting.tag_installation import FrozenTagInstallation, matrix_tuple -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.core.urdf.measured_acceptance import validate_measured_joint_holdout -from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout -from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher, FrozenTagArtifactValidator -from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper -from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import from_report, serialize_report - - -@pytest.fixture -def transfer_artifacts(tmp_path): - """Two synthetic chains; only source/source_tip have observation evidence.""" - session = tmp_path / "session" - session.mkdir() - source = tmp_path / "source.urdf" - parts = [''] - for name, parent, xyz, mimic in ( - ("source", "base", "0 0 0", None), ("source_tip", "source_link", ".04 0 0", "source"), - ("target", "base", "0 .1 0", None), ("target_tip", "target_link", ".04 0 0", "target")): - parts.append(f'' - f'' - '' - + (f'' if mimic else '') + '') - source.write_text(''.join(parts) + '') - urdf = session / "corrected.urdf" - urdf.write_bytes(source.read_bytes()) - model = UrdfKinematicModel(source) - transfers = {"target": "source", "target_tip": "source_tip"} - assumptions = {name: "synthetic accepted CAD baseline" for name in ("source_tip", "target_tip")} - profile = SimpleNamespace(key=SimpleNamespace(profile_id="VIRTUAL/right/transfers/v1", model="VIRTUAL", side="right"), - command_based_release=False, vision_motion=False, retained_joints=frozenset(), - command=SimpleNamespace(unit="u8", names=("source", "target"), baseline_values=(0, 0), - feedback_by_index=True, feedback_name_aliases={}), - measurement=SimpleNamespace(transferred_motion_sources=transfers, release_basis="feedback_and_command"), - artifacts=SimpleNamespace(directional_command_joints=frozenset()), - acquisition=SimpleNamespace(command_capture_mode="interleaved", motion_observation="feedback", - fixed_reference_mode="tag_pose"), - zero=SimpleNamespace(known_baseline_geometry={}, cad_zero_assumptions=assumptions, - transferred_zero_sources={"target": "source"})) - knots = tuple(float(i) for i in range(0, 256, 51)) - curves, mappings, commands, references = {}, {}, {}, {} - identity = JointZeroPose((0, 0, 0, 1), (0, 0, 0)) - for name in ("source", "source_tip"): - x = np.asarray(knots) / 255 - angles = tuple(x if name == "source" else .2*x + .4*x*x) - mappings[name] = JointMapping(name, 0, "feedback_u8", knots, angles, angles, angles) - commands[name] = replace(mappings[name], input_domain="command_u8") - curves[name] = JointCurveFit(angles, angles, angles, { - "space": "relative_rotation_3d", "reference_quaternion_xyzw": (0, 0, 0, 1), - "axis_xyz": (0, 0, 1), "training_sample_ids": (f"training:{name}",)}, 0, 0, {}) - sample = JointZeroSample("side:1", "side", 1, (0, 0, 0, 1), (0, 0, 0), - (0, 0), (0, 0), ("decreasing", "decreasing"), identity, identity) - references[name] = JointZeroReference(name, "source_scan", 0, 0, 1, 1, (sample,)) - for target, donor in transfers.items(): - mappings[target] = replace(mappings[donor], joint=target, motor_index=1, transferred_from=donor) - commands[target] = replace(commands[donor], joint=target, motor_index=1, transferred_from=donor) - methods = {"source": "urdf_serial_axis_geometry", "source_tip": "assumed_source_cad_zero", - "target": "transferred_static_zero_on_own_cad", "target_tip": "assumed_source_cad_zero"} - mimic_output = {"source_tip": ("source", .6, 0), "target_tip": ("target", .6, 0)} - approximations = {} - for child, (parent, multiplier, offset) in mimic_output.items(): - approximate = LinearMimicFit(parent, child, multiplier, offset, True, (0, 1, 2), (0, -.1, 0)) - approximations[child] = MimicApproximation(approximate, ("training:source_tip",), ("held:source_tip",), - (0, -.1, 0), transferred_from_joint=transfers.get(child)) - fit = SimpleNamespace(curves=curves, output_mappings=mappings, command_mappings=commands, - zero_offsets_rad={name: 0 for name in mappings}, zero_method_by_joint=methods, - joint_zero_references=references, mimic_approximations=approximations, - command_applicability={name: {"scope": "single_channel_at_declared_pose"} for name in curves}, - command_holdout_metrics={name: {"independently_measured": True} for name in curves}) - prepared = SimpleNamespace(mappings=mappings, plan=SimpleNamespace(mimic_output=mimic_output)) - hashes = {"source_urdf_sha256": hashlib.sha256(source.read_bytes()).hexdigest()} - report = serialize_report(profile, fit, prepared, serial_number="SYNTHETIC_ONLY", protected_inputs=hashes) - payload = from_report(report) - observations, poses = [], [] - for name in curves: - for direction in ("increasing", "decreasing"): - for index, value in enumerate(knots): - angles = {joint: mapping.evaluate(value if mapping.motor_index == 0 else 0) - for joint, mapping in commands.items()} - image = f"holdout:{direction}:{index}" - observations.append({"joint": name, "sample_id": image, "cycle": 3, - "sample_phase": "steady", "direction": direction, "command_vector_u8": (value, 0), - "relative_quaternion_xyzw": Rotation.from_rotvec([0, 0, angles[name]]).as_quat().tolist()}) - transform = model.link_transform(name, zero_offsets={}, joint_angles=angles, independent_mimic_angles=True) - poses.append(TagHoldout(image, name, 3, (value, 0), {name: direction}, matrix_tuple(transform))) - mounts = {name: FrozenTagInstallation(name, name+"_link", matrix_tuple(np.eye(4)), - (f"training:{name}",), 0, 0) for name in curves} - validator = FrozenTagArtifactValidator(profile.key.profile_id, source, hashes["source_urdf_sha256"], - {}, fit.zero_offsets_rad, matrix_tuple(np.eye(4)), mounts, tuple(poses), tuple(curves), lambda _: None, - minimum_samples=3, protected_inputs=hashes, command_observations=tuple(poses), - joint_observations=tuple(observations), joint_curves=curves, - joint_channels={name: mapping.motor_index for name, mapping in mappings.items()}, - transferred_motion_sources=transfers, cad_zero_assumptions=assumptions, - transferred_zero_sources={"target": "source"}, joint_zero_methods=methods) - json_path = session / "calibration.json" - json_path.write_text(json.dumps(payload)) - (session / "calibration_report.json").write_text(json.dumps(report)) - return SimpleNamespace(profile=profile, report=report, payload=payload, fit=fit, validator=validator, - json_path=json_path, urdf=urdf, observations=observations, transfers=transfers) - - -def test_mixed_release_preserves_recipient_channels_without_claiming_recipient_precision(transfer_artifacts): - data = transfer_artifacts - release = ArtifactPublisher(data.json_path.parent.parent, "latest").publish( - session_directory=data.json_path.parent, calibration_json=data.json_path, - corrected_urdf=data.urdf, validate=data.validator) - validation = json.loads(release.manifest.read_text())["validation"] - assert validation["acceptance"] == "measured_and_transferred_json_urdf_v3" - assert set(validation["independent_mapping_holdout"]) == {"source", "source_tip"} - assert set(validation["full_json_urdf_holdout"]) == {"source", "source_tip"} - assert validation["transferred_joints"] == ["target", "target_tip"] - assert max(row["maximum_deg"] for row in validation["urdf_mimic_approximation"].values()) > 3 - mapper = load_unified_mapper(release.manifest) - positions = dict(zip(mapper.urdf_joint_names, mapper.map_positions((0, 102)))) - assert positions["source"] == 0 - assert positions["target"] == pytest.approx(.4) - assert positions["target_tip"] == pytest.approx(.144) - assert mapper.transferred_motion_sources == data.transfers - for target, donor in data.transfers.items(): - assert data.payload["joints"][target]["angle_rad"] == data.payload["joints"][donor]["angle_rad"] - row = data.report["joints"][target] - assert row["independently_measured"] is False - assert "zero_reference" not in row and "command_holdout" not in row - assert row["zero_reference_source_joint"] == donor - - -@pytest.mark.parametrize("corruption", ["undeclared", "pretend_measured", "invent_reference", "changed_curve", "feedback_channel", "wrong_baseline", "chained_source", "invent_mimic_samples"]) -def test_reader_rejects_unfaithful_transfer_reports(transfer_artifacts, corruption): - report = deepcopy(transfer_artifacts.report) - target = report["joints"]["target"] - if corruption == "undeclared": - report.pop("transferred_motion_sources") - elif corruption == "pretend_measured": - target["independently_measured"] = True - elif corruption == "invent_reference": - target["zero_reference"] = deepcopy(report["joints"]["source"]["zero_reference"]) - elif corruption == "changed_curve": - target["command_to_rad"]["angle_rad"][1] += .001 - elif corruption == "feedback_channel": - target["feedback_to_rad"]["channel_index"] = 0 - elif corruption == "wrong_baseline": - report["baseline_command"][1] = 1 - elif corruption == "chained_source": - report["transferred_motion_sources"]["target"] = "target_tip" - else: - report["joints"]["target_tip"]["mimic_approximation"]["evidence"]["training_sample_ids"] = ["invented:recipient"] - with pytest.raises(ValueError): - from_report(report) - - -def test_publisher_binds_report_transfers_to_authoritative_profile(transfer_artifacts): - data = transfer_artifacts - with pytest.raises(ValueError, match="transfer sources differ"): - replace(data.validator, transferred_motion_sources={})(data.json_path, data.urdf) - with pytest.raises(ValueError, match="CAD zero assumptions differ"): - replace(data.validator, cad_zero_assumptions={})(data.json_path, data.urdf) - - -def test_publisher_rejects_consistently_rewritten_recipient_channel(transfer_artifacts): - data = transfer_artifacts - report = deepcopy(data.report) - for kind in ("command_to_rad", "feedback_to_rad"): - report["joints"]["target"][kind]["channel_index"] = 0 - # The file pair is internally consistent, but its channel is not the one - # declared by the actual Profile supplied independently to the publisher. - data.json_path.write_text(json.dumps(from_report(report))) - (data.json_path.parent / "calibration_report.json").write_text(json.dumps(report)) - with pytest.raises(ValueError, match="SDK channel bindings differ"): - data.validator(data.json_path, data.urdf) - - -def test_reader_rejects_cad_assumption_relabelled_as_independent_geometry(transfer_artifacts): - report = deepcopy(transfer_artifacts.report) - report["joints"]["target_tip"]["zero_method"] = "independent_known_baseline_geometry" - with pytest.raises(ValueError, match="assumed_cad_zero_provenance"): - from_report(report) - - -def test_transferred_native_rad_tables_preserve_target_channel_and_support(transfer_artifacts): - from linkerhand_calibration.runtime.artifacts.reader import UnifiedCommandMapper - data = transfer_artifacts - report = deepcopy(data.report) - report["command_unit"] = "rad" - for row in report["joints"].values(): - for kind in ("command", "feedback"): - mapping = row[kind+"_to_rad"] - mapping.update(input_domain=kind+"_rad", input_unit="rad", - knots=[value/255 for value in mapping["knots"]], valid_input_range=[0, 1]) - payload = from_report(report) - mapper = UnifiedCommandMapper(payload, data.urdf, report=report) - values = dict(zip(mapper.urdf_joint_names, mapper.map_positions((0, .4)))) - assert values["source"] == 0 - assert values["target"] == pytest.approx(.4) - assert values["target_tip"] == pytest.approx(.144) - assert payload["joints"]["target"]["input_values"] == payload["joints"]["source"]["input_values"] - with pytest.raises(ValueError, match="outside"): - mapper.map_positions((0, 1.01)) - - -def test_holdout_never_credits_copied_or_fabricated_recipient_observations(transfer_artifacts): - data = transfer_artifacts - result = validate_measured_joint_holdout(data.payload, data.fit.curves, data.observations, - transferred_motion_sources=data.transfers) - assert set(result) == {"source", "source_tip"} - with pytest.raises(ValueError, match="wrong partition or joint"): - validate_measured_joint_holdout(data.payload, data.fit.curves, - data.observations+[dict(data.observations[0], joint="target")], transferred_motion_sources=data.transfers) - wrong = deepcopy(data.payload) - wrong["joints"]["target"]["angle_rad"][102] += .001 - with pytest.raises(ValueError, match="transferred_final_table"): - validate_measured_joint_holdout(wrong, data.fit.curves, data.observations, transferred_motion_sources=data.transfers) - - -def test_recipient_limit_violation_is_rejected_instead_of_clipping(transfer_artifacts): - data = transfer_artifacts - import xml.etree.ElementTree as ET - root = ET.fromstring(data.urdf.read_text()) - root.find("joint[@name='target']/limit").set("upper", ".3") - data.urdf.write_text(ET.tostring(root, encoding="unicode")) - validator = replace(data.validator, authorized_fields={"target": ("limit.upper",)}) - with pytest.raises(ValueError, match="range|limit"): - validator(data.json_path, data.urdf) diff --git a/src/linkerhand_calibration/test/test_transferred_hinge_geometry.py b/src/linkerhand_calibration/test/test_transferred_hinge_geometry.py deleted file mode 100644 index 12bbefd..0000000 --- a/src/linkerhand_calibration/test/test_transferred_hinge_geometry.py +++ /dev/null @@ -1,126 +0,0 @@ -"""Independent motion images distinguish geometry only with bound source evidence.""" - -from copy import deepcopy -from dataclasses import asdict, replace -import gzip -import json -from pathlib import Path - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.core.geometry.image_motion_replay import image_model_sha256 -from linkerhand_calibration.core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry -from linkerhand_calibration.core.geometry.tag_pose.source_hinge import SourceHinge -from linkerhand_calibration.core.geometry.tag_pose.production_image_motion import ( - resolve_image_motion, image_motion_model_from_dict, select_image_motion_frame, -) -from linkerhand_calibration.core.geometry.tag_pose.motion_evidence import MotionRelation -from linkerhand_calibration.runtime.parent_reference_evidence import validate_source_hinges -from test_production_image_motion import recorded_image_frames -from test_cad_image_geometry import synthetic_frames - - -def source_from(result): - winner = next(h for h in result.hypotheses if h.branches == result.model.branches) - _, axis, point = winner.child_frame_uncertainty[0] - return SourceHinge(result.model.geometry[0], "a"*64, image_model_sha256(asdict(result.model)), axis, point) - - -@pytest.fixture(scope="module") -def recorded_transfer(): - payload = json.loads(gzip.decompress((Path(__file__).parent/ - "fixtures/o30_transferred_ip_geometry.json.gz").read_bytes())) - sources, parent_relations = payload["parent"], tuple(MotionRelation(**r) for r in payload["parent"]["relations"]) - parent = resolve_image_motion(recorded_image_frames(sources["frames"]), parent_relations) - assert parent.resolved, parent.reason - source = source_from(parent) - frames = recorded_image_frames(payload["child"]["frames"]) - relations = tuple(MotionRelation(**r) for r in payload["child"]["relations"]) - constraint = ParallelAxisGeometry("thumb_mcp", "thumb_ip", 1, .048556745401643224) - options = dict(geometry_constraints=(constraint,), source_urdf_sha256=payload["source_urdf_sha256"], - source_hinges=(source,)) - result = resolve_image_motion(frames, relations, **options) - return parent, frames, relations, options, result - - -def test_recorded_ip_ambiguity_resolves_without_relaxing_gates(recorded_transfer): - _, frames, relations, _, result = recorded_transfer - free = resolve_image_motion(frames, relations) - assert not free.resolved and free.reason == "image_motion_families_not_distinguishable" - assert result.resolved, result.reason - 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) - winner = next(h for h in result.hypotheses if h.branches == result.model.branches) - assert max(p for _, p in winner.comparison_p_values) < .01 - assert winner.axis_uncertainty_95_rad[0][1] >= result.model.source_hinges[0].child_axis_uncertainty_rad - assert image_motion_model_from_dict(asdict(result.model)) == result.model - - -def test_uncertain_source_cannot_be_made_exact_by_eliminating_axis_parameters(recorded_transfer): - _, frames, relations, options, _ = recorded_transfer - invalid = replace(options["source_hinges"][0], child_axis_uncertainty_rad=.2) - result = resolve_image_motion(frames, relations, **{**options, "source_hinges": (invalid,)}) - assert not result.resolved - assert any(h.reason == "image_motion_geometry_uncertain" for h in result.hypotheses) - result = resolve_image_motion(frames, relations, source_hinges=(invalid,)) - assert result.reason == "image_motion_source_hinge_requires_geometry" - - -@pytest.mark.parametrize("change", ["missing", "axis", "uncertainty", "hash", "epoch", "late"]) -def test_local_geometry_requires_the_actual_earlier_source(recorded_transfer, change): - parent, _, _, _, result = recorded_transfer - source = result.model.source_hinges[0] - original = dict(frozen_image_model=asdict(parent.model), image_model_sha256=source.model_sha256, - geometry_uncertainty=[("thumb_mcp", source.child_axis_uncertainty_rad, source.child_point_uncertainty_m)], - view="front", tag_role="thumb_mcp", session_epoch=1, source_image_stamps=parent.model.source_stamps_ns) - child = dict(frozen_image_model=asdict(result.model), image_model_sha256=image_model_sha256(asdict(result.model)), - view="front", tag_role="thumb_ip", session_epoch=1, source_image_stamps=result.model.source_stamps_ns) - evidence = {source.evidence_id: original, "b"*64: child} - validate_source_hinges(evidence) - changed = deepcopy(evidence) - if change == "missing": del changed[source.evidence_id] - elif change == "axis": - g = changed[source.evidence_id]["frozen_image_model"]["geometry"][0] - g["axis_parent_xyz"] = tuple(-v for v in g["axis_parent_xyz"]) - changed[source.evidence_id]["image_model_sha256"] = image_model_sha256(changed[source.evidence_id]["frozen_image_model"]) - elif change == "uncertainty": changed[source.evidence_id]["geometry_uncertainty"] = [("thumb_mcp", .001, .001)] - elif change == "hash": changed[source.evidence_id]["image_model_sha256"] = "f"*64 - elif change == "epoch": changed[source.evidence_id]["session_epoch"] = 2 - else: changed[source.evidence_id]["source_image_stamps"] = child["source_image_stamps"] - with pytest.raises(ValueError): - validate_source_hinges(changed) - - -@pytest.mark.parametrize("sign", [-1, 1]) -def test_source_axis_in_tag_frame_survives_upstream_motion(sign): - frames, truth = synthetic_frames(sign) - relation = MotionRelation("first", "root", "moving") - parent_frames = tuple(replace(f, evidence=replace(f.evidence, roles=f.evidence.roles[:2]), - observations=f.observations[:2]) for f in frames) - parent = resolve_image_motion(parent_frames, (relation,)) - assert parent.resolved, parent.reason - source = source_from(parent) - child_frames = [] - for frame in frames: - # Independently rendered current root poses. The parent moves between - # frames, so transporting its world-space axis would give wrong angles. - root, tip = frame.evidence.roles[1:] - root = replace(root, candidates=(truth[len(child_frames)]["moving"],), frozen=True) - child_frames.append(replace(frame, evidence=replace(frame.evidence, - stamp_ns=frame.evidence.stamp_ns+10000000000, roles=(root, tip)), observations=frame.observations[1:])) - result = resolve_image_motion(child_frames, (MotionRelation("second", "moving", "terminal"),), - geometry_constraints=(ParallelAxisGeometry("first", "second", sign, .037),), - source_urdf_sha256="c"*64, source_hinges=(source,)) - assert result.resolved, result.reason - before = asdict(result.model) - for i in (0, 9, 24, 40, 48): - selection = select_image_motion_frame(result.model, child_frames[i]) - assert selection.resolved, selection.reason - pose = next(item.pose for item in selection.selections if item.role == "terminal") - expected = truth[i]["terminal"] - assert (Rotation.from_quat(pose.quaternion_xyzw).inv()*Rotation.from_quat(expected.quaternion_xyzw)).magnitude() < 1e-4 - np.testing.assert_allclose(pose.translation_xyz_m, expected.translation_xyz_m, atol=1e-5) - assert asdict(result.model) == before diff --git a/src/linkerhand_calibration/test/test_turnaround_support.py b/src/linkerhand_calibration/test/test_turnaround_support.py new file mode 100644 index 0000000..dbeb4e7 --- /dev/null +++ b/src/linkerhand_calibration/test/test_turnaround_support.py @@ -0,0 +1,191 @@ +"""Unobserved outgoing endpoints cannot create calibrated travel.""" +import copy +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.profiles.observations import compile_tag_links +from linkerhand_calibration.core.geometry.tag_pose.urdf_command_images import UrdfCommandImages +from linkerhand_calibration.core.fitting.raw_joint_uncertainty import angle_derivatives +from linkerhand_calibration.core.fitting.command_support import validate_turnaround_mapping +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.runtime.artifacts.raw_joint_report import prepare_raw_motion_ranges + + +@pytest.fixture(scope='module') +def inputs(): + from raw_image_fixture import raw_image_rows + profile = load_bundled_hand_profile('o30_right_18') + model = UrdfKinematicModel(Path(__file__).resolve().parents[1]/ + 'urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf') + rows = copy.deepcopy(raw_image_rows(cycles=(0, 1))) + tags = {t.role: t for v in profile.vision.views for t in v.tags} + for row in rows: + row['command_direction_by_index'] = [ + 'increasing' if value == 255 else 'decreasing' if value == 0 else direction + for value, direction in zip(row['command_vector'], row['command_direction_by_index'])] + row.update(feedback_vector=row['command_vector'], target_command_vector=row['command_vector'], + command_image_skew_ns=0, state_image_sync_error_ms=0., observation_protocol='raw_joint_images_v2') + for role, item in row['tags'].items(): + item.update(tag_role=role, tag_id=tags[role].tag_id, image_stamp_ns=row['image_stamp_ns'], + pose_source='raw_corners') + bundle = UrdfCommandImages(profile, model, SimpleNamespace(transform=lambda _: np.eye(4)), + rows, compile_tag_links(profile, model)) + parameters = np.zeros(len(bundle.scale)) + parameters[5] = 1. + for columns in bundle.curve_columns.values(): + parameters[columns[columns >= 0]] = .08 + return bundle, parameters, rows + + +def test_unobserved_increment_cannot_change_curve_range_or_export(inputs): + bundle, parameters, rows = inputs + original = bundle.output_mappings(parameters) + ranges = prepare_raw_motion_ranges(bundle, parameters, rows) + shifted = parameters.copy() + for name in ('thumb_cmc_roll', 'thumb_cmc_yaw', 'thumb_mcp', 'thumb_ip'): + column = bundle.curve_columns[name][1, -1] + assert column not in bundle.optimization_columns + shifted[column] += np.radians(20) + support = ranges[name].command_support + assert support['constraints'] and support['independently_measured_outgoing_endpoint'] is False + validate_turnaround_mapping(original[name], support) + assert np.array_equal(bundle.residual(parameters), bundle.residual(shifted)) + assert bundle.output_mappings(shifted) == original + assert prepare_raw_motion_ranges(bundle, shifted, rows) == ranges + + +def test_shared_endpoint_derivative_and_uncertainty_follow_observed_branch(inputs): + bundle, parameters, _ = inputs + channel = bundle.channels['thumb_cmc_yaw'] + commands = np.tile(bundle.profile.command.baseline_values, (4, 1)).astype(float) + commands[:, channel] = [223, 240, 255, 255] + directions = np.zeros(commands.shape, dtype=int) + directions[:, channel] = [1, 1, 1, -1] + derivatives = angle_derivatives(bundle, commands, directions) + rng = np.random.default_rng(42) + vector = rng.normal(size=len(parameters)) + h = 1e-6 + plus = bundle.joint_angles(parameters+h*vector, commands, directions) + minus = bundle.joint_angles(parameters-h*vector, commands, directions) + for name in bundle.joints: + cad = bundle.cad_names[name] + assert np.allclose((plus[cad]-minus[cad])/(2*h), derivatives[cad]@vector, atol=1e-8) + assert np.all(derivatives['thumb_cmc_yaw'][:, bundle.curve_columns['thumb_cmc_yaw'][1, -1]] == 0) + numeric = (bundle.residual(parameters+h*vector)-bundle.residual(parameters-h*vector))/(2*h) + assert np.allclose(numeric, bundle.jacobian(parameters)@vector, rtol=1e-6, atol=1e-5) + + +def test_missing_endpoint_is_rejected_without_using_validation(inputs): + bundle, parameters, rows = inputs + filtered = [r for r in rows if not (r['task_name'] == 'thumb_cmc_yaw_top' + and r['command_vector'][1] == 255)] + with pytest.raises(ValueError, match='endpoint_support_missing:thumb_cmc_yaw'): + prepare_raw_motion_ranges(bundle, parameters, filtered) + holdout = [dict(r, cycle=3) for r in rows] + with pytest.raises(ValueError, match='training_partition_invalid'): + UrdfCommandImages(bundle.profile, bundle.model, bundle.extrinsics, holdout, bundle.tag_links) + + +def test_observed_return_endpoint_remains_independent(inputs): + bundle, _, rows = inputs + observed = copy.deepcopy(rows) + for row in observed: + if row['task_name'] == 'thumb_cmc_yaw_top' and row['direction'] == 'decreasing': + row['command_direction_by_index'][1] = 'decreasing' + other = UrdfCommandImages(bundle.profile, bundle.model, bundle.extrinsics, observed, bundle.tag_links) + assert not other.turnaround_constraints.get('thumb_cmc_yaw') + assert other.curve_columns['thumb_cmc_yaw'][1, -1] in other.optimization_columns + + +def test_saved_endpoint_constraint_rejects_different_curve(inputs): + bundle, parameters, rows = inputs + from dataclasses import replace + mapping = bundle.output_mappings(parameters)['thumb_cmc_yaw'] + support = prepare_raw_motion_ranges(bundle, parameters, rows)['thumb_cmc_yaw'].command_support + wrong = replace(mapping, decreasing_rad=(*mapping.decreasing_rad[:-1], mapping.decreasing_rad[-1]+.05)) + with pytest.raises(ValueError, match='differs_from_evidence'): + validate_turnaround_mapping(wrong, support) + wrong_support = copy.deepcopy(support) + wrong_support['constraints'][0]['source_cycles'] = [3] + with pytest.raises(ValueError, match='differs_from_evidence'): + validate_turnaround_mapping(mapping, wrong_support) + + +def test_unknown_initial_history_does_not_create_a_free_outgoing_endpoint(inputs): + bundle, parameters, rows = inputs + unknown = copy.deepcopy(rows) + for row in unknown: + if row['task_name'] == 'thumb_cmc_yaw_top' and row['command_vector'][1] == 255 and row['cycle'] == 0: + row['command_direction_by_index'][1] = '' + other = UrdfCommandImages(bundle.profile, bundle.model, bundle.extrinsics, unknown, bundle.tag_links) + assert other.turnaround_constraints['thumb_cmc_yaw'] + shifted = parameters.copy(); shifted[other.curve_columns['thumb_cmc_yaw'][1, -1]] += 1. + assert np.array_equal(other.residual(parameters), other.residual(shifted)) + + +def test_snapshot_can_load_offline_and_is_sealed(tmp_path): + from linkerhand_calibration.product import load_product_config + from linkerhand_calibration.runtime.raw_completion import ( + snapshot_capture_inputs, save_capture_manifest, verify_capture_manifest, + ) + package = Path(__file__).resolve().parents[1] + config = load_product_config(package/'config/o30_right_product.yaml', check_can=False) + (tmp_path/'capture_profile.yaml').write_bytes(config.profile_config.read_bytes()) + snapshot_capture_inputs(config, tmp_path) + saved = load_product_config(tmp_path/'capture_product.yaml', workspace=config.workspace, check_can=False) + assert saved.calibration_contract.typed_profile == config.calibration_contract.typed_profile + assert saved.source_urdf_sha256 == config.source_urdf_sha256 + assert saved.camera_extrinsics_sha256 == config.camera_extrinsics_sha256 + import xml.etree.ElementTree as ET + assets = {node.get('filename') for node in ET.parse(saved.source_urdf).findall('.//mesh')} + assert assets and all((saved.source_urdf.parent/name).is_file() for name in assets) + raw = tmp_path/'raw_samples.jsonl'; raw.write_text('{}\n') + save_capture_manifest(raw, profile=saved.calibration_contract.typed_profile, + serial_number=saved.serial_number, protected_inputs={}, completed_units=set(), incomplete_units={}, retries={}) + verify_capture_manifest(raw) + import json + manifest_path = tmp_path/'raw_capture_manifest.json' + original = manifest_path.read_text() + manifest = json.loads(original) + manifest['files']['../outside'] = '0'*64 + manifest_path.write_text(json.dumps(manifest)) + with pytest.raises(ValueError, match='invalid_sealed_path'): + verify_capture_manifest(raw) + manifest_path.write_text(original) + saved.camera_extrinsics.write_text('changed') + with pytest.raises(ValueError, match='sealed_file_changed'): + verify_capture_manifest(raw) + + +def test_reduced_covariance_removes_only_declared_constraints(): + bundle = object.__new__(UrdfCommandImages) + bundle.scale = np.ones(3) + bundle.optimization_columns = np.array([0, 1]) + jac = np.array([[1., 0., 0.], [0., 2., 0.], [1., 1., 0.], [2., 1., 0.]]) + fit = SimpleNamespace(jac=jac, fun=np.ones(4)*.001, x=np.zeros(3)) + covariance = bundle.covariance(fit) + assert covariance[0, 0] > 0 and covariance[1, 1] > 0 + assert np.all(covariance[2] == 0) and np.all(covariance[:, 2] == 0) + # A second unobservable parameter must still fail the original rank gate. + fit.jac[:, 1] = 0 + with pytest.raises(ValueError, match='geometry_rank_deficient'): + bundle.covariance(fit) + + +def test_linear_drive_keeps_actual_limits_and_nonzero_byte_baseline(inputs): + from linkerhand_calibration.core.urdf.linear_drive import UrdfLinearImages + bundle, parameters, _ = inputs + probe = np.tile(bundle.profile.command.baseline_values, (3, 1)).astype(float) + probe[:, 1] = [0., 128., 255.] + before = parameters.copy() + angles = UrdfLinearImages(bundle, bundle.model).joint_angles(parameters, probe, np.zeros(probe.shape)) + joint = bundle.model.joints['thumb_cmc_yaw'] + assert np.allclose(angles['thumb_cmc_yaw'], joint.lower+(joint.upper-joint.lower)*probe[:, 1]/255) + middle = bundle.model.joints['middle_mcp_roll'] + expected = middle.lower+(middle.upper-middle.lower)*196/255 + assert np.allclose(angles['middle_mcp_roll'], expected) + assert np.array_equal(parameters, before) diff --git a/src/linkerhand_calibration/test/test_unified_artifact_pair.py b/src/linkerhand_calibration/test/test_unified_artifact_pair.py deleted file mode 100644 index 73211d4..0000000 --- a/src/linkerhand_calibration/test/test_unified_artifact_pair.py +++ /dev/null @@ -1,81 +0,0 @@ -"""Both input domains must pass the actual serialized standard URDF gate.""" - -from dataclasses import replace -import copy -import json - -import numpy as np -import pytest - -from test_frozen_tag_replay import fixture -from linkerhand_calibration.core.fitting.tag_installation import matrix_tuple -from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher, FrozenTagArtifactValidator -from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper - - -def dual_fixture(tmp_path): - source, plan, base, _, mounts, feedback, legacy = fixture(tmp_path) - old = legacy["joints"]["drive"] - def mapping(kind): - knots = old["curve_input_knots_rad"] - if kind == "command": - knots = [2*v+.1 for v in knots] - return {"input_domain": kind+"_rad", "input_unit": "rad", "output_unit": "rad", - "channel_index": 0, "knots": knots, "valid_input_range": [knots[0], knots[-1]], - "interpolation": "piecewise_linear", "extrapolation": "reject", - **{key: old[key].copy() for key in ("angle_rad", "increasing_rad", "decreasing_rad")}} - parent, multiplier, offset = plan.mimic_output["passive"] - payload = {"format": "unified_calibration_v1", "schema_version": 1, - "profile_id": legacy["profile_id"], "model": "VIRTUAL", "side": "right", "serial_number": "TEST", - "command_unit": "rad", "command_names": ["vendor_channel"], "joints": { - "drive": {"urdf_joint": "drive", "passive": False, - "command_to_rad": mapping("command"), "feedback_to_rad": mapping("feedback")}, - "passive": {"urdf_joint": "passive", "passive": True, - "mimic": {"joint": parent, "multiplier": multiplier, "offset_rad": offset}}}} - commands = tuple(replace(r, sample_id="command:"+r.sample_id, sdk_values=(2*r.sdk_values[0]+.1,)) for r in feedback) - validator = FrozenTagArtifactValidator(payload["profile_id"], source, plan.source_sha256, - plan.authorized_fields, plan.zero_offsets_rad, matrix_tuple(base), mounts, - tuple(feedback), ("a", "b"), lambda _: None, command_observations=commands) - session = tmp_path/"session"; session.mkdir() - urdf = plan.write(source, session/"corrected.urdf") - path = session/"calibration.json" - path.write_text(json.dumps(payload)) - return session, path, urdf, payload, validator - - -def test_dual_final_file_gate_manifest_reader_and_standard_passive(tmp_path): - session, path, urdf, payload, validator = dual_fixture(tmp_path) - release = ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, - calibration_json=path, corrected_urdf=urdf, validate=validator) - command = load_unified_mapper(release.manifest) - feedback = load_unified_mapper(path, input_kind="feedback") - assert command.map_positions((1.1,)) == pytest.approx(feedback.map_positions((.5,)), abs=1e-9) - assert command.map_positions((1.1,))[0] == pytest.approx(.2+.8*.5-.07) - feedback.feedback_name_aliases = {"vendor_reported_name": "vendor_channel"} - assert feedback.map_positions((.5,), ("vendor_reported_name",)) == pytest.approx(command.map_positions((1.1,))) - # Unnamed vendor positions retain the fixed array contract; no G20/O12 registry. - with pytest.raises(ValueError, match="outside"): - command.map_positions((3.,)) - path.write_text(path.read_text()+" ") - with pytest.raises(ValueError, match="SHA256"): - load_unified_mapper(path) - - -@pytest.mark.parametrize("corruption", ["command_curve", "domain", "passive_bypass", "missing_command_holdout"]) -def test_good_feedback_cannot_hide_bad_command_mapping(tmp_path, corruption): - session, path, urdf, payload, validator = dual_fixture(tmp_path) - if corruption == "command_curve": - for key in ("angle_rad", "increasing_rad", "decreasing_rad"): - values = payload["joints"]["drive"]["command_to_rad"][key] - payload["joints"]["drive"]["command_to_rad"][key] = (np.asarray(values)+.07*np.sin(np.linspace(0, np.pi, len(values)))).tolist() - elif corruption == "domain": - payload["joints"]["drive"]["command_to_rad"]["input_domain"] = "feedback_rad" - elif corruption == "passive_bypass": - payload["joints"]["passive"]["command_to_rad"] = copy.deepcopy(payload["joints"]["drive"]["command_to_rad"]) - else: - validator = replace(validator, command_observations=()) - path.write_text(json.dumps(payload)) - with pytest.raises(ValueError): - ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, - calibration_json=path, corrected_urdf=urdf, validate=validator) - assert not (tmp_path/"latest").exists() diff --git a/src/linkerhand_calibration/test/test_unified_engine.py b/src/linkerhand_calibration/test/test_unified_engine.py deleted file mode 100644 index bca8644..0000000 --- a/src/linkerhand_calibration/test/test_unified_engine.py +++ /dev/null @@ -1,308 +0,0 @@ -from __future__ import annotations - -import math -from pathlib import Path - -import numpy as np -import pytest - -from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry -from linkerhand_calibration.core import ( - ArtifactPolicy, - CalibrationProfile, - CommandLayout, - MeasurementPolicy, - MeasurementSpec, - MotionPolicy, - ProfileKey, - QualityPolicy, - ScopePolicy, - TagSpec, - TaskSpec, - ViewSpec, - VisionRigSpec, - ZeroSolvePolicy, -) -from linkerhand_calibration.core.urdf import ( - UrdfJointPatch, - UrdfPatchSet, - write_urdf_patches, -) -from linkerhand_calibration.runtime import ( - ACQUISITION_POLICY_VERSION, - CalibrationEngine, -) -from linkerhand_calibration.runtime.adapters import ProfileSdkAdapter -from linkerhand_calibration.core.geometry.rotation import fit_rotation_axis -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import JointCurveFit -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.fitting import fit_coupling_model - - -def test_all_product_profiles_use_unified_engine_and_sdk_adapter() -> None: - registry = get_default_registry() - products = { - registered.profile.key.model: registered - for registered in registry - if registered.profile.key.layout - in {"g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"} - } - assert set(products) == {"G20", "L6", "O6", "O12"} - for registered in products.values(): - engine = registered.build_engine() - adapter = registered.build_sdk_adapter() - assert engine.checkpoint_token == ACQUISITION_POLICY_VERSION - assert adapter.command_layout is registered.profile.command - units = engine.scan_units() - assert len([u for u in units if u.sample_phase == "sweep"]) == len(registered.profile.motion.tasks) * 8 - separate = registered.profile.acquisition.command_capture_mode == "separate" - assert len([u for u in units if u.sample_phase == "steady"]) == len(registered.profile.motion.tasks) * (4 if separate else 0) - - -def test_only_o12_has_one_three_degree_mapping_probe() -> None: - for registered in get_default_registry(): - if registered.profile.key.layout not in { - "g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16" - }: - continue - engine = registered.build_engine() - task = registered.profile.motion.tasks[0] - probe = engine.mapping_probe_delta(task) - if registered.profile.key.model == "O12": - assert probe is not None - assert 0.0 < abs(probe) <= math.radians(3.0) - else: - assert probe is None - - -def test_short_vision_loss_and_low_ideal_rates_are_warnings() -> None: - profile = next( - item.profile for item in get_default_registry() - if item.profile.key.layout == "o12_right_16" - ) - result = CalibrationEngine(profile).evaluate_sweep( - [index / 63.0 for index in range(64)], - minimum_span=0.85, - total_frames=90, - joint_frame_rate=0.71, - feedback_hz=12.0, - detection_rate=0.72, - bin_count=64, - ) - assert result.passed - assert any(value.startswith("tag_rate=") for value in result.warnings) - assert any(value.startswith("joint_frame_rate=") for value in result.warnings) - - -def test_observability_failure_gets_only_one_same_speed_rescan() -> None: - profile = next( - item.profile for item in get_default_registry() - if item.profile.key.layout == "l6_right_8" - ) - engine = CalibrationEngine(profile) - failed = engine.evaluate_sweep( - [index / 255.0 for index in range(20)], - minimum_span=240.0 / 255.0, - total_frames=200, - joint_frame_rate=0.1, - feedback_hz=5.0, - detection_rate=0.1, - ) - assert not failed.passed - assert engine.retry_speed(40.0, 2) == 40.0 - with pytest.raises(ValueError, match="exactly one"): - engine.retry_speed(40.0, 3) - - -def test_o12_effective_endpoint_scale_is_not_an_internal_blind_spot() -> None: - profile = next( - item.profile for item in get_default_registry() - if item.profile.key.layout == "o12_right_16" - ) - result = CalibrationEngine(profile).evaluate_sweep( - [index / 255.0 for index in range(20, 256)], - minimum_span=0.85, - total_frames=236, - joint_frame_rate=1.0, - feedback_hz=50.0, - detection_rate=1.0, - ) - assert result.passed - assert result.metrics["gap_scope"] == "observed_feedback_span" - assert result.metrics["maximum_bin_gap"] == 0 - - -def test_rotation_axis_uses_observed_endpoints_for_physical_feedback() -> None: - commands = np.arange(25, 254, dtype=int) - physical_angle = (253.0 - commands.astype(float)) / 228.0 - expected_axis = np.asarray([-0.25, 0.1, -0.96], dtype=float) - expected_axis /= np.linalg.norm(expected_axis) - vectors = physical_angle[:, None] * expected_axis[None, :] - - axis = fit_rotation_axis(vectors, commands) - projections = vectors @ axis - - assert float(np.median(projections[commands <= 48])) > float( - np.median(projections[commands >= 230]) - ) - - - - -def test_directional_lookup_does_not_force_passive_curve_into_polynomial() -> None: - source = tuple(float(value) for value in np.linspace(1.5, 0.0, 256)) - target = tuple(float(value) for value in 1.2 * np.asarray(source) ** 3) - - def curve(values: tuple[float, ...]) -> JointCurveFit: - return JointCurveFit( - angle_rad=values, - decreasing_rad=values, - increasing_rad=tuple(value + 0.05 for value in values), - circle={}, - maximum_monotonic_correction_rad=0.0, - maximum_hysteresis_rad=0.05, - quality={}, - ) - - fit = fit_coupling_model( - "source", "target", curve(source), curve(target), - model="direction_aware_knots", - minimum_multiplier=0.1, - maximum_multiplier=3.0, - ) - - assert fit.model == "direction_aware_knots" - assert fit.urdf_mimic_policy == "endpoint_linear_fallback" - assert fit.urdf_mimic_multiplier == pytest.approx(target[0] / source[0]) - assert fit.residual_max_rad == 0.0 - - -def test_o12_internal_blind_spot_larger_than_one_sixteenth_is_rejected() -> None: - profile = next( - item.profile for item in get_default_registry() - if item.profile.key.layout == "o12_right_16" - ) - values = [ - index / 255.0 for index in range(256) - if not 100 <= index <= 130 - ] - result = CalibrationEngine(profile).evaluate_sweep( - values, - minimum_span=0.85, - total_frames=len(values), - joint_frame_rate=1.0, - feedback_hz=50.0, - detection_rate=1.0, - ) - assert not result.passed - assert any(value.startswith("maximum_gap=") for value in result.failures) - - -def test_old_checkpoint_policy_is_intentionally_incompatible() -> None: - from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION - profile = next( - item.profile for item in get_default_registry() - if item.profile.key.layout == "g20_right_19" - ) - engine = CalibrationEngine(profile) - assert not engine.resume_compatible({"profile_id": profile.key.profile_id}) - assert engine.resume_compatible({ - "profile_id": profile.key.profile_id, - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "capture_schedule_version": engine.capture_schedule_version, - "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, - }) - - -def test_removed_soft_stop_gates_are_not_in_live_nodes() -> None: - package = Path(__file__).resolve().parents[1] / "linkerhand_calibration/runtime" - live = "\n".join( - (package / relative).read_text(encoding="utf-8") - for relative in ("ros/calibration_node.py", "ros/io.py", "coordinator.py", "safety.py") - ) - for removed in ( - "non_target_motor_moved", - "o12_auxiliary_settle_timeout", - "o12_sdk_feedback_coupling_hard_limit", - "o12_roll_coupling_exceeded", - ): - assert removed not in live - - -def test_virtual_new_model_needs_only_adapter_profile_and_raw_urdf( - tmp_path: Path, -) -> None: - joint = "finger_joint" - profile = CalibrationProfile( - key=ProfileKey("VIRTUAL", "right", "one_joint"), - namespace="/virtual/right", - command=CommandLayout( - names=("finger",), - baseline_u8=(255,), - command_index_by_joint={joint: 0}, - urdf_joint_by_joint={joint: joint}, - ), - vision=VisionRigSpec( - views=(ViewSpec("front", ( - TagSpec("base", 0, True), TagSpec("finger", 1), - )),), - common_frame="front", - extrinsic_reference_view="front", - ), - motion=MotionPolicy(tasks=( - TaskSpec( - "finger_sweep", "front", 0, (joint,), - start_u8=255, end_u8=0, formal_speed_u8=40, - ), - )), - measurement=MeasurementPolicy(measurements={ - joint: MeasurementSpec( - joint, "rotation", "front", "base", "finger" - ), - }), - zero=ZeroSolvePolicy( - active_joints=frozenset({joint}), - passive_joints=frozenset(), - direct_zero_joints=(joint,), - axis_joints=(joint,), - mechanical_endpoint_joints=frozenset(), - post_solve_endpoint_joints=frozenset(), - mimic_source_by_joint={}, - cad_frozen_joints=frozenset(), - ), - quality=QualityPolicy( - (0, 1, 2), 3, frozenset({"holdout"}), isolated_holdout=True - ), - scope=ScopePolicy( - calibrate_joints={"full": frozenset({joint})}, - frozen_joints={"full": frozenset()}, - ), - artifacts=ArtifactPolicy( - 1, "virtual.json", "virtual.urdf", frozenset(), - publish_corrected_urdf=True, - ), - ) - engine = CalibrationEngine(profile) - adapter = ProfileSdkAdapter(profile.command) - assert len(engine.scan_units()) == 8 - assert adapter.parse_feedback(("finger",), (127.0,)) == (127.0,) - - source = tmp_path / "virtual_raw.urdf" - source.write_text( - '' - '' - '' - '' - '' - '', - encoding="utf-8", - ) - output = write_urdf_patches( - source_urdf=source, - destination_urdf=tmp_path / "virtual.urdf", - patches=UrdfPatchSet({ - joint: UrdfJointPatch(origin_rpy="0 0 0.01") - }), - ) - corrected = output.read_text(encoding="utf-8") - assert 'rpy="0 0 0.01"' in corrected - assert '' in corrected diff --git a/src/linkerhand_calibration/test/test_unified_launch.py b/src/linkerhand_calibration/test/test_unified_launch.py deleted file mode 100644 index f5c9b52..0000000 --- a/src/linkerhand_calibration/test/test_unified_launch.py +++ /dev/null @@ -1,181 +0,0 @@ -"""Build real launch actions without executing a process or opening hardware.""" - -import hashlib -import importlib.util -from pathlib import Path - -import pytest -import yaml -from launch import LaunchContext -from launch.actions import DeclareLaunchArgument, ExecuteProcess -from launch.utilities import perform_substitutions -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.utilities import evaluate_parameters, to_parameters_list -from ros2launch.api.api import parse_launch_arguments - -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.runtime.runner_support import launch_command - - -PACKAGE = Path(__file__).resolve().parents[1] - - -@pytest.fixture(autouse=True) -def isolated_launch_logs(tmp_path, monkeypatch): - monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path / "ros_logs")) - - -def launch_module(): - spec = importlib.util.spec_from_file_location("test_calibration_launch", - PACKAGE / "launch/three_camera_calibration.launch.py") - module = importlib.util.module_from_spec(spec) - spec.loader.exec_module(module) - return module - - -@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) -@pytest.mark.parametrize("renamed_views", [False, True]) -def test_actual_launch_builds_from_protected_profile(tmp_path, model, renamed_views): - config = load_product_config(PACKAGE / f"config/{model}_right_product.yaml", - workspace=PACKAGE.parents[1], check_can=False) - arguments = dict(parse_launch_arguments(launch_command(config, tmp_path / "session", - record_bag=False, commands_enabled=False)[4:])) - if renamed_views: - payload = yaml.safe_load(config.profile_config.read_text()) - rename = {view.name: f"inspection_{i}" for i, view in enumerate(config.calibration_contract.typed_profile.vision.views)} - for view in payload["vision"]["views"]: - view["name"] = rename[view["name"]] - payload["vision"]["extrinsic_reference_view"] = rename[payload["vision"]["extrinsic_reference_view"]] - for task in payload["motion"]["tasks"]: - task["view"] = rename[task["view"]] - for measurement in payload["measurement"]["measurements"].values(): - measurement["view"] = rename[measurement["view"]] - path = tmp_path / "renamed.yaml" - path.write_text(yaml.safe_dump(payload)) - arguments["profile_config"] = str(path) - arguments["profile_config_expected_sha256"] = hashlib.sha256(path.read_bytes()).hexdigest() - for old, new in rename.items(): - for suffix in ("camera_serial", "camera_name", "camera_info_url"): - arguments[f"{new}_{suffix}"] = arguments.pop(f"{old}_{suffix}") - context = LaunchContext() - context.launch_configurations.update(arguments) - module = launch_module() - for action in module.generate_launch_description().entities: - if isinstance(action, DeclareLaunchArgument): - action.execute(context) - actions = module._launch_stack(context) - # Cameras, detector container, border filter, SDK and calibration owner. - assert sum(isinstance(action, Node) for action in actions) == 7 - assert not (tmp_path / "session/raw_samples.jsonl").exists() - - -def test_launch_rejects_missing_profile_before_creating_processes(): - context = LaunchContext() - context.launch_configurations.update(model="O6", hand_type="right", tag_layout="o6_right_8") - module = launch_module() - for action in module.generate_launch_description().entities: - if isinstance(action, DeclareLaunchArgument): - action.execute(context) - with pytest.raises(RuntimeError, match="protected YAML Profile"): - module._launch_stack(context) - - -@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) -def test_detector_receives_protected_model_settings(tmp_path, monkeypatch, model): - """Resolve the actual component parameters, including YAML/override order.""" - config = load_product_config(PACKAGE / f"config/{model}_right_product.yaml", - workspace=PACKAGE.parents[1], check_can=False) - context = LaunchContext() - context.launch_configurations.update(dict(parse_launch_arguments( - launch_command(config, tmp_path / "session", record_bag=False, - commands_enabled=False)[4:]))) - module = launch_module() - for action in module.generate_launch_description().entities: - if isinstance(action, DeclareLaunchArgument): - action.execute(context) - components = [] - - def capture_container(**kwargs): - components.extend(kwargs["composable_node_descriptions"]) - return ComposableNodeContainer(**kwargs) - - monkeypatch.setattr(module, "ComposableNodeContainer", capture_container) - module._launch_stack(context) - protected = yaml.safe_load(config.tag_config.read_text()) - checked = 0 - for component in components: - if perform_substitutions(context, component.node_plugin) != "AprilTagNode": - continue - name = perform_substitutions(context, component.node_name) - namespace = perform_substitutions(context, component.node_namespace) - effective = {parameter.name: parameter.value for parameter in to_parameters_list( - context, name, namespace, evaluate_parameters(context, component.parameters))} - expected = protected[f"{namespace}/{name}"]["ros__parameters"] - assert effective["detector.decimate"] == expected["detector"]["decimate"] - assert tuple(effective["tag.ids"]) == tuple(expected["tag"]["ids"]) - assert tuple(effective["tag.sizes"]) == tuple(expected["tag"]["sizes"]) - checked += 1 - assert checked == len(config.calibration_contract.typed_profile.vision.views) - - -@pytest.mark.parametrize("renamed_views", [False, True]) -def test_thumb_launch_omits_unused_camera_and_all_its_transport(tmp_path, monkeypatch, renamed_views): - config = load_product_config(PACKAGE / "config/o6_right_product.yaml", - workspace=PACKAGE.parents[1], check_can=False) - namespace = config.calibration_contract.typed_profile.namespace - arguments = dict(parse_launch_arguments(launch_command(config, tmp_path / "session", - record_bag=True, commands_enabled=True, diagnostic_capture="o6_thumb")[4:])) - rename = {"front": "front", "side": "side", "top": "top"} - if renamed_views: - rename = {"front": "inspection_a", "side": "inspection_b", "top": "inspection_c"} - payload = yaml.safe_load(config.profile_config.read_text()) - for view in payload["vision"]["views"]: - view["name"] = rename[view["name"]] - payload["vision"]["extrinsic_reference_view"] = rename[payload["vision"]["extrinsic_reference_view"]] - for task in payload["motion"]["tasks"]: - task["view"] = rename[task["view"]] - for measurement in payload["measurement"]["measurements"].values(): - measurement["view"] = rename[measurement["view"]] - path = tmp_path / "renamed.yaml" - path.write_text(yaml.safe_dump(payload)) - arguments["profile_config"] = str(path) - arguments["profile_config_expected_sha256"] = hashlib.sha256(path.read_bytes()).hexdigest() - for old, new in rename.items(): - for suffix in ("camera_serial", "camera_name", "camera_info_url"): - arguments[f"{new}_{suffix}"] = arguments.pop(f"{old}_{suffix}") - - # No side-camera launch arguments are supplied. A disconnected unused - # camera must not produce a driver process that could fail this session. - for suffix in ("camera_serial", "camera_name", "camera_info_url"): - arguments.pop(f"{rename['side']}_{suffix}") - context = LaunchContext() - context.launch_configurations.update(arguments) - module = launch_module() - for action in module.generate_launch_description().entities: - if isinstance(action, DeclareLaunchArgument): - action.execute(context) - - components = [] - def capture_container(**kwargs): - components.extend(kwargs["composable_node_descriptions"]) - return ComposableNodeContainer(**kwargs) - monkeypatch.setattr(module, "ComposableNodeContainer", capture_container) - actions = module._launch_stack(context) - nodes = [action for action in actions if isinstance(action, Node)] - assert len(nodes) == 6 # Two cameras, vision, border filter, SDK, coordinator. - assert len(components) == 4 # Rectification and detection for each active view. - component_namespaces = {perform_substitutions(context, component.node_namespace) - for component in components} - assert component_namespaces == {f"{namespace}/{view}/{kind}" - for view in (rename["front"], rename["top"]) - for kind in ("camera", "apriltag")} - - bag, = [action for action in actions - if isinstance(action, ExecuteProcess) and not isinstance(action, Node)] - bag_command = [perform_substitutions(context, part) for part in bag.cmd] - assert all(f"/{rename['side']}/" not in argument for argument in bag_command) - for view in (rename["front"], rename["top"]): - assert f"{namespace}/{view}/camera/image_raw" in bag_command - assert f"{namespace}/{view}/camera/camera_info" in bag_command - assert f"{namespace}/{view}/apriltag/detections" in bag_command - assert not (tmp_path / "session/raw_samples.jsonl").exists() diff --git a/src/linkerhand_calibration/test/test_unified_ros_host.py b/src/linkerhand_calibration/test/test_unified_ros_host.py deleted file mode 100644 index b107624..0000000 --- a/src/linkerhand_calibration/test/test_unified_ros_host.py +++ /dev/null @@ -1,110 +0,0 @@ -"""Actual ROS host, isolated DDS domain; no SDK, camera or hand is launched.""" - -from pathlib import Path - -import numpy as np -import pytest - - -@pytest.mark.parametrize("model,diagnostic_capture", [ - ("l6", ""), ("o6", ""), ("g20", ""), ("o12", ""), ("o6", "o6_thumb"), -]) -def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, model, diagnostic_capture): - rclpy = pytest.importorskip("rclpy") - from sensor_msgs.msg import JointState - from std_srvs.srv import Trigger - from linkerhand_calibration.product import load_product_config - from linkerhand_calibration.runtime.runner_support import protected_inputs - from linkerhand_calibration.runtime.ros.calibration_node import UnifiedCalibrationNode - from linkerhand_calibration.runtime.session import CalibrationPhase as Phase - package = Path(__file__).resolve().parents[1] - config = load_product_config(package / f"config/{model}_right_product.yaml", workspace=package.parents[1], check_can=False) - profile = config.calibration_contract.typed_profile - params = {"serial_number": "OFFLINE_HOST", "session_dir": str(tmp_path / model), - "source_urdf_path": str(config.source_urdf), "camera_extrinsics_file": str(config.camera_extrinsics), - **{key.replace("_sha256", "_expected_sha256"): value for key, value in protected_inputs(config).items()}} - if diagnostic_capture: - params["diagnostic_capture"] = diagnostic_capture - args = ["--ros-args", "--params-file", str(config.calibration_config)] - for key, value in params.items(): - args += ["-p", f"{key}:={value}"] - monkeypatch.setenv("ROS_DOMAIN_ID", "221") - monkeypatch.setenv("ROS_LOCALHOST_ONLY", "1") - monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path / "ros_logs")) - rclpy.init(args=args) - node = None - try: - node = UnifiedCalibrationNode(profile) - host = node.coordinator - views = ("front", "top") if diagnostic_capture else profile.vision.view_names - assert all(subscription.callback_group is node.default_callback_group - for subscription in node.subscriptions) - subscriptions = {subscription.topic_name for subscription in node.subscriptions} - expected_state_topic = host.parameters.state_topic + ("_timed" if profile.sdk_adapter == "legacy_byte_sdk" else "") - assert expected_state_topic in subscriptions - if profile.sdk_adapter == "legacy_byte_sdk": - assert host.parameters.state_topic not in subscriptions - for view in profile.vision.view_names: - assert (f"{profile.namespace}/{view}/camera/camera_info" in subscriptions) == (view in views) - assert f"{profile.namespace}/{view}/camera/image_rect" not in subscriptions - assert (f"{profile.namespace}/{view}/apriltag/detections_checked" in subscriptions) == (view in views) - assert host.parameters.tracking.reprojection_tie_px == 0.03 - assert all(tracker.reprojection_tie_px == 0.03 for tracker in host.capture.trackers.values()) - if profile.sdk_adapter == "o12_hcan_sdk": - from linkerhand_calibration.runtime.adapters import HardwareHealth - monkeypatch.setattr(host.sdk_adapter, "_health", lambda: HardwareHealth(True, True)) - writes = [] - monkeypatch.setattr(node.io.command_publisher, "publish", writes.append) - message = JointState() - message.position = list(map(float, profile.command.baseline_values)) - message.name = list(profile.command.names) - message.header.stamp = node.get_clock().now().to_msg() - def feed_message(): - if profile.sdk_adapter == "legacy_byte_sdk": - import json - from std_msgs.msg import String - stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec - timed = String() - timed.data = json.dumps(dict(schema="can_feedback_v1", name=list(message.name), - position=list(message.position), stamp_ns=stamp, - channel_stamps_ns=[stamp]*len(message.position))) - node._timed_state_callback(timed) - else: - node._state_callback(message) - feed_message() - host.cameras.matrices = {view: np.eye(3) for view in views} - import time - host.cameras.info_received_at = {view: time.monotonic() for view in views} - host.cameras.detections_received_at = dict(host.cameras.info_received_at) - node._tick() - assert host.execution.session.phase == Phase.READY - assert not writes # Preview never sends an open/baseline command. - assert host.snapshot().as_dict()["overall_progress_01"] == 0 - preview_capture = host.capture - preview_reference = host.reference_lock - # A once-ready camera going silent must revoke readiness, including - # when Start arrives before the next control timer callback. - host.cameras.detections_received_at.clear() - response = node._start(Trigger.Request(), Trigger.Response()) - assert not response.success - assert host.execution.session.phase == Phase.WAIT_DEVICE - assert not writes - assert host.capture is preview_capture - host.cameras.detections_received_at = {view: time.monotonic() for view in views} - node._tick() - response = node._start(Trigger.Request(), Trigger.Response()) - assert response.success - assert host.capture is not preview_capture - assert host.reference_lock is not preview_reference - assert all(tracker.reprojection_tie_px == 0.03 for tracker in host.capture.trackers.values()) - assert not host.state_history - node._tick() - assert not writes # Still waits for a fresh, post-Start feedback frame. - message.header.stamp = node.get_clock().now().to_msg() - feed_message() - node._tick() - assert host.execution.session.phase == Phase.BASELINE - finally: - if node is not None: - node.destroy_node() - rclpy.shutdown() diff --git a/src/linkerhand_calibration/test/test_unified_runner.py b/src/linkerhand_calibration/test/test_unified_runner.py deleted file mode 100644 index 28e1fc0..0000000 --- a/src/linkerhand_calibration/test/test_unified_runner.py +++ /dev/null @@ -1,443 +0,0 @@ -from __future__ import annotations - -import json -from dataclasses import replace -from pathlib import Path -from types import SimpleNamespace - -import pytest - -from linkerhand_calibration.profiles import load_hand_profile -from linkerhand_calibration.core.geometry.pnp import POSE_TRACKING_POLICY_VERSION -from linkerhand_calibration.product import load_product_config -from linkerhand_calibration.runtime import runner -from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION -from linkerhand_calibration.runtime.resume import discover_resume_candidate -from linkerhand_calibration.runtime.status import normalize_status, render_status_zh - - -PACKAGE = Path(__file__).resolve().parents[1] -WORKSPACE = PACKAGE.parents[1] - - -class LifecycleHarness: - def __init__(self, messages, *, step=1.0, publishers=1, response=True): - self.messages = iter(messages) - self.now = 0.0 - self.step = step - self.publisher_count = publishers - self.status = {} - self.last_status_at = 0.0 - self.status_topic = "/virtual/status" - self.start_client = SimpleNamespace(service_is_ready=lambda: True) - self.returncode = None - self.starts = 0 - self.response = response - - def spin(self): - self.now += self.step - value = next(self.messages) - if value is not None: - self.status = value - self.last_status_at = self.now - - def count_publishers(self, _topic): - return self.publisher_count - - def poll(self): - return self.returncode - - def request(self): - self.starts += 1 - return SimpleNamespace(done=lambda: True, - result=lambda: SimpleNamespace(success=self.response, message="fixture response")) - - def run(self, **options): - return runner.drive_session(self, self, spin=self.spin, ok=lambda: True, - request_start=self.request, clock=lambda: self.now, **options) - - -@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) -def test_actual_cli_uses_common_runner_not_model_dispatch(monkeypatch, model): - calls = [] - monkeypatch.setattr(runner, "run_online", lambda config, **kw: calls.append((config, kw)) or 0) - # Failure of any old Python CLI strategy must not affect the real entry. - import linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry as registry - monkeypatch.setattr(registry, "get_default_registry", lambda: pytest.fail("model runner dispatch")) - with pytest.raises(SystemExit) as error: - runner.main(["--config", str(PACKAGE / "config" / f"{model}_right_product.yaml"), - "--workspace", str(WORKSPACE), "--commands-disabled", "--no-resume"]) - assert error.value.code == 0 - assert calls[0][0].model == model.upper() - assert calls[0][1] == dict(record_bag=False, commands_enabled=False, allow_resume=False) - - -@pytest.mark.parametrize("ready,terminal", [("WAIT_START", "COMPLETE"), ("READY", "PASSED"), ("READY", "COMPLETE")]) -def test_ready_sent_once_even_when_node_repeats_ready(ready, terminal): - harness = LifecycleHarness([{"state": ready}, {"state": ready}, {"state": "SWEEP"}, {"state": terminal}]) - assert normalize_status(harness.run())["state"] == "COMPLETE" - assert harness.starts == 1 - - -def test_reference_wait_and_slow_fit_do_not_become_false_communication_faults(): - harness = LifecycleHarness([ - {"state": "READY"}, {"state": "REFERENCE_LOCKING"}, None, None, - {"state": "FITTING"}, None, None, {"state": "COMPLETE"}, - ], step=300.0) - assert harness.run()["state"] == "COMPLETE" - assert harness.starts == 1 - - -def test_missing_status_owner_is_not_confused_with_slow_status(): - harness = LifecycleHarness([{"state": "READY"}, None], step=20.0, publishers=0) - status = harness.run() - assert status["state"] == "FAILED" - assert status["reason"] == "calibration_node_process_exited" - - -def test_rejected_start_preserves_exact_reason(): - harness = LifecycleHarness([{"state": "READY"}], response=False) - assert harness.run()["reason"] == "calibration_start_rejected:fixture response" - assert harness.starts == 1 - - -def test_preview_never_calls_start(): - harness = LifecycleHarness([{"state": "READY"}, {"state": "READY"}, {"state": "ABORTED"}]) - assert harness.run(auto_start=False)["state"] == "ABORTED" - assert harness.starts == 0 - - -def test_start_service_response_has_its_own_deadline(): - harness = LifecycleHarness([{"state": "READY"}, None], step=20.0) - harness.request = lambda: SimpleNamespace(done=lambda: False) - assert harness.run()["reason"] == "calibration_start_response_timeout" - - -def test_explicit_startup_wait_allows_slow_checkpoint_then_starts_once(): - messages = [None, None, {"state": "READY"}, {"state": "COMPLETE"}] - default = LifecycleHarness(messages, step=65.0) - assert default.run()["reason"] == "calibration_node_initial_status_timeout" - assert default.starts == 0 - extended = LifecycleHarness(messages, step=65.0) - assert extended.run(startup_timeout=600.0)["state"] == "COMPLETE" - assert extended.starts == 1 - - -def test_explicit_startup_wait_preserves_start_service_deadline(): - harness = LifecycleHarness([None, {"state": "READY"}, None], step=65.0) - harness.request = lambda: SimpleNamespace(done=lambda: False) - assert harness.run(startup_timeout=600.0)["reason"] == "calibration_start_response_timeout" - - -def test_cli_passes_explicit_startup_wait_to_online_runner(monkeypatch): - calls = [] - monkeypatch.setattr(runner, "run_online", lambda config, **kw: calls.append(kw) or 0) - with pytest.raises(SystemExit) as error: - runner.main(["--config", str(PACKAGE / "config/g20_right_product.yaml"), - "--workspace", str(WORKSPACE), "--commands-disabled", - "--startup-timeout-seconds", "600"]) - assert error.value.code == 0 - assert calls[0]["startup_timeout"] == 600.0 - - -@pytest.mark.parametrize("value", ["0", "-1", "nan", "inf"]) -def test_cli_rejects_invalid_startup_wait_before_loading_devices(monkeypatch, value): - monkeypatch.setattr(runner, "load_product_config", lambda *a, **kw: pytest.fail("loaded devices")) - with pytest.raises(SystemExit) as error: - runner.main(["--startup-timeout-seconds", value]) - assert error.value.code == 2 - - -def test_progress_uses_profile_tags_including_secondary_view(): - profile = load_hand_profile(PACKAGE / "config/profiles/o12_right_16.yaml") - status = normalize_status({"state": "RUNNING", "task_name": "middle_roll_front", - "phase": "sweep", "cycle": 0, "attempt": 1, - "reference_locked": True}, profile=profile) - assert status["task"]["cycle"] == 1 - assert status["task"]["required_tag_ids_by_view"] == {"front": [0, 12], "side": [4, 8]} - text = render_status_zh(status) - assert "ID0/ID12" in text and "ID4/ID8" in text - assert "请勿移动" in text - - -def test_empty_preview_fields_do_not_break_status_rendering(): - status = normalize_status({"state": "WAIT_DEVICES", "attempt": None, - "cycle": None, "step_index": None}) - assert "等待设备" in render_status_zh(status) - - -def test_unified_progress_keeps_sdk_fault_diagnostics(): - text = render_status_zh(normalize_status({"state": "PAUSED", - "reason": "o12_active_motor_fault:channel=1", "error_faults": ["motor_1:overcurrent"]})) - assert "活动电机故障" in text - assert "motor_1:overcurrent" in text - assert "未分类" not in text - - -def _journal(path, *, version=ACQUISITION_POLICY_VERSION, digest="a" * 64, complete=True): - path.mkdir() - rows = [{"kind": "session_start", "profile_id": "VIRTUAL/right/layout/v1", - "serial_number": "TEST_001", "acquisition_policy_version": version, - "capture_schedule_version": CAPTURE_SCHEDULE_VERSION, - "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, - "protected_hashes": {"source": digest}}, - {"kind": "fixed_base_reference_locked"}] - if complete: - rows.append({"kind": "scan_unit_complete", "passed": True}) - (path / "raw_samples.jsonl").write_text("\n".join(json.dumps(row) for row in rows)) - - -def test_discovery_keeps_prior_evidence_and_ignores_new_empty_incompatible_sessions(tmp_path): - _journal(tmp_path / "01") - _journal(tmp_path / "02", complete=False) - _journal(tmp_path / "03", digest="b" * 64) - _journal(tmp_path / "04", version="unified_engine_v1") - _journal(tmp_path / "05", version="unified_engine_v2") - before = (tmp_path / "01/raw_samples.jsonl").read_bytes() - found = discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", - serial_number="TEST_001", protected_hashes={"source": "a" * 64}) - assert found == tmp_path / "01" - assert found.joinpath("raw_samples.jsonl").read_bytes() == before - - -def test_discovery_never_reuses_released_session_or_external_symlink(tmp_path): - _journal(tmp_path / "01") - (tmp_path / "latest_passed").symlink_to(tmp_path / "01", target_is_directory=True) - (tmp_path / "99").symlink_to(tmp_path / "01", target_is_directory=True) - assert discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", - serial_number="TEST_001", protected_hashes={"source": "a" * 64}) is None - - -def test_passed_capture_cannot_be_reused_to_fake_an_independent_session(tmp_path): - _journal(tmp_path / "01") - (tmp_path / "01/calibration_summary_zh.json").write_text(json.dumps({"result": "PASS"})) - assert discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", - serial_number="TEST_001", protected_hashes={"source": "a" * 64}) is None - - -@pytest.mark.parametrize("fresh_restart", [False, True]) -def test_partial_resume_keeps_more_complete_source_but_respects_explicit_restart(tmp_path, fresh_restart): - for name, count, resumed in (("01", 24, False), ("02", 8, not fresh_restart), ("03", 8, True)): - path = tmp_path / name - _journal(path, complete=False) - journal = path / "raw_samples.jsonl" - rows = [json.loads(line) for line in journal.read_text().splitlines()] - rows[0]["resume_checkpoint_requested"] = resumed - rows.extend({"kind": "scan_unit_complete", "task_name": f"task_{i}", - "cycle": 0, "direction": "increasing", "attempt": 1, "passed": True} - for i in range(count)) - # Repeated completion records must not inflate coverage. - if name == "03": - rows.extend([rows[-1]] * 30) - journal.write_text("\n".join(json.dumps(row) for row in rows)) - found = discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", - serial_number="TEST_001", protected_hashes={"source": "a" * 64}) - assert found == tmp_path / ("03" if fresh_restart else "01") - - -@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) -@pytest.mark.parametrize("terminal", ["COMPLETE", "PAUSED", "INTERRUPTED", "ABORT_TIMEOUT"]) -def test_common_online_process_chain_with_virtual_ros_and_no_hardware(monkeypatch, tmp_path, model, terminal): - """Use run_online itself, not just its selector or a schedule list.""" - import rclpy - from rclpy.signals import SignalHandlerOptions - from linkerhand_calibration.runtime import runner_support - from linkerhand_calibration.runtime.artifacts import completion as online_artifacts - import linkerhand_calibration.hikrobot_camera as camera - - config = load_product_config(PACKAGE / "config" / f"{model}_right_product.yaml", - workspace=WORKSPACE, check_can=False) - config = replace(config, output_root=tmp_path) - harness = LifecycleHarness([ - {"state": "READY"}, {"state": "REFERENCE_LOCKING"}, - {"state": "SWEEP"}, {"state": terminal, "reason": "synthetic_terminal"}, - ]) - harness.abort_client = SimpleNamespace(service_is_ready=lambda: True) - harness.start_client.call_async = lambda _request: harness.request() - harness.destroy_node = lambda: calls.append("destroy_monitor") - calls = [] - launched = [] - ros_active = [False] - abort_pending = [False] - abort_acknowledged = [False] - def abort(_request): - assert ros_active[0] - abort_pending[0] = True - calls.append("request_abort") - return SimpleNamespace(done=lambda: abort_acknowledged[0]) - harness.abort_client.call_async = abort - def init(*, signal_handler_options): - assert signal_handler_options == SignalHandlerOptions.SIGTERM - ros_active[0] = True - def shutdown(): - ros_active[0] = False - calls.append("shutdown") - def popen(command, **kwargs): - launched.append((command, kwargs)) - return harness - def spin(_node, **_kwargs): - if abort_pending[0]: - harness.now += 0.1 - if terminal != "ABORT_TIMEOUT": - abort_acknowledged[0] = True - calls.append("abort_acknowledged") - elif launched: - harness.spin() - if harness.status["state"] in {"INTERRUPTED", "ABORT_TIMEOUT"}: - raise KeyboardInterrupt - else: - harness.now += 0.5 - harness.count_publishers = lambda _topic: 1 if launched else 0 - monkeypatch.setattr(rclpy, "init", init) - monkeypatch.setattr(rclpy, "shutdown", shutdown) - monkeypatch.setattr(rclpy, "ok", lambda: ros_active[0]) - monkeypatch.setattr(rclpy, "spin_once", spin) - monkeypatch.setattr(runner.time, "monotonic", lambda: harness.now) - monkeypatch.setattr(runner.subprocess, "Popen", popen) - monkeypatch.setattr(runner_support, "CalibrationMonitor", lambda **kw: harness) - monkeypatch.setattr(runner_support, "stop_stack", lambda process: calls.append("stop_owned_stack")) - monkeypatch.setattr(runner_support, "overlay_environment", lambda path: {"TEST_OVERLAY": str(path)}) - monkeypatch.setattr(camera, "configure_fastdds_large_image_transport", lambda: None) - def finish(product, session, status): - assert product == config and status["state"] == "COMPLETE" - assert (session / "node_status.json").is_file() - calls.append("artifact_validation") - return {"JSON": "virtual_only"} - monkeypatch.setattr(online_artifacts, "finish_online_artifacts", finish) - - code = runner.run_online(config, allow_resume=False) - assert code == {"COMPLETE": 0, "PAUSED": 3, - "INTERRUPTED": 130, "ABORT_TIMEOUT": 130}[terminal] - assert harness.starts == 1 - assert ("artifact_validation" in calls) == (terminal == "COMPLETE") - assert calls[-3:] == ["stop_owned_stack", "destroy_monitor", "shutdown"] - if terminal == "INTERRUPTED": - assert calls[:2] == ["request_abort", "abort_acknowledged"] - elif terminal == "ABORT_TIMEOUT": - assert calls[0] == "request_abort" - assert "abort_acknowledged" not in calls - assert len(launched) == 1 - command, kwargs = launched[0] - assert "unified_calibration.launch.py" in command - assert f"profile_config:={config.profile_config}" in command - assert f"profile_config_expected_sha256:={config.profile_config_sha256}" in command - assert kwargs["start_new_session"] is True - if config.sdk_setup: - assert kwargs["env"]["TEST_OVERLAY"] == str(config.sdk_setup) - - -def test_session_directory_collision_never_sleeps_or_overwrites(monkeypatch, tmp_path): - from linkerhand_calibration.runtime import runner_support - monkeypatch.setattr(runner_support.time, "sleep", lambda _: pytest.fail("blocking collision sleep")) - first = runner_support.create_session_directory(tmp_path) - (first / "keep.txt").write_text("user evidence") - second = runner_support.create_session_directory(tmp_path) - assert first != second - assert (first / "keep.txt").read_text() == "user evidence" - - -@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) -def test_launch_arguments_parse_with_ros_cli_and_preserve_sdk_config(model, tmp_path): - from ros2launch.api.api import parse_launch_arguments - from linkerhand_calibration.runtime.runner_support import launch_command - - config = load_product_config(PACKAGE / "config" / f"{model}_right_product.yaml", - workspace=WORKSPACE, check_can=False) - session = tmp_path / "session with spaces" - command = launch_command(config, session, record_bag=False, commands_enabled=False) - arguments = dict(parse_launch_arguments(command[4:])) - assert arguments["session_dir"] == str(session) - assert arguments["commands_enabled"] == "false" - for name, value in { - "vendor_sdk_config": config.sdk_config, - "sdk_config_expected_sha256": config.sdk_config_sha256, - "vendor_sdk_python_package": config.sdk_python_package, - "sdk_package_expected_sha256": config.sdk_package_sha256, - }.items(): - if value: - assert arguments[name] == str(value) - else: - assert name not in arguments - - -def test_runner_does_not_override_reviewed_speed_defaults(): - from linkerhand_calibration.runtime.runner_support import launch_command - for model in ("g20", "o6", "l6", "o12"): - config = load_product_config(PACKAGE / "config" / f"{model}_right_product.yaml", - workspace=WORKSPACE, check_can=False) - command = launch_command(config, Path("/tmp/virtual_session"), record_bag=False, commands_enabled=False) - speed = config.calibration_contract.typed_profile.motion.speed_parameters.get("baseline_u8") - if speed is not None: - assert f"calibration_speed:={int(speed)}" in command - else: - assert not any(arg.startswith("calibration_speed:=") for arg in command) - assert "index_roll_calibration_speed:=1" not in command - assert "index_flex_calibration_speed:=1" not in command - - -@pytest.mark.parametrize("model,layout", [("g20", "g20_right_19"), ("l6", "l6_right_8"), - ("o6", "o6_right_8"), ("o12", "o12_right_16")]) -def test_node_profile_handoff_checks_actual_yaml_and_hash(monkeypatch, model, layout): - import hashlib - from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry - from linkerhand_calibration.runtime.ros import entrypoint as calibration - path = PACKAGE / "config/profiles" / f"{layout}.yaml" - profile = load_hand_profile(path) - registered = get_default_registry().get(profile.key) - assert registered.profile == profile - forwarded = [] - monkeypatch.setattr(calibration, "run_profile_node", lambda declared, args: forwarded.append(args)) - assert not hasattr(calibration, "get_default_registry") - args = ["--profile-id", profile.key.profile_id, "--profile-config", str(path)] - with pytest.raises(ValueError, match="Profile changed"): - calibration.main(args + ["--profile-sha256", "0" * 64]) - assert not forwarded - calibration.main(args + ["--profile-sha256", hashlib.sha256(path.read_bytes()).hexdigest(), "--ros-args"]) - assert forwarded == [["--ros-args"]] - - -def test_owned_child_group_receives_shutdown_even_if_launch_parent_exited(monkeypatch): - from linkerhand_calibration.runtime import runner_support - import signal - signals = [] - process = SimpleNamespace(pid=12345, poll=lambda: 2, wait=lambda timeout: 2) - monkeypatch.setattr(runner_support.os, "killpg", lambda pid, sig: signals.append((pid, sig))) - runner_support.stop_stack(process) - assert signals == [(12345, signal.SIGINT)] - - -def test_native_profile_node_handoff_does_not_require_a_model_registration(tmp_path, monkeypatch): - import hashlib - import yaml - from linkerhand_calibration.runtime.ros import entrypoint as calibration - data = yaml.safe_load((PACKAGE / "config/profiles/l6_right_8.yaml").read_text()) - data.update(profile_id="VIRTUAL/right/no_python_model/v1", namespace="/virtual_calibration") - path = tmp_path / "new_profile.yaml" - path.write_text(yaml.safe_dump(data)) - def forbidden(): - raise AssertionError("new Profile must not look up a Python model") - import linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry as registry - monkeypatch.setattr(registry, "get_default_registry", forbidden) - calls = [] - monkeypatch.setattr(calibration, "run_profile_node", lambda profile, args: calls.append((profile, args))) - calibration.main(["--profile-id", data["profile_id"], "--profile-config", str(path), - "--profile-sha256", hashlib.sha256(path.read_bytes()).hexdigest(), "--ros-args"]) - assert calls[0][0].key.model == "VIRTUAL" - assert calls[0][1] == ["--ros-args"] - - -@pytest.mark.parametrize("model", ["o6", "o12"]) -def test_abort_never_commands_baseline_return_for_byte_or_rad(tmp_path, model): - from runtime_host_fixture import coordinator_fixture - host, clock = coordinator_fixture(tmp_path, model=model) - try: - safe = tuple((lo+hi)/2 for lo, hi in zip(host.profile.command.minimum_values, - host.profile.command.maximum_values)) - if host.profile.command.unit == "u8": - safe = tuple(float(round(value)) for value in safe) - host._publish_command(safe) - response = host.abort() - assert host.state == "ABORTED" and response.success - assert clock.positions == [safe, safe] - finally: - host.close() diff --git a/src/linkerhand_calibration/test/test_urdf_command_images.py b/src/linkerhand_calibration/test/test_urdf_command_images.py deleted file mode 100644 index ee0d008..0000000 --- a/src/linkerhand_calibration/test/test_urdf_command_images.py +++ /dev/null @@ -1,129 +0,0 @@ -"""Independent elementary-FK truth for the original-URDF image graph.""" -from types import SimpleNamespace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from o30_capture_fixture import synthetic_capture, rigid -from linkerhand_calibration.core.domain.capture_plan import configure_training, RAW_JOINT -from linkerhand_calibration.core.geometry.extrinsics import transform_matrix -from linkerhand_calibration.core.geometry.tag_pose.ippe import square_object_points -from linkerhand_calibration.core.geometry.tag_pose.urdf_command_images import UrdfCommandImages, pose_vector -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.profiles.observations import compile_tag_links - - -@pytest.fixture(scope='module') -def graph(): - profile, source, samples, offsets = synthetic_capture(include_same_finger_tags=True) - profile = configure_training(profile, RAW_JOINT) - model = UrdfKinematicModel(source) - matrix = np.array([[1400., 0., 800.], [0., 1400., 600.], [0., 0., 1.]]) - tags = {t.role: t for v in profile.vision.views for t in v.tags} - rows, selected = {}, set() - for sample in samples: - if sample.get('sample_phase') != 'steady' or sample['cycle'] not in (0, 1): - continue - key = (sample['task_name'], sample['cycle'], sample['direction'], - sample.get('segment_key', ''), sample['steady_index'], sample['joint']) - if key in selected: - continue - selected.add(key) - identity = sample['view'], sample['image_stamp_ns'] - row = rows.setdefault(identity, dict(kind='raw_observation_frame', - task_name=sample['task_name'], cycle=sample['cycle'], direction=sample['direction'], - view=sample['view'], image_stamp_ns=sample['image_stamp_ns'], sample_phase='steady', - accepted_sample_window_open=True, camera_matrix=matrix.tolist(), - command_vector=sample['command_vector_u8'], - command_direction_by_index=sample['command_direction_by_index'], tags={})) - for role, payload in sample['same_finger_poses_common'].items(): - pose = transform_matrix(**payload) - points = square_object_points(tags[role].size_m) @ pose[:3, :3].T + pose[:3, 3] - pixels = points @ matrix.T - row['tags'][role] = dict(corners_xy=(pixels[:, :2]/pixels[:, 2:]).tolist()) - bundle = UrdfCommandImages(profile, model, SimpleNamespace(transform=lambda view: np.eye(4)), - tuple(rows.values()), compile_tag_links(profile, model)) - truth = np.zeros(len(bundle.scale)) - truth[:6] = pose_vector(rigid((.11, -.08, .17), (.11, -.04, .72))) - for name, column in bundle.zero_columns.items(): - truth[column] = offsets[name] - for name, columns in bundle.curve_columns.items(): - channel = bundle.channels[name] - x, z = bundle.knots[name]/255, profile.command.baseline_values[channel]/255 - for direction in range(2): - values = ((1.0+.007*channel)*(x-z)+.09*(x*x-z*z) - +(1 if direction == 0 else -1)*.006*(x-z)*(1-x)) - values *= profile.command.joint_directions[channel] - bundle.set_initial_curve(truth, name, direction, values) - for column in bundle.mount_columns.values(): - truth[column:column+6] = pose_vector(rigid((.12, -.09, .2), (.009, -.025, .016))) - return bundle, truth - - -def test_original_urdf_graph_matches_independent_fk_including_thumb(graph): - bundle, truth = graph - residual = bundle.residual(truth) - assert np.sqrt(np.mean(residual**2)) < .12 - assert set(bundle.zero_offsets(truth)) == set(bundle.joints) - assert all(bundle.zero_offsets(truth)[name] == 0 for name in bundle.profile.zero.cad_zero_assumptions) - assert len(bundle.output_mappings(truth)) == 20 - - -def test_wrong_zero_cannot_be_hidden_without_changing_frozen_model(graph): - bundle, truth = graph - wrong = truth.copy() - wrong[bundle.zero_columns['thumb_cmc_yaw']] += np.radians(5) - assert np.sqrt(np.mean(bundle.residual(wrong)**2)) > 1. - - -def test_training_constructor_rejects_holdout(graph): - bundle, _ = graph - rows = [*bundle.rows, {**bundle.rows[0], 'cycle': 3}] - with pytest.raises(ValueError, match='training_partition_invalid'): - UrdfCommandImages(bundle.profile, bundle.model, bundle.extrinsics, rows, bundle.tag_links) - - -def test_five_root_axes_initialize_palm_without_endpoint_zero(graph): - bundle, truth = graph - parameters = bundle.initialize((0,)*6) - rotation_error = Rotation.from_rotvec(parameters[:3])*Rotation.from_rotvec(truth[:3]).inv() - assert rotation_error.magnitude() < np.radians(5) - # The parallel root lines alone cannot locate translation along their - # common axis. That component must come from the remaining chain images. - axis = bundle.model.axis_line('thumb_cmc_roll', zero_offsets={}, joint_angles={})[0] - axis = Rotation.from_rotvec(truth[:3]).apply(axis) - delta = parameters[3:6]-truth[3:6] - assert np.linalg.norm(delta-axis*(delta@axis)) < .015 - - -def test_analytic_chain_jacobian_matches_independent_central_differences(graph): - bundle, truth = graph - jac = bundle.jacobian(truth) - # Every zero/curve/mount column, including nonparallel thumb derivatives. - for column, scale in enumerate(bundle.scale): - upper, lower = truth.copy(), truth.copy() - step = 1e-5*scale - upper[column] += step; lower[column] -= step - numeric = (bundle.residual(upper)-bundle.residual(lower))/(2*step) - assert np.allclose(jac[:, column], numeric, atol=2e-5, rtol=1e-5), column - - -def test_physical_covariance_derivatives_match_independent_finite_differences(graph): - from linkerhand_calibration.core.fitting.raw_joint_uncertainty import pose_derivatives - bundle, truth = graph - commands, directions = bundle.commands[::51], bundle.directions[::51] - poses, derivatives, frames = pose_derivatives(bundle, truth, commands, directions) - step = 1e-6 - columns = list(range(6))+list(bundle.zero_columns.values())+[ - int(c[c>=0][0]) for c in bundle.curve_columns.values()] - for column in columns: - a, b = truth.copy(), truth.copy() - a[column] += step; b[column] -= step - upper, lower = bundle.link_poses(a, commands, directions), bundle.link_poses(b, commands, directions) - for link, (position, angular) in derivatives.items(): - np.testing.assert_allclose(position[:,:,column], - (upper[link][:,:3,3]-lower[link][:,:3,3])/(2*step), atol=1e-8) - rotated = upper[link][:,:3,:3] @ lower[link][:,:3,:3].transpose(0,2,1) - np.testing.assert_allclose(angular[:,:,column], - Rotation.from_matrix(rotated).as_rotvec()/(2*step), atol=1e-8) diff --git a/src/linkerhand_calibration/test/test_urdf_comparison.py b/src/linkerhand_calibration/test/test_urdf_comparison.py deleted file mode 100644 index efb13ff..0000000 --- a/src/linkerhand_calibration/test/test_urdf_comparison.py +++ /dev/null @@ -1,59 +0,0 @@ -import math -import xml.etree.ElementTree as ET - -import numpy as np -import pytest - -from linkerhand_calibration.urdf_comparison import compare_urdfs, _Model, main - - -def model(tmp_path, name, zero=0.): - path = tmp_path / name - path.write_text(f''' - - - - - - ''') - return path - - -def test_identical_geometry_has_zero_pose_difference(tmp_path): - a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf") - result = compare_urdfs(a, b) - assert result["maximum_origin_distance_mm"] == pytest.approx(0.) - assert result["maximum_orientation_difference_deg"] == pytest.approx(0.) - assert not result["is_accuracy_certificate"] - assert result["pose_count"] == 4 - assert result["scope"] == "same_urdf_joint_angles_not_sdk_feedback" - - -def test_reports_actual_link_frame_shift_not_just_parameter_delta(tmp_path): - a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf", .1) - result = compare_urdfs(a, b) - assert result["maximum_origin_distance_mm"] == pytest.approx(200 * math.sin(.05)) - assert result["maximum_orientation_difference_deg"] == pytest.approx(math.degrees(.1)) - - -def test_mimic_source_on_other_branch_is_resolved_recursively(tmp_path): - path = model(tmp_path, "a.urdf") - root = ET.parse(path).getroot() - ET.SubElement(root, "link", name="follower_link") - j = ET.SubElement(root, "joint", name="follower", type="revolute") - ET.SubElement(j, "parent", link="base") - ET.SubElement(j, "child", link="follower_link") - ET.SubElement(j, "axis", xyz="0 0 1") - ET.SubElement(j, "mimic", joint="motor", multiplier="0.5", offset="0.1") - ET.ElementTree(root).write(path) - poses = _Model(path).fk({"motor": .4}) - assert poses["follower_link"][0, 0] == pytest.approx(math.cos(.3)) - assert np.isfinite(poses["follower_link"]).all() - - -def test_does_not_overwrite_existing_files(tmp_path): - a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf", .1) - before = a.read_bytes() - with pytest.raises(SystemExit): - main(["--reference", str(a), "--candidate", str(b), "--output", str(a)]) - assert a.read_bytes() == before diff --git a/src/linkerhand_calibration/test/test_urdf_patch_engine.py b/src/linkerhand_calibration/test/test_urdf_patch_engine.py deleted file mode 100644 index 6f3cc13..0000000 --- a/src/linkerhand_calibration/test/test_urdf_patch_engine.py +++ /dev/null @@ -1,199 +0,0 @@ -from __future__ import annotations - -from pathlib import Path -import xml.etree.ElementTree as ET - -import pytest - -from linkerhand_calibration.core.urdf import ( - MujocoEqualityPatch, - UrdfJointPatch, - UrdfPatchSet, - apply_urdf_patch_text, - validate_urdf_mimic_ranges, - write_urdf_patches, -) - - -SOURCE_TEXT = """ - - - - - - - - - - - - - - - - - - - - - - - -""" - - -def _patches() -> UrdfPatchSet: - return UrdfPatchSet( - joints={ - "active": UrdfJointPatch( - origin_rpy="0 0.2 0", - limit_lower="0", - limit_upper="1.2", - ), - "passive": UrdfJointPatch( - mimic_multiplier="0.9", - mimic_offset="0.01", - ), - }, - mujoco_equalities={ - "couple": MujocoEqualityPatch( - polycoef="0.01 1.1 -0.2 0 0 0", - expected_joint1="passive", - expected_joint2="active", - ) - }, - ) - - -def test_patch_engine_changes_only_declared_attributes() -> None: - corrected = apply_urdf_patch_text( - SOURCE_TEXT, ET.fromstring(SOURCE_TEXT), _patches() - ) - - expected = SOURCE_TEXT - expected = expected.replace('rpy="0 0 0"', 'rpy="0 0.2 0"', 1) - expected = expected.replace('lower="-1"', 'lower="0"', 1) - expected = expected.replace('upper="1"', 'upper="1.2"', 1) - expected = expected.replace('multiplier="1"', 'multiplier="0.9"', 1) - expected = expected.replace('offset="0"', 'offset="0.01"', 1) - expected = expected.replace( - 'polycoef="0 1 0 0 0 0"', - 'polycoef="0.01 1.1 -0.2 0 0 0"', - 1, - ) - assert corrected == expected - assert '' in corrected - - -def test_patch_engine_rejects_missing_fields_and_wrong_topology() -> None: - root = ET.fromstring(SOURCE_TEXT) - with pytest.raises(ValueError, match="has no multiplier"): - apply_urdf_patch_text( - SOURCE_TEXT, - root, - UrdfPatchSet( - joints={"active": UrdfJointPatch(mimic_multiplier="1.1")} - ), - ) - with pytest.raises(ValueError, match="joint2 differs"): - apply_urdf_patch_text( - SOURCE_TEXT, - root, - UrdfPatchSet( - joints={}, - mujoco_equalities={ - "couple": MujocoEqualityPatch( - polycoef="0 1 0 0 0 0", - expected_joint2="wrong", - ) - }, - ), - ) - - -def test_patch_engine_atomically_writes_and_refuses_overwrite( - tmp_path: Path, -) -> None: - source = tmp_path / "source.urdf" - source.write_text(SOURCE_TEXT, encoding="utf-8") - destination = tmp_path / "output/corrected.urdf" - - result = write_urdf_patches( - source_urdf=source, - destination_urdf=destination, - patches=_patches(), - ) - - assert result == destination.resolve() - assert result.is_file() - with pytest.raises(ValueError, match="refusing to overwrite"): - write_urdf_patches( - source_urdf=source, - destination_urdf=destination, - patches=_patches(), - ) - - -def test_patch_engine_copies_referenced_and_complete_mesh_bundle( - tmp_path: Path, -) -> None: - source_dir = tmp_path / "source" - meshes = source_dir / "meshes" - meshes.mkdir(parents=True) - (meshes / "used.stl").write_bytes(b"used") - (meshes / "auxiliary.stl").write_bytes(b"auxiliary") - text = SOURCE_TEXT.replace( - '', - '' - '' - '', - ) - source = source_dir / "source.urdf" - source.write_text(text, encoding="utf-8") - destination = tmp_path / "output/corrected.urdf" - - write_urdf_patches( - source_urdf=source, - destination_urdf=destination, - patches=_patches(), - copy_complete_mesh_directory=True, - ) - - assert (destination.parent / "meshes/used.stl").read_bytes() == b"used" - assert ( - destination.parent / "meshes/auxiliary.stl" - ).read_bytes() == b"auxiliary" - - -def test_mimic_range_validation_resolves_full_chain(tmp_path: Path) -> None: - text = """ - - - -""" - valid = tmp_path / "valid.urdf" - valid.write_text(text, encoding="utf-8") - ranges = validate_urdf_mimic_ranges(valid) - assert ranges["b"] == pytest.approx((0.0, 2.0)) - assert ranges["c"] == pytest.approx((0.0, 1.0)) - - invalid = tmp_path / "invalid.urdf" - invalid.write_text(text.replace('upper="1.1"', 'upper="0.9"'), encoding="utf-8") - with pytest.raises(ValueError, match="c=0.100000000rad"): - validate_urdf_mimic_ranges(invalid) - - -def test_mimic_range_validation_never_enlarges_cad_excess( - tmp_path: Path, -) -> None: - reference = tmp_path / "reference.urdf" - reference.write_text(SOURCE_TEXT, encoding="utf-8") - validate_urdf_mimic_ranges(reference, reference_urdf=reference) - - worsened = tmp_path / "worsened.urdf" - worsened.write_text( - SOURCE_TEXT.replace('multiplier="1"', 'multiplier="1.1"', 1), - encoding="utf-8", - ) - with pytest.raises(ValueError, match="passive"): - validate_urdf_mimic_ranges(worsened, reference_urdf=reference) diff --git a/src/linkerhand_calibration/test/test_urdf_validation.py b/src/linkerhand_calibration/test/test_urdf_validation.py deleted file mode 100644 index 5c88b08..0000000 --- a/src/linkerhand_calibration/test/test_urdf_validation.py +++ /dev/null @@ -1,53 +0,0 @@ -from __future__ import annotations - -from pathlib import Path - -import pytest - -from linkerhand_calibration.core.urdf import ( - validate_runtime_curve_limits, - validate_structural_urdf_diff, -) - - -SOURCE = """ - - - - -""" - - -def test_structural_diff_accepts_only_declared_joint_attribute(tmp_path: Path) -> None: - source = tmp_path / "source.urdf" - corrected = tmp_path / "corrected.urdf" - source.write_text(SOURCE, encoding="utf-8") - corrected.write_text(SOURCE.replace('rpy="0 0 0"', 'rpy="0 0 0.1"'), encoding="utf-8") - changes = validate_structural_urdf_diff( - source, corrected, authorized_fields={"finger": ("origin.rpy",)} - ) - assert changes == ("finger.origin.rpy",) - validate_runtime_curve_limits(corrected, {"finger": (0.0, 0.5, 1.0)}) - - -def test_structural_diff_rejects_mesh_inertial_or_undeclared_limit(tmp_path: Path) -> None: - source = tmp_path / "source.urdf" - source.write_text(SOURCE, encoding="utf-8") - for name, changed in { - "mesh": SOURCE.replace("meshes/tip.stl", "meshes/other.stl"), - "mass": SOURCE.replace('mass value="1"', 'mass value="2"'), - "limit": SOURCE.replace('upper="1"', 'upper="1.1"'), - }.items(): - output = tmp_path / f"{name}.urdf" - output.write_text(changed, encoding="utf-8") - with pytest.raises(ValueError, match="unauthorized"): - validate_structural_urdf_diff( - source, output, authorized_fields={"finger": ("origin.rpy",)} - ) - - -def test_runtime_curve_cannot_exceed_corrected_limit(tmp_path: Path) -> None: - urdf = tmp_path / "source.urdf" - urdf.write_text(SOURCE, encoding="utf-8") - with pytest.raises(ValueError, match="exceeds"): - validate_runtime_curve_limits(urdf, {"finger": (0.0, 1.01)}) diff --git a/src/linkerhand_calibration/test/test_urdf_zero.py b/src/linkerhand_calibration/test/test_urdf_zero.py deleted file mode 100644 index 1731717..0000000 --- a/src/linkerhand_calibration/test/test_urdf_zero.py +++ /dev/null @@ -1,2284 +0,0 @@ -import math -from pathlib import Path -import xml.etree.ElementTree as ET - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.extrinsics import ( - camera_info_fingerprint, - dump_three_camera_extrinsics, - load_three_camera_extrinsics, -) -from linkerhand_calibration.urdf_zero import ( - AXIS_JOINTS, - DIRECT_ZERO_JOINTS, - INHERITED_ZERO_JOINTS, - JointAxisMeasurement, - PalmOrientationMeasurement, - UrdfKinematicModel, - _angles_from_state, - _zero_sensitive_axis_error_rad, - anchor_right_19_mechanical_endpoint_curves, - baseline_hysteresis_by_cycle_rad, - derive_right_19_mechanical_endpoint_offsets, - fit_joint_axis_measurement, - fit_partial_palm_orientation_measurement, - fit_partial_palm_orientation_measurements, - fit_rotation_joint_curve, - select_cross_view_roll_direction_source, - solve_urdf_zero_offsets, - get_zero_calibration_profile, - get_right_19_thumb_zero_profile, - expand_right_19_thumb_zero_result_with_cad_fingers, - merge_right_19_thumb_zero_result, - write_zero_corrected_urdf, -) -from linkerhand_calibration.full_hand import ( - ACTIVE_JOINTS, - JOINT_SPECS, - MEASURED_JOINTS, - PASSIVE_JOINTS, - JointCurveFit, - get_hand_calibration_profile, -) -from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.urdf_input import ( - build_g20_urdf_input_payload, - load_g20_urdf_input, -) -from linkerhand_calibration.storage import atomic_write_json - - -PACKAGE_ROOT = Path(__file__).resolve().parents[1] -SOURCE_URDF = PACKAGE_ROOT / ( - "urdf/g20_left/linkerhand_g20_left.urdf" -) -RIGHT_SOURCE_URDF = PACKAGE_ROOT / ( - "urdf/g20_right/linkerhand_g20_right.urdf" -) - - -def test_cross_view_roll_direction_is_selected_once_by_group_consensus() -> None: - # Session 20260824_171302: the side residual in cycle 3 was 5.037 deg, - # just outside the 5 deg cone gate, while the other three side cycles - # passed and every front cycle was near 8 deg. Per-cycle selection mixed - # three side axes with one front axis and fabricated a 3.24 deg spread. - source = select_cross_view_roll_direction_source( - [math.radians(value) for value in (8.0078, 8.1710, 8.0712, 8.0236)], - [math.radians(value) for value in (4.9043, 4.9136, 5.0374, 4.9389)], - math.radians(5.0), - ) - - assert source == "secondary" - - -def test_cross_view_roll_direction_keeps_primary_without_consensus() -> None: - source = select_cross_view_roll_direction_source( - [math.radians(value) for value in (5.2, 4.8, 5.1, 4.9)], - [math.radians(value) for value in (4.7, 5.3, 4.8, 5.2)], - math.radians(5.0), - ) - - assert source == "primary" - - -def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch(): - parent = np.asarray([0.0, 0.0, 1.0]) - predicted = np.asarray([1.0, 0.0, 0.0]) - cone_mismatch = np.asarray( - [math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))] - ) - zero_mismatch = np.asarray( - [math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0] - ) - - assert _zero_sensitive_axis_error_rad( - predicted, cone_mismatch, parent - ) == pytest.approx(0.0, abs=1.0e-12) - assert math.degrees( - _zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent) - ) == pytest.approx(3.0, abs=1.0e-9) - - -def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None: - parent = np.asarray([0.0, 0.0, 1.0]) - cone = math.radians(32.0) - phase = math.radians(7.0) - predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)]) - observed = Rotation.from_rotvec(parent * phase).apply(predicted) - - error = _zero_sensitive_axis_error_rad(predicted, observed, parent) - - assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9) - - -def _payload(transform: np.ndarray) -> dict[str, list[float]]: - return { - "translation_xyz_m": transform[:3, 3].tolist(), - "quaternion_xyzw": Rotation.from_matrix( - transform[:3, :3] - ).as_quat().tolist(), - } - - -def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]: - axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float) - axis_parent /= np.linalg.norm(axis_parent) - centre_parent = np.asarray([0.012, -0.008, 0.021]) - radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2])) - radial = 0.035 * radial / np.linalg.norm(radial) - child_tag_mount = Rotation.from_euler( - "xyz", [1.1, -0.7, 0.45] - ) - common_from_parent = np.eye(4) - common_from_parent[:3, :3] = Rotation.from_euler( - "xyz", [-0.8, 0.55, 1.3] - ).as_matrix() - common_from_parent[:3, 3] = [0.41, -0.12, 0.73] - expected_axis = common_from_parent[:3, :3] @ axis_parent - expected_point = ( - common_from_parent[:3, :3] @ centre_parent - + common_from_parent[:3, 3] - ) - - commands = list(range(0, 256, 16)) + [255] - records = [] - for cycle in range(3): - for direction in ("decreasing", "increasing"): - for command in commands: - angle = math.radians(62.0) * (255.0 - command) / 255.0 - motion = Rotation.from_rotvec(axis_parent * angle) - relative_rotation = motion * child_tag_mount - relative_translation = centre_parent + motion.apply(radial) - child_common = common_from_parent.copy() - child_common[:3, :3] = ( - common_from_parent[:3, :3] - @ relative_rotation.as_matrix() - ) - child_common[:3, 3] = ( - common_from_parent[:3, :3] @ relative_translation - + common_from_parent[:3, 3] - ) - state = [255.0] * 20 - state[5] = float(command) - records.append( - { - "cycle": cycle, - "direction": direction, - "command_u8": command, - "relative_translation_xyz_m": relative_translation.tolist(), - "relative_quaternion_xyzw": relative_rotation.as_quat().tolist(), - "parent_pose_common": _payload(common_from_parent), - "child_pose_common": _payload(child_common), - "state_u8": state, - } - ) - return records, expected_axis, expected_point - - -def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None: - records, expected_axis, expected_point = _arbitrary_tag_records() - curve = fit_rotation_joint_curve(records, zero_command_u8=255) - measurement = fit_joint_axis_measurement( - "thumb_cmc_roll", records, cycle=0, zero_command_u8=255 - ) - - observed_axis = np.asarray(measurement.axis_common_xyz) - observed_point = np.asarray(measurement.point_common_xyz_m) - assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05)) - assert np.linalg.norm( - np.cross(observed_point - expected_point, expected_axis) - ) < 1.0e-6 - assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9) - assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6) - - -def test_baseline_hysteresis_uses_only_revolute_axis_component() -> None: - off_axis_noise = Rotation.from_rotvec( - np.radians([1.2, 0.0, 0.2]) - ).as_quat().tolist() - records = [] - for cycle in range(3): - records.extend( - ( - { - "cycle": cycle, - "direction": "decreasing", - "command_u8": 255, - "relative_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], - }, - { - "cycle": cycle, - "direction": "increasing", - "command_u8": 255, - "relative_quaternion_xyzw": off_axis_noise, - }, - ) - ) - - full_pose = baseline_hysteresis_by_cycle_rad( - records, zero_command_u8=255 - ) - joint_angle = baseline_hysteresis_by_cycle_rad( - records, zero_command_u8=255, axis_xyz=[0.0, 0.0, 1.0] - ) - - assert math.degrees(full_pose[0]) == pytest.approx( - math.hypot(1.2, 0.2), abs=1.0e-9 - ) - assert math.degrees(joint_angle[0]) == pytest.approx(0.2, abs=1.0e-9) - - -def test_canonical_zero_preserves_opposite_direction_baseline_offset() -> None: - records = [] - commands = sorted({0, 127, 255, *range(0, 256, 16)}) - branch_offset = math.radians(1.0) - for cycle in range(3): - for direction in ("decreasing", "increasing"): - offset = branch_offset if direction == "increasing" else 0.0 - for command in commands: - angle = math.radians(50.0) * (127.0 - command) / 255.0 - angle += offset - rotation = Rotation.from_rotvec([0.0, 0.0, angle]) - records.append( - { - "cycle": cycle, - "direction": direction, - "command_u8": command, - "relative_quaternion_xyzw": rotation.as_quat().tolist(), - } - ) - - curve = fit_rotation_joint_curve( - records, - zero_command_u8=127, - canonical_zero_direction="decreasing", - ) - - assert curve.angle_rad == curve.decreasing_rad - assert curve.decreasing_rad[127] == pytest.approx(0.0, abs=1.0e-9) - assert curve.increasing_rad[127] == pytest.approx( - branch_offset, abs=1.0e-6 - ) - assert curve.maximum_hysteresis_rad == pytest.approx( - branch_offset, abs=1.0e-6 - ) - - -@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"]) -def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth( - joint: str, -) -> None: - records, expected_axis, expected_point = _arbitrary_tag_records() - parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) - axis_parent = parent_rotation.inv().apply(expected_axis) - tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7])) - tangent /= np.linalg.norm(tangent) - # Reproduce monocular planar-PnP depth bias: the centre trajectory remains - # precise in its dominant directions but receives a command-correlated - # component that makes a free 3-D plane normal substantially wrong. - biased_records = [] - for record in records: - biased = dict(record) - point = np.asarray(record["relative_translation_xyz_m"], dtype=float) - depth_bias = 0.30 * float(point @ tangent) - biased["relative_translation_xyz_m"] = ( - point + depth_bias * axis_parent - ).tolist() - biased_records.append(biased) - - measurement = fit_joint_axis_measurement( - joint, biased_records, cycle=0, zero_command_u8=255 - ) - - observed_axis = np.asarray(measurement.axis_common_xyz) - observed_point = np.asarray(measurement.point_common_xyz_m) - assert abs(float(observed_axis @ expected_axis)) > math.cos( - math.radians(0.05) - ) - assert np.linalg.norm( - np.cross(observed_point - expected_point, expected_axis) - ) < 0.003 - assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0) - assert measurement.plane_rms_m < 0.003 - assert measurement.radial_rms_m < 0.003 - - -def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None: - records, expected_axis, expected_point = _arbitrary_tag_records() - common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) - axis_parent = common_from_parent.inv().apply(expected_axis) - # Exact end-on depth is a gauge along the physical axis and therefore - # cannot alter the observable axis line. An oblique camera has a small - # irreducible coupling between monocular depth and radial position; that - # case must be bounded by the residual/holdout gates, not asserted to be - # exactly recoverable from one view. - view_normal_parent = axis_parent - view_normal_common = common_from_parent.apply(view_normal_parent) - biased = [] - for record in records: - changed = dict(record) - fraction = (255.0 - float(record["command_u8"])) / 255.0 - depth_bias = 0.03 * (fraction - 0.5) - changed["relative_translation_xyz_m"] = ( - np.asarray(record["relative_translation_xyz_m"], dtype=float) - + depth_bias * view_normal_parent - ).tolist() - biased.append(changed) - - measurement = fit_joint_axis_measurement( - "thumb_cmc_pitch", - biased, - cycle=0, - zero_command_u8=255, - view_normal_common_xyz=view_normal_common, - ) - - observed_point = np.asarray(measurement.point_common_xyz_m) - assert measurement.axis_point_source == "pose_trajectory_image_plane" - assert measurement.pose_axis_line_rms_m < 1.0e-6 - assert np.linalg.norm( - np.cross(observed_point - expected_point, expected_axis) - ) < 1.0e-6 - - -def test_side_roll_axis_point_ignores_real_screw_translation() -> None: - records, expected_axis, expected_point = _arbitrary_tag_records() - common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) - axis_parent = common_from_parent.inv().apply(expected_axis) - screw_records = [] - for record in records: - changed = dict(record) - fraction = (255.0 - float(record["command_u8"])) / 255.0 - changed["relative_translation_xyz_m"] = ( - np.asarray(record["relative_translation_xyz_m"], dtype=float) - + 0.008 * fraction * axis_parent - ).tolist() - screw_records.append(changed) - - measurement = fit_joint_axis_measurement( - "middle_mcp_roll_side", - screw_records, - cycle=0, - zero_command_u8=255, - constrained_circle_joints=frozenset({"middle_mcp_roll_side"}), - ) - - observed_point = np.asarray(measurement.point_common_xyz_m) - assert measurement.pose_axis_line_rms_m < 1.0e-6 - assert np.linalg.norm( - np.cross(observed_point - expected_point, expected_axis) - ) < 1.0e-6 - - -def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None: - records, expected_axis, _ = _arbitrary_tag_records() - assert not any(record["command_u8"] == 127 for record in records) - - curve = fit_rotation_joint_curve(records, zero_command_u8=127) - measurement = fit_joint_axis_measurement( - "index_mcp_roll", records, cycle=0, zero_command_u8=127 - ) - - observed_axis = np.asarray(measurement.axis_common_xyz) - assert abs(float(observed_axis @ expected_axis)) > math.cos( - math.radians(0.05) - ) - assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9) - - -def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None: - records, expected_axis, expected_point = _arbitrary_tag_records() - common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) - physical_axis_parent = common_from_parent.inv().apply(expected_axis) - wrong_axis_parent = np.cross( - physical_axis_parent, np.asarray([0.2, 0.8, -0.1]) - ) - wrong_axis_parent /= np.linalg.norm(wrong_axis_parent) - mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"]) - contradictory = [] - for record in records: - changed = dict(record) - angle = math.radians(62.0) * ( - 255.0 - float(record["command_u8"]) - ) / 255.0 - changed["relative_quaternion_xyzw"] = ( - Rotation.from_rotvec(wrong_axis_parent * angle) * mount - ).as_quat().tolist() - contradictory.append(changed) - - measurement = fit_joint_axis_measurement( - "index_dip", - contradictory, - cycle=0, - zero_command_u8=255, - axis_common_constraint=expected_axis, - ) - - observed_axis = np.asarray(measurement.axis_common_xyz) - observed_point = np.asarray(measurement.point_common_xyz_m) - assert abs(float(observed_axis @ expected_axis)) > math.cos( - math.radians(0.05) - ) - assert np.linalg.norm( - np.cross(observed_point - expected_point, expected_axis) - ) < 1.0e-6 - - -def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None: - cameras = { - view: { - "serial_number": f"SERIAL_{view}", - "width": 1624, - "height": 1240, - "intrinsics_sha256": camera_info_fingerprint( - width=1624, - height=1240, - camera_matrix=np.asarray( - [[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]] - ), - ), - } - for view in ("front", "side", "top") - } - transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)} - transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix() - transforms["side"][:3, 3] = [0.2, 0.0, 0.1] - transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix() - transforms["top"][:3, 3] = [-0.1, 0.3, 0.2] - destination = tmp_path / "extrinsics.yaml" - - dump_three_camera_extrinsics( - destination, - cameras=cameras, - front_from_view=transforms, - quality={ - "passed": True, - "reprojection_rms_px": 0.3, - "maximum_rotation_repeatability_deg": 0.2, - "maximum_translation_repeatability_m": 0.001, - "front_side_captures": 15, - "front_top_captures": 15, - }, - ) - loaded = load_three_camera_extrinsics(destination) - - assert loaded.cameras["front"].serial_number == "SERIAL_front" - assert np.allclose(loaded.transform("side"), transforms["side"]) - assert np.allclose(loaded.transform("top"), transforms["top"]) - assert loaded.camera_matches( - "front", - serial_number="SERIAL_front", - width=1624, - height=1240, - intrinsics_sha256=cameras["front"]["intrinsics_sha256"], - ) - assert not loaded.camera_matches( - "front", - serial_number="WRONG_SERIAL", - width=1624, - height=1240, - intrinsics_sha256=cameras["front"]["intrinsics_sha256"], - ) - - relaxed_destination = tmp_path / "relaxed_extrinsics.yaml" - relaxed_quality_limits = { - "reprojection_rms_px": 1.5, - "maximum_rotation_repeatability_deg": 0.3, - "maximum_translation_repeatability_m": 0.0015, - } - dump_three_camera_extrinsics( - relaxed_destination, - cameras=cameras, - front_from_view=transforms, - quality={ - "passed": True, - "reprojection_rms_px": 1.4, - "maximum_rotation_repeatability_deg": 0.2, - "maximum_translation_repeatability_m": 0.001, - "front_side_captures": 15, - "front_top_captures": 15, - }, - quality_limits=relaxed_quality_limits, - ) - with pytest.raises(ValueError, match="exceeds 1.2"): - load_three_camera_extrinsics(relaxed_destination) - relaxed = load_three_camera_extrinsics( - relaxed_destination, - quality_limits=relaxed_quality_limits, - ) - assert relaxed.quality["reprojection_rms_px"] == pytest.approx(1.4) - - -def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]: - joint = next( - element - for element in ET.parse(path).getroot().findall("joint") - if element.get("name") == name - ) - origin = joint.find("origin") - axis = joint.find("axis") - xyz = np.asarray([float(value) for value in origin.get("xyz").split()]) - rpy = np.asarray([float(value) for value in origin.get("rpy").split()]) - axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()]) - return np.block( - [ - [Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]], - [np.asarray([[0.0, 0.0, 0.0, 1.0]])], - ] - ), axis_xyz / np.linalg.norm(axis_xyz) - - -def _joint_limit(path: Path, name: str) -> tuple[float, float]: - joint = next( - element - for element in ET.parse(path).getroot().findall("joint") - if element.get("name") == name - ) - limit = joint.find("limit") - return float(limit.get("lower")), float(limit.get("upper")) - - -def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None: - offset = math.radians(7.3) - destination = write_zero_corrected_urdf( - source_urdf=SOURCE_URDF, - output_directory=tmp_path, - serial_number="G20_LEFT_001", - offsets_rad={"thumb_cmc_yaw": offset}, - timestamp="20260806_120000", - ) - original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw") - corrected, _ = _joint_origin(destination, "thumb_cmc_yaw") - expected = original.copy() - expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec( - axis * offset - ).as_matrix() - - assert destination != SOURCE_URDF - assert np.allclose(corrected, expected, atol=1.0e-12) - with pytest.raises(ValueError, match="refusing to overwrite"): - write_zero_corrected_urdf( - source_urdf=SOURCE_URDF, - output_directory=tmp_path, - serial_number="G20_LEFT_001", - offsets_rad={"thumb_cmc_yaw": offset}, - timestamp="20260806_120000", - ) - with pytest.raises(ValueError, match="original CAD URDF"): - write_zero_corrected_urdf( - source_urdf=destination, - output_directory=tmp_path, - serial_number="G20_LEFT_001", - offsets_rad={"thumb_cmc_yaw": offset}, - timestamp="20260806_120001", - ) - with pytest.raises(ValueError, match="finite and within"): - write_zero_corrected_urdf( - source_urdf=SOURCE_URDF, - output_directory=tmp_path, - serial_number="G20_LEFT_001", - offsets_rad={"thumb_cmc_yaw": math.nan}, - timestamp="20260806_120002", - ) - - -def test_g20_json_handoff_preserves_existing_urdf_bytes(tmp_path: Path) -> None: - offsets = {"thumb_cmc_yaw": math.radians(7.3)} - direct = write_zero_corrected_urdf( - source_urdf=SOURCE_URDF, - output_directory=tmp_path / "direct", - serial_number="G20_LEFT_JSON_HANDOFF", - offsets_rad=offsets, - timestamp="20260902_140001", - ) - input_path = tmp_path / "g20_urdf_correction_input.json" - atomic_write_json( - input_path, - build_g20_urdf_input_payload( - side="left", - layout_id="legacy_11", - serial_number="G20_LEFT_JSON_HANDOFF", - source_urdf=SOURCE_URDF, - offsets_rad=offsets, - endpoint_anchored_offsets_rad={}, - ), - ) - loaded_offsets, loaded_endpoints = load_g20_urdf_input( - input_path, - source_urdf=SOURCE_URDF, - side="left", - layout_id="legacy_11", - serial_number="G20_LEFT_JSON_HANDOFF", - ) - via_json = write_zero_corrected_urdf( - source_urdf=SOURCE_URDF, - output_directory=tmp_path / "via_json", - serial_number="G20_LEFT_JSON_HANDOFF", - offsets_rad=loaded_offsets, - endpoint_anchored_offsets_rad=loaded_endpoints, - timestamp="20260902_140001", - ) - - assert direct.read_bytes() == via_json.read_bytes() - - -def test_urdf_writer_changes_only_the_16_active_zero_origins( - tmp_path: Path, -) -> None: - before = SOURCE_URDF.read_bytes() - offsets = { - name: math.radians(0.25 * (index + 1)) - for index, name in enumerate(ACTIVE_JOINTS) - } - destination = write_zero_corrected_urdf( - source_urdf=SOURCE_URDF, - output_directory=tmp_path, - serial_number="G20_LEFT_001", - offsets_rad=offsets, - timestamp="20260807_180000", - ) - - assert len(offsets) == 16 - assert SOURCE_URDF.read_bytes() == before - for name in ACTIVE_JOINTS: - original, axis = _joint_origin(SOURCE_URDF, name) - corrected, corrected_axis = _joint_origin(destination, name) - expected = original.copy() - expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec( - axis * offsets[name] - ).as_matrix() - assert np.allclose(corrected, expected, atol=1.0e-12) - assert np.allclose(corrected_axis, axis, atol=1.0e-12) - for name in PASSIVE_JOINTS: - original, axis = _joint_origin(SOURCE_URDF, name) - corrected, corrected_axis = _joint_origin(destination, name) - assert np.allclose(corrected, original, atol=1.0e-12) - assert np.allclose(corrected_axis, axis, atol=1.0e-12) - # Numeric limits move with their coordinate zero; physical limits do not. - for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS): - assert np.asarray(_joint_limit(destination, name)) + offsets.get(name, 0.0) == pytest.approx( - _joint_limit(SOURCE_URDF, name) - ) - - -def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit: - values = np.asarray( - [travel * (255.0 - command) / 255.0 for command in range(256)] - ) - values -= values[zero_command] - data = tuple(float(value) for value in values) - return JointCurveFit( - angle_rad=data, - decreasing_rad=data, - increasing_rad=data, - circle={}, - maximum_monotonic_correction_rad=0.0, - maximum_hysteresis_rad=0.0, - quality={}, - ) - - -def _solve_synthetic_offsets( - side: str, - offset_degrees: list[float], - *, - layout_id: str = "legacy_11", - inject_oblique_optical_depth_bias: bool = False, - inject_secondary_root_axis_bias_degrees: float = 0.0, - inject_secondary_root_point_bias_m: float = 0.0, - inject_observer_cone_bias_degrees: float = 0.0, - palm_orientation_frame_bias_degrees: float = 0.0, - maximum_systematic_axis_cone_bias_degrees: float | None = None, - pose_axis_line_rms_by_joint_m: dict[str, float] | None = None, - joint_maximum_offset_degrees: dict[str, float] | None = None, - validation_offset_bias_degrees: dict[str, float] | None = None, - static_output_offsets_degrees: dict[str, float] | None = None, - fixed_direct_offsets_degrees: dict[str, float] | None = None, - base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8), - base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72), - solve_thumb_only: bool = False, -): - hand = get_hand_calibration_profile(side, layout_id) - zero = get_zero_calibration_profile(side, layout_id) - source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF - baseline = [255.0] * 20 - baseline[6:10] = [127.0] * 4 - curves = { - name: _synthetic_curve( - int(baseline[hand.joint_specs[name].motor_index]), - math.radians(50.0), - ) - for name in hand.measured_joints - } - if inject_secondary_root_axis_bias_degrees: - # Make the thumb root the higher-travel, directly observed direction, - # matching the real right-hand data where the short pinky splay arc is - # the less reliable root-axis orientation estimate. - curves["thumb_cmc_roll"] = _synthetic_curve( - int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]), - math.radians(70.0), - ) - motor_by_joint = { - name: spec.motor_index for name, spec in hand.joint_specs.items() - } - offsets = { - name: math.radians(value) - for name, value in zip(zero.direct_zero_joints, offset_degrees) - } - model = UrdfKinematicModel(source) - base_rotation = Rotation.from_euler("xyz", base_euler_xyz_rad) - base_translation = np.asarray(base_translation_xyz_m) - measurements: list[JointAxisMeasurement] = [] - palm_orientation_measurements: list[PalmOrientationMeasurement] = [] - validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2 - training_cycles = tuple(range(validation_cycle)) - for cycle in range(validation_cycle + 1): - cycle_offsets = dict(offsets) - if cycle == validation_cycle: - cycle_offsets.update( - { - name: cycle_offsets[name] + math.radians(value) - for name, value in ( - validation_offset_bias_degrees or {} - ).items() - } - ) - for joint in zero.axis_joints: - state = list(baseline) - if joint == "thumb_cmc_yaw": - state[5] = 145.0 - angles = _angles_from_state( - state, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=zero.inherited_zero_joints, - ) - axis, point = model.axis_line( - joint, zero_offsets=cycle_offsets, joint_angles=angles - ) - point_common = base_rotation.apply(point) + base_translation - if ( - inject_secondary_root_point_bias_m - and joint == f"{zero.reference_finger}_mcp_roll" - ): - # A repeatable monocular depth error on the second parallel - # root line must affect translation only, never palm rotation - # or the inferred thumb-roll zero. - point_common = point_common + base_rotation.apply( - np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m]) - ) - view_normal_common = None - if ( - inject_oblique_optical_depth_bias - and joint in zero.phase_parent_joint - ): - parent_axis = model.axis_line( - zero.phase_parent_joint[joint], - zero_offsets=offsets, - joint_angles=angles, - )[0] - helper = ( - np.asarray([1.0, 0.0, 0.0]) - if abs(float(parent_axis[0])) < 0.8 - else np.asarray([0.0, 1.0, 0.0]) - ) - tilt_axis = np.cross(parent_axis, helper) - tilt_axis /= np.linalg.norm(tilt_axis) - view_normal = Rotation.from_rotvec( - math.radians(15.0) * tilt_axis - ).apply(parent_axis) - view_normal_common = tuple(base_rotation.apply(view_normal)) - # Simulate an independent planar-PnP depth error on the child - # Tag. It is large enough to drive the old 3-D phase solve to - # a configured offset bound. - point_common = point_common + 0.03 * np.asarray( - view_normal_common - ) - axis_common = base_rotation.apply(axis) - if ( - inject_observer_cone_bias_degrees - and joint == "thumb_cmc_pitch" - ): - parent_axis = model.axis_line( - zero.axis_parent_joint[joint], - zero_offsets=offsets, - joint_angles=angles, - )[0] - cone_normal = np.cross(axis, parent_axis) - cone_normal /= np.linalg.norm(cone_normal) - axis_common = base_rotation.apply( - Rotation.from_rotvec( - math.radians(inject_observer_cone_bias_degrees) - * cone_normal - ).apply(axis) - ) - if ( - inject_secondary_root_axis_bias_degrees - and joint == f"{zero.reference_finger}_mcp_roll" - ): - helper = np.asarray([0.0, 0.0, 1.0]) - if abs(float(axis_common @ helper)) > 0.8: - helper = np.asarray([0.0, 1.0, 0.0]) - bias_axis = np.cross(axis_common, helper) - bias_axis /= np.linalg.norm(bias_axis) - axis_common = Rotation.from_rotvec( - math.radians(inject_secondary_root_axis_bias_degrees) - * bias_axis - ).apply(axis_common) - measurements.append( - JointAxisMeasurement( - joint=joint, - cycle=cycle, - axis_common_xyz=tuple(axis_common), - point_common_xyz_m=tuple(point_common), - condition_state_u8=tuple(state), - plane_rms_m=0.0002, - radial_rms_m=0.0002, - rotation_circle_axis_difference_rad=math.radians(0.1), - view_normal_common_xyz=view_normal_common, - pose_axis_line_rms_m=( - pose_axis_line_rms_by_joint_m or {} - ).get(joint, 0.0), - ) - ) - for source_joint, model_joint in ( - hand.palm_orientation_sources or {} - ).items(): - state = list(baseline) - angles = _angles_from_state( - state, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=zero.inherited_zero_joints, - ) - axis, _ = model.axis_line( - model_joint, - zero_offsets=cycle_offsets, - joint_angles=angles, - ) - axis_common = base_rotation.apply(axis) - if palm_orientation_frame_bias_degrees: - bias_axis = np.asarray([0.3, -0.2, 0.4], dtype=float) - bias_axis /= np.linalg.norm(bias_axis) - axis_common = Rotation.from_rotvec( - math.radians(palm_orientation_frame_bias_degrees) - * bias_axis - ).apply(axis_common) - palm_orientation_measurements.append( - PalmOrientationMeasurement( - source_joint=source_joint, - model_joint=model_joint, - cycle=cycle, - axis_common_xyz=tuple(axis_common), - condition_state_u8=tuple(state), - observed_arc_rad=math.radians(45.0), - rotation_orthogonal_rms_rad=math.radians(0.1), - ) - ) - solve_profile = ( - get_right_19_thumb_zero_profile() if solve_thumb_only else zero - ) - result = solve_urdf_zero_offsets( - source_urdf=source, - measurements=[ - item for item in measurements if item.joint in solve_profile.axis_joints - ], - palm_orientation_measurements=palm_orientation_measurements, - curves=curves, - motor_by_joint=motor_by_joint, - hand_type=side, - tag_layout=layout_id, - joint_maximum_offset_rad={ - name: math.radians(value) - for name, value in (joint_maximum_offset_degrees or {}).items() - }, - training_cycles=training_cycles, - validation_cycle=validation_cycle, - maximum_systematic_axis_cone_bias_rad=( - None - if maximum_systematic_axis_cone_bias_degrees is None - else math.radians( - maximum_systematic_axis_cone_bias_degrees - ) - ), - static_output_zero_offsets_rad={ - name: math.radians(value) - for name, value in (static_output_offsets_degrees or {}).items() - }, - fixed_direct_zero_offsets_rad=( - None - if fixed_direct_offsets_degrees is None - else { - name: math.radians(value) - for name, value in fixed_direct_offsets_degrees.items() - if name in solve_profile.direct_zero_joints - } - ), - zero_profile=solve_profile, - ) - return zero, result - - -def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supplied() -> None: - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - zero, result = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_19" - ) - - assert len(zero.direct_zero_joints) == 16 - assert len(zero.axis_joints) == 21 - assert result.passed is True - finger_rolls = tuple( - name - for name in zero.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) - roll_common = float( - np.median( - [ - injected[zero.direct_zero_joints.index(name)] - for name in finger_rolls - ] - ) - ) - for name, expected in zip(zero.direct_zero_joints, injected): - if name in zero.fixed_direct_zero_offsets_rad: - expected = zero.fixed_direct_zero_offsets_rad[name] - elif name in finger_rolls: - expected -= roll_common - assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( - expected, abs=0.05 - ) - assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx( - -1.5, abs=0.05 - ) - assert zero.fixed_direct_zero_offsets_rad == {} - assert result.training_cycles == (0, 1, 2) - assert result.validation_cycle == 3 - assert all( - value == pytest.approx(0.0, abs=1.0e-12) - for value in result.offset_confidence_half_width_rad.values() - ) - assert result.observability_parameter_count == 22 - assert result.observability_rank == 22 - assert math.isfinite(result.observability_condition_number) - assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints) - - -def test_right_19_thumb_kernel_is_independent_and_matches_full_hand() -> None: - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - _, full = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, - static_output_offsets_degrees={"thumb_cmc_roll": 2.0}, - ) - _, thumb = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - solve_thumb_only=True, - fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, - static_output_offsets_degrees={"thumb_cmc_roll": 2.0}, - ) - - assert thumb.passed is True - assert set(thumb.direct_offsets_rad) == { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - for name in thumb.direct_offsets_rad: - assert thumb.direct_offsets_rad[name] == pytest.approx( - full.direct_offsets_rad[name], abs=math.radians(0.05) - ) - - finger_offsets = { - name: full.direct_offsets_rad[name] - for name in get_zero_calibration_profile( - "right", "g20_right_19" - ).direct_zero_joints - if not name.startswith("thumb_") - } - thumb_only = merge_right_19_thumb_zero_result( - thumb_result=thumb, - preserved_offsets_rad=finger_offsets, - ) - full_hand = merge_right_19_thumb_zero_result( - thumb_result=thumb, - companion_result=full, - ) - for name in thumb.direct_offsets_rad: - assert thumb_only.direct_offsets_rad[name] == pytest.approx( - full_hand.direct_offsets_rad[name], abs=1.0e-12 - ) - standalone = expand_right_19_thumb_zero_result_with_cad_fingers(thumb) - assert all( - standalone.direct_offsets_rad[name] == 0.0 - for name in standalone.direct_offsets_rad - if not name.startswith("thumb_") - ) - - -def test_thumb_endpoint_output_uses_published_offset_safety_limit() -> None: - # The visual roll scalar may carry a root-frame gauge beyond the global - # limit. A separately validated mechanical endpoint is the value written - # to the URDF and therefore the value the configured safety gate guards. - injected = [ - 21.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - - _, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - solve_thumb_only=True, - fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, - static_output_offsets_degrees={"thumb_cmc_roll": 3.45}, - ) - - assert result.passed is True - assert "thumb_cmc_roll" not in result.failure_reasons - assert math.degrees( - result.direct_offsets_rad["thumb_cmc_roll"] - ) == pytest.approx(3.45, abs=0.01) - - -def test_right_19_thumb_kernel_is_invariant_to_rigid_hand_movement() -> None: - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - arguments = { - "layout_id": "g20_right_19", - "solve_thumb_only": True, - "fixed_direct_offsets_degrees": {"thumb_mcp": -1.5}, - "static_output_offsets_degrees": {"thumb_cmc_roll": 2.0}, - } - _, first = _solve_synthetic_offsets("right", injected, **arguments) - _, moved = _solve_synthetic_offsets( - "right", - injected, - base_euler_xyz_rad=(-0.2, 0.7, -0.35), - base_translation_xyz_m=(-0.15, 0.42, 0.55), - **arguments, - ) - - assert first.passed is True - assert moved.passed is True - for name in first.direct_offsets_rad: - assert moved.direct_offsets_rad[name] == pytest.approx( - first.direct_offsets_rad[name], abs=math.radians(0.01) - ) - - -def test_right_19_independent_roll_output_cannot_change_yaw_solution() -> None: - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - _, visual = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_19" - ) - _, independent = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - static_output_offsets_degrees={"thumb_cmc_roll": 3.4}, - ) - - assert math.degrees( - independent.direct_offsets_rad["thumb_cmc_roll"] - ) == pytest.approx(3.4) - assert independent.direct_offsets_rad["thumb_cmc_yaw"] == pytest.approx( - visual.direct_offsets_rad["thumb_cmc_yaw"], abs=1.0e-10 - ) - assert independent.direct_offsets_rad["thumb_cmc_pitch"] == pytest.approx( - visual.direct_offsets_rad["thumb_cmc_pitch"], abs=1.0e-10 - ) - - -def _partial_orientation_records( - *, - tag_mount: Rotation, - common_rotation: Rotation, - maximum_angle_deg: float = 30.0, - tag_offset_xyz_m: tuple[float, float, float] = (0.01, 0.02, -0.015), - include_child_pose_common: bool = False, -) -> list[dict[str, object]]: - axis_parent = np.asarray([0.0, 1.0, 0.0]) - parent_pose = { - "translation_xyz_m": [0.2, -0.1, 0.7], - "quaternion_xyzw": list(common_rotation.as_quat()), - } - records: list[dict[str, object]] = [] - commands = tuple(range(255, 174, -4)) - for direction, ordered in ( - ("decreasing", commands), - ("increasing", tuple(reversed(commands))), - ): - for command in ordered: - fraction = (255 - command) / (255 - commands[-1]) - angle = math.radians(maximum_angle_deg) * fraction - relative = ( - Rotation.from_rotvec(axis_parent * angle) * tag_mount - ) - relative_translation = Rotation.from_rotvec( - axis_parent * angle - ).apply(tag_offset_xyz_m) - state = [255.0] * 20 - state[1] = float(command) - record: dict[str, object] = { - "cycle": 0, - "direction": direction, - "command_u8": command, - "relative_quaternion_xyzw": list(relative.as_quat()), - "relative_translation_xyz_m": list( - relative_translation - ), - "parent_pose_common": parent_pose, - "state_u8": state, - } - if include_child_pose_common: - child_common = common_rotation * relative - record["child_pose_common"] = { - "translation_xyz_m": list( - np.asarray(parent_pose["translation_xyz_m"]) - + common_rotation.apply(relative_translation) - ), - "quaternion_xyzw": list(child_common.as_quat()), - } - records.append(record) - return records - - -def test_partial_palm_direction_ignores_fixed_tag_mount_pose() -> None: - common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) - first = fit_partial_palm_orientation_measurement( - "index_mcp_pitch_front_axis", - "index_mcp_pitch", - _partial_orientation_records( - tag_mount=Rotation.from_euler("xyz", [0.1, 0.2, -0.4]), - common_rotation=common_rotation, - ), - cycle=0, - zero_command_u8=255, - ) - second = fit_partial_palm_orientation_measurement( - "index_mcp_pitch_front_axis", - "index_mcp_pitch", - _partial_orientation_records( - tag_mount=Rotation.from_euler("xyz", [-0.7, 0.45, 0.9]), - common_rotation=common_rotation, - tag_offset_xyz_m=(-0.035, 0.008, 0.041), - ), - cycle=0, - zero_command_u8=255, - ) - expected = common_rotation.apply([0.0, 1.0, 0.0]) - - assert abs(np.dot(first.axis_common_xyz, expected)) == pytest.approx( - 1.0, abs=1.0e-8 - ) - assert abs( - np.dot(first.axis_common_xyz, second.axis_common_xyz) - ) == pytest.approx(1.0, abs=1.0e-8) - - -def test_partial_palm_direction_is_not_weighted_by_dwell_frame_count() -> None: - records = _partial_orientation_records( - tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), - common_rotation=Rotation.identity(), - ) - baseline = fit_partial_palm_orientation_measurement( - "index_mcp_pitch_front_axis", - "index_mcp_pitch", - records, - cycle=0, - zero_command_u8=255, - ) - source = next( - record - for record in records - if record["direction"] == "decreasing" - and record["command_u8"] == 223 - ) - biased = dict(source) - biased["relative_quaternion_xyzw"] = list( - ( - Rotation.from_rotvec([math.radians(2.0), 0.0, 0.0]) - * Rotation.from_quat(source["relative_quaternion_xyzw"]) - ).as_quat() - ) - fitted = fit_partial_palm_orientation_measurement( - "index_mcp_pitch_front_axis", - "index_mcp_pitch", - [*records, *([biased] * 200)], - cycle=0, - zero_command_u8=255, - ) - - difference = math.acos( - abs( - float( - np.clip( - np.dot(baseline.axis_common_xyz, fitted.axis_common_xyz), - -1.0, - 1.0, - ) - ) - ) - ) - assert difference < math.radians(0.25) - - -def test_partial_palm_direction_ignores_far_stroke_pnp_bias() -> None: - records = _partial_orientation_records( - tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), - common_rotation=Rotation.identity(), - maximum_angle_deg=30.0, - ) - expected = fit_partial_palm_orientation_measurement( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll", - records, - cycle=0, - zero_command_u8=255, - minimum_arc_rad=math.radians(5.0), - maximum_command_distance_u8=64, - ) - biased: list[dict[str, object]] = [] - for source in records: - record = dict(source) - if abs(int(record["command_u8"]) - 255) > 64: - rotation = Rotation.from_quat( - record["relative_quaternion_xyzw"] - ) - record["relative_quaternion_xyzw"] = list( - ( - Rotation.from_rotvec([math.radians(12.0), 0.0, 0.0]) - * rotation - ).as_quat() - ) - biased.append(record) - - fitted = fit_partial_palm_orientation_measurement( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll", - biased, - cycle=0, - zero_command_u8=255, - minimum_arc_rad=math.radians(5.0), - maximum_command_distance_u8=64, - ) - - assert abs( - float(np.dot(expected.axis_common_xyz, fitted.axis_common_xyz)) - ) == pytest.approx(1.0, abs=1.0e-10) - - -def test_partial_palm_direction_uses_robust_full_stroke_local_motion() -> None: - common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) - records = _partial_orientation_records( - tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), - common_rotation=common_rotation, - maximum_angle_deg=48.0, - include_child_pose_common=True, - ) - expected = fit_partial_palm_orientation_measurement( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll", - records, - cycle=0, - zero_command_u8=255, - ) - - biased: list[dict[str, object]] = [] - for source in records: - record = dict(source) - pose = dict(record["child_pose_common"]) - # Simulate a command-correlated planar-PnP branch curvature affecting - # fewer than one quarter of the full-stroke local increments. The - # zero-adjacent quality trajectory remains unchanged. - if int(record["command_u8"]) <= 187: - rotation = Rotation.from_quat(pose["quaternion_xyzw"]) - amount = math.radians(8.0) * ( - (187 - int(record["command_u8"])) / 12.0 - ) - pose["quaternion_xyzw"] = list( - ( - Rotation.from_rotvec([amount, 0.0, 0.0]) * rotation - ).as_quat() - ) - record["child_pose_common"] = pose - biased.append(record) - - fitted = fit_partial_palm_orientation_measurement( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll", - biased, - cycle=0, - zero_command_u8=255, - ) - difference = math.acos( - abs( - float( - np.clip( - np.dot(expected.axis_common_xyz, fitted.axis_common_xyz), - -1.0, - 1.0, - ) - ) - ) - ) - - assert expected.axis_estimator == "robust_full_stroke_local_so3_v1" - assert expected.incremental_pair_count >= 24 - assert fitted.axis_estimator == expected.axis_estimator - assert difference < math.radians(0.25) - - -def test_full_stroke_palm_axis_tracks_reestablished_common_frame() -> None: - tag_mount = Rotation.from_euler("xyz", [0.2, -0.1, 0.3]) - first_common = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) - repositioning = Rotation.from_euler("xyz", [-0.18, 0.27, -0.41]) - second_common = repositioning * first_common - first = fit_partial_palm_orientation_measurement( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll", - _partial_orientation_records( - tag_mount=tag_mount, - common_rotation=first_common, - maximum_angle_deg=48.0, - include_child_pose_common=True, - ), - cycle=0, - zero_command_u8=255, - ) - second = fit_partial_palm_orientation_measurement( - "thumb_cmc_roll_top_axis", - "thumb_cmc_roll", - _partial_orientation_records( - tag_mount=tag_mount, - common_rotation=second_common, - maximum_angle_deg=48.0, - include_child_pose_common=True, - ), - cycle=0, - zero_command_u8=255, - ) - expected_second = repositioning.apply(first.axis_common_xyz) - - assert first.axis_estimator == "robust_full_stroke_local_so3_v1" - assert second.axis_estimator == first.axis_estimator - assert abs(float(np.dot(expected_second, second.axis_common_xyz))) == ( - pytest.approx(1.0, abs=1.0e-8) - ) - - -def test_right_19_top_axis_pair_holdout_ignores_shared_frame_bias() -> None: - injected = [0.0] * 16 - _, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - palm_orientation_frame_bias_degrees=8.0, - ) - - assert result.passed is True - assert math.degrees( - result.direct_offsets_rad["thumb_cmc_yaw"] - ) == pytest.approx(0.0, abs=0.05) - - -def test_partial_palm_direction_rejects_too_short_visible_arc() -> None: - records = _partial_orientation_records( - tag_mount=Rotation.identity(), - common_rotation=Rotation.identity(), - maximum_angle_deg=5.0, - ) - - with pytest.raises(ValueError, match="visible rotation arc"): - fit_partial_palm_orientation_measurement( - "index_mcp_pitch_front_axis", - "index_mcp_pitch", - records, - cycle=0, - zero_command_u8=255, - ) - - -def test_partial_palm_direction_uses_three_of_four_visible_sources() -> None: - sources = { - f"{finger}_mcp_pitch_front_axis": f"{finger}_mcp_pitch" - for finger in ("index", "middle", "ring", "pinky") - } - records = { - source: _partial_orientation_records( - tag_mount=Rotation.from_euler( - "xyz", [0.1 * index, -0.2, 0.3] - ), - common_rotation=Rotation.identity(), - maximum_angle_deg=(5.0 if index == 3 else 30.0), - ) - for index, source in enumerate(sources) - } - - fitted, rejected = fit_partial_palm_orientation_measurements( - sources=sources, - records_by_joint=records, - motor_by_source={source: index + 1 for index, source in enumerate(sources)}, - baseline_command_u8=(255,) * 20, - cycles=(0,), - minimum_sources=3, - ) - - assert len(fitted) == 3 - assert len(rejected) == 1 - assert next(iter(rejected)).startswith("pinky_mcp_pitch_front_axis") - - -def test_right_19_offsets_are_invariant_to_rigid_hand_repositioning() -> None: - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - _, first = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_19" - ) - _, moved = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - base_euler_xyz_rad=(-0.25, 0.55, -0.35), - base_translation_xyz_m=(-0.12, 0.28, 0.91), - ) - - assert moved.direct_offsets_rad == pytest.approx( - first.direct_offsets_rad, abs=1.0e-8 - ) - - -def test_right_19_roll_and_verified_contact_zeros_use_independent_endpoints() -> None: - hand = get_hand_calibration_profile("right", "g20_right_19") - curves = { - name: _synthetic_curve(255, math.radians(50.0)) - for name in hand.measured_joints - } - for finger in ("index", "middle", "ring", "pinky"): - curves[f"{finger}_mcp_pitch"] = _synthetic_curve( - 255, math.radians(71.0) - ) - curves[f"{finger}_pip"] = _synthetic_curve( - 255, math.radians(103.0) - ) - curves["thumb_mcp"] = _synthetic_curve(255, math.radians(70.0)) - curves["thumb_cmc_roll"] = _synthetic_curve( - 255, math.radians(76.0) - ) - curves["thumb_cmc_yaw"] = _synthetic_curve( - 255, math.radians(91.0) - ) - curves["thumb_cmc_pitch"] = _synthetic_curve( - 255, math.radians(47.0) - ) - - offsets = derive_right_19_mechanical_endpoint_offsets( - RIGHT_SOURCE_URDF, curves - ) - - assert set(offsets) == { - "thumb_cmc_roll", - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") - ), - } - assert math.degrees(offsets["thumb_mcp"]) == pytest.approx( - math.degrees(1.25) - 70.0 - ) - assert math.degrees(offsets["thumb_cmc_roll"]) == pytest.approx( - math.degrees(1.39) - 76.0 - ) - assert not {"thumb_cmc_yaw", "thumb_cmc_pitch"}.intersection(offsets) - assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx( - math.degrees(1.22) - 71.0 - ) - assert math.degrees(offsets["index_pip"]) == pytest.approx( - math.degrees(1.75) - 103.0 - ) - - -def _settled_endpoint_records( - travel_rad: float, - *, - increasing_travel_rad: float | None = None, -) -> list[dict[str, object]]: - fixed_mounting = Rotation.from_euler("xyz", [0.7, -0.4, 1.1]) - mounted_axis = Rotation.from_euler("xyz", [-0.3, 0.8, 0.2]).apply( - [0.0, 0.0, 1.0] - ) - result: list[dict[str, object]] = [] - for direction, branch_travel in ( - ("decreasing", travel_rad), - ( - "increasing", - travel_rad - if increasing_travel_rad is None - else increasing_travel_rad, - ), - ): - endpoint = fixed_mounting * Rotation.from_rotvec( - mounted_axis * branch_travel - ) - for command, rotation in ((0, endpoint), (255, fixed_mounting)): - result.append( - { - "cycle": 0, - "direction": direction, - "requested_command_u8": command, - "feedback_u8": float(command), - "relative_quaternion_xyzw": rotation.as_quat().tolist(), - } - ) - return result - - -def test_right_19_endpoint_curve_scale_uses_direct_rigid_rotation() -> None: - hand = get_hand_calibration_profile("right", "g20_right_19") - names = { - "thumb_cmc_roll", - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") - ), - } - projected_travel = math.radians(105.832) - direct_travel = math.radians(103.820) - curves = { - name: _synthetic_curve(255, projected_travel) - for name in hand.measured_joints - } - records = { - name: _settled_endpoint_records(direct_travel) for name in names - } - - anchored = anchor_right_19_mechanical_endpoint_curves( - curves, - records, - feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), - ) - - assert curves["middle_pip"].angle_rad[0] == pytest.approx( - projected_travel - ) - for name in names: - assert anchored[name].angle_rad[0] == pytest.approx(direct_travel) - assert anchored[name].angle_rad[255] == pytest.approx(0.0) - assert anchored[name].circle[ - "mechanical_endpoint_direct_travel_rad" - ] == pytest.approx(direct_travel) - assert anchored["thumb_cmc_pitch"] is curves["thumb_cmc_pitch"] - - -def test_right_19_endpoint_curve_scale_rejects_direction_disagreement() -> None: - hand = get_hand_calibration_profile("right", "g20_right_19") - names = { - "thumb_cmc_roll", - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") - ), - } - curves = { - name: _synthetic_curve(255, math.radians(103.0)) - for name in hand.measured_joints - } - records = { - name: _settled_endpoint_records(math.radians(103.0)) - for name in names - } - records["middle_pip"] = _settled_endpoint_records( - math.radians(103.0), - increasing_travel_rad=math.radians(104.5), - ) - - with pytest.raises(ValueError, match="endpoint directions disagree"): - anchor_right_19_mechanical_endpoint_curves( - curves, - records, - feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), - ) - - -def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None: - # A shared electrical centre belongs to the four-motor common datum; the - # strict 3 deg assembly guard applies to finger-to-finger deviations. - injected = [ - 2.0, -3.0, 4.0, -1.5, - 6.0, -1.0, 0.7, - 6.4, -0.7, -0.8, - 5.7, 0.6, 0.9, - 6.2, -1.0, -0.6, - ] - - zero, result = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_19" - ) - - assert result.passed is True - finger_rolls = tuple( - name - for name in zero.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) - roll_common = float( - np.median( - [ - injected[zero.direct_zero_joints.index(name)] - for name in finger_rolls - ] - ) - ) - for name, expected in zip(zero.direct_zero_joints, injected): - if name in zero.fixed_direct_zero_offsets_rad: - expected = zero.fixed_direct_zero_offsets_rad[name] - elif name in finger_rolls: - expected -= roll_common - assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( - expected, abs=0.05 - ) - assert np.median( - [result.direct_offsets_rad[name] for name in finger_rolls] - ) == pytest.approx(0.0, abs=1.0e-10) - - -def test_right_19_roll_gauge_at_search_bound_uses_relative_deviation() -> None: - # One raw roll reaches the diagnostic search edge, as in session 160326, - # while all four physical deviations from their shared datum remain safe. - injected = [ - 2.0, -3.0, 4.0, -1.5, - 9.3, -1.0, 0.0, - 8.4, -0.7, 0.0, - 8.7, 0.6, 0.0, - 8.8, -1.0, 0.0, - ] - - zero, result = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_19" - ) - - assert result.passed is True - finger_rolls = tuple( - name - for name in zero.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) - assert max( - abs(math.degrees(result.direct_offsets_rad[name])) - for name in finger_rolls - ) < 3.0 - assert not result.failure_reasons - - -def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - zero, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - validation_offset_bias_degrees={"thumb_cmc_roll": 2.0}, - ) - - finger_rolls = tuple( - name - for name in zero.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) - roll_common = float( - np.median( - [ - injected[zero.direct_zero_joints.index(name)] - for name in finger_rolls - ] - ) - ) - assert math.degrees( - result.direct_offsets_rad["thumb_cmc_roll"] - ) == pytest.approx(injected[0], abs=0.05) - assert result.validation_cycle not in result.training_cycles - - -def test_failed_thumb_holdout_does_not_blame_fixed_partial_scope_zeros() -> None: - zero = get_zero_calibration_profile("right", "g20_right_19") - injected = [ - 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, 0.7, - 0.8, -0.7, -0.8, - -0.5, 0.6, 0.9, - 1.1, -1.0, -0.6, - ] - frozen = { - name: injected[index] - for index, name in enumerate(zero.direct_zero_joints) - if not name.startswith("thumb_") - } - - _, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - validation_offset_bias_degrees={"thumb_cmc_yaw": 8.0}, - fixed_direct_offsets_degrees=frozen, - ) - - assert result.passed is False - assert result.failure_reasons == { - "palm_orientation": "palm_orientation_holdout_too_large" - } - assert set(frozen).isdisjoint(result.failure_reasons) - - -def test_right_19_palm_pose_and_16_zero_observation_jacobian_is_full_rank() -> None: - """Guard the reviewed 6-palm-DOF plus 16-static-zero observability.""" - zero = get_zero_calibration_profile("right", "g20_right_19") - model = UrdfKinematicModel(RIGHT_SOURCE_URDF) - parameter_count = 6 + len(zero.direct_zero_joints) - - def line_observations(parameters: np.ndarray) -> np.ndarray: - palm_rotation = Rotation.from_rotvec(parameters[:3]) - palm_translation = parameters[3:6] - offsets = { - name: float(value) - for name, value in zip( - zero.direct_zero_joints, parameters[6:] - ) - } - result: list[float] = [] - for joint in zero.axis_joints: - axis, point = model.axis_line( - joint, zero_offsets=offsets, joint_angles={} - ) - axis = palm_rotation.apply(axis) - point = palm_rotation.apply(point) + palm_translation - # An oriented 3-D line is represented by its direction and - # Pluecker moment. The moment is invariant to choosing a different - # point along the same axis, so no unobservable along-axis Tag - # placement is accidentally counted as information. - result.extend(float(value) for value in axis) - result.extend(float(value) for value in np.cross(point, axis)) - return np.asarray(result, dtype=float) - - origin = np.zeros(parameter_count, dtype=float) - step = 1.0e-6 - jacobian = np.column_stack( - [ - ( - line_observations( - origin + np.eye(parameter_count, dtype=float)[index] * step - ) - - line_observations( - origin - np.eye(parameter_count, dtype=float)[index] * step - ) - ) - / (2.0 * step) - for index in range(parameter_count) - ] - ) - - assert parameter_count == 22 - assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count - - -def test_right_19_urdf_writer_rejects_inconsistent_source_mimic_limits(tmp_path: Path) -> None: - zero = get_zero_calibration_profile("right", "g20_right_19") - offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints} - before = RIGHT_SOURCE_URDF.read_bytes() - with pytest.raises(ValueError, match="mimic reachable range exceeds limits"): - write_zero_corrected_urdf( - source_urdf=RIGHT_SOURCE_URDF, output_directory=tmp_path, - serial_number="G20_RIGHT_019", offsets_rad=offsets, - timestamp="20260818_120000") - assert RIGHT_SOURCE_URDF.read_bytes() == before - assert not list(tmp_path.glob("*.urdf")) - - -def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None: - zero, result = _solve_synthetic_offsets("right", [0.1] * 7) - assert result.passed is True - static_policy = { - **zero.fixed_direct_zero_offsets_rad, - **zero.static_output_zero_offsets_rad, - } - for name, value in result.direct_offsets_rad.items(): - assert value == pytest.approx(static_policy.get(name, 0.0)) - - -def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None: - right = get_zero_calibration_profile("right") - right_19 = get_zero_calibration_profile("right", "g20_right_19") - left = get_zero_calibration_profile("left") - - assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad - assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad - assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad - assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad - assert "thumb_mcp" not in right_19.fixed_direct_zero_offsets_rad - - -def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None: - zero, result = _solve_synthetic_offsets( - "right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0] - ) - reference_roll = f"{zero.reference_finger}_mcp_roll" - assert result.passed is True - assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx( - 0.0, abs=1.0e-12 - ) - - -def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None: - zero, result = _solve_synthetic_offsets( - "right", - [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], - inject_secondary_root_axis_bias_degrees=15.0, - ) - - assert result.passed is True - expected = dict( - zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]) - ) - expected.update( - { - name: math.degrees(value) - for name, value in { - **zero.fixed_direct_zero_offsets_rad, - **zero.static_output_zero_offsets_rad, - }.items() - } - ) - for name, value in expected.items(): - assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( - value, abs=0.05 - ) - - -def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None: - zero, result = _solve_synthetic_offsets( - "right", - [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], - inject_secondary_root_point_bias_m=0.02, - ) - - assert result.passed is True - assert math.degrees( - result.direct_offsets_rad["thumb_cmc_roll"] - ) == pytest.approx(2.0, abs=0.05) - assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0 - - -def test_thumb_mcp_is_not_hard_coded_to_original_cad_zero() -> None: - offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0, *([0.0] * 9)] - _, result = _solve_synthetic_offsets( - "right", - offsets, - layout_id="g20_right_19", - joint_maximum_offset_degrees={"thumb_mcp": 45.0}, - ) - - assert result.passed is True - assert math.degrees(result.direct_offsets_rad["thumb_mcp"]) == pytest.approx( - -40.0, abs=0.05 - ) - assert all( - math.degrees(value) == pytest.approx(-40.0, abs=0.05) - for value in result.cycle_offsets_rad["thumb_mcp"] - ) - assert result.validation_error_by_joint_rad["thumb_ip"] < math.radians(0.05) - for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"): - assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0 - for name in ("pinky_mcp_pitch", "pinky_pip"): - assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0 - assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0) - - -def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None: - zero, result = _solve_synthetic_offsets( - "right", - [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], - inject_oblique_optical_depth_bias=True, - ) - - assert result.passed is True - expected = dict( - zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]) - ) - expected.update( - { - name: math.degrees(value) - for name, value in { - **zero.fixed_direct_zero_offsets_rad, - **zero.static_output_zero_offsets_rad, - }.items() - } - ) - for name, value in expected.items(): - assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( - value, abs=0.05 - ) - - -def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None: - _, result = _solve_synthetic_offsets( - "right", - [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], - inject_observer_cone_bias_degrees=8.0, - ) - - assert result.passed is False - assert result.failure_reasons["thumb_cmc_yaw"] == ( - "zero_axis_cone_mismatch_too_large" - ) - - -def test_right_19_audits_stable_cross_view_cone_bias_without_retry() -> None: - injected = [0.0] * 16 - _, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - inject_observer_cone_bias_degrees=8.0, - maximum_systematic_axis_cone_bias_degrees=15.0, - ) - - assert result.passed is True - assert math.degrees( - result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"] - ) == pytest.approx(8.0) - assert result.axis_cone_bias_classification_by_joint[ - "thumb_cmc_yaw" - ] == "stable_cross_view_or_planar_pnp_bias" - - -def test_right_19_thumb_yaw_uses_top_axis_pair_not_front_cone_bias() -> None: - zero = get_zero_calibration_profile("right", "g20_right_19") - injected = [0.0] * len(zero.direct_zero_joints) - injected[zero.direct_zero_joints.index("thumb_cmc_roll")] = 3.611 - injected[zero.direct_zero_joints.index("thumb_cmc_yaw")] = -1.319 - zero, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - inject_observer_cone_bias_degrees=3.803, - maximum_systematic_axis_cone_bias_degrees=15.0, - ) - - assert result.passed is True - assert math.degrees( - result.direct_offsets_rad["thumb_cmc_roll"] - ) == pytest.approx(3.611, abs=0.05) - assert math.degrees( - result.direct_offsets_rad["thumb_cmc_yaw"] - ) == pytest.approx(-1.319, abs=0.05) - assert math.degrees( - result.axis_cone_mismatch_by_joint_rad["thumb_cmc_yaw"] - ) == pytest.approx(3.803, abs=0.01) - - -def test_right_19_still_rejects_gross_cross_view_cone_mismatch() -> None: - injected = [0.0] * 16 - _, result = _solve_synthetic_offsets( - "right", - injected, - layout_id="g20_right_19", - inject_observer_cone_bias_degrees=16.0, - maximum_systematic_axis_cone_bias_degrees=15.0, - ) - - assert result.passed is False - assert result.failure_reasons["thumb_cmc_yaw"] == ( - "zero_axis_cone_mismatch_too_large" - ) - - -def test_right_19_finger_roll_common_gauge_is_removed_before_limits() -> None: - zero = get_zero_calibration_profile("right", "g20_right_19") - injected = [0.0] * len(zero.direct_zero_joints) - deviations = { - "index_mcp_roll": -0.4, - "middle_mcp_roll": 0.7, - "ring_mcp_roll": 0.1, - "pinky_mcp_roll": -0.1, - } - for name, deviation in deviations.items(): - injected[zero.direct_zero_joints.index(name)] = 18.0 + deviation - - _, result = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_19" - ) - - assert result.passed is True - common = float(np.median(list(deviations.values()))) - for name, deviation in deviations.items(): - expected = deviation - common - assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( - expected, abs=0.05 - ) - - -def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None: - _, result = _solve_synthetic_offsets( - "right", - [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], - pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002}, - ) - - assert result.passed is False - assert result.failure_reasons["thumb_cmc_pitch"] == ( - "zero_phase_axis_line_residual_too_large" - ) - - -def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None: - zero = get_zero_calibration_profile("left") - baseline = [255.0] * 20 - baseline[6:10] = [127.0] * 4 - curves = { - name: _synthetic_curve( - int(baseline[JOINT_SPECS[name].motor_index]), - math.radians(50.0), - ) - for name in MEASURED_JOINTS - } - motor_by_joint = { - name: spec.motor_index for name, spec in JOINT_SPECS.items() - } - true_offsets = { - name: math.radians(value) - for name, value in zip( - DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0] - ) - } - model = UrdfKinematicModel(SOURCE_URDF) - base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) - base_translation = np.asarray([0.31, -0.19, 0.72]) - measurements = [] - yaw_axis_without_clearance = None - yaw_axis_with_clearance = None - for cycle in range(3): - for joint in AXIS_JOINTS: - state = list(baseline) - if joint == "thumb_cmc_yaw": - state[5] = 145.0 - angles = _angles_from_state( - state, curves=curves, motor_by_joint=motor_by_joint - ) - axis, point = model.axis_line( - joint, - zero_offsets=true_offsets, - joint_angles=angles, - ) - if joint == "thumb_cmc_yaw": - yaw_axis_with_clearance = axis.copy() - baseline_angles = _angles_from_state( - baseline, curves=curves, motor_by_joint=motor_by_joint - ) - yaw_axis_without_clearance = model.axis_line( - joint, - zero_offsets=true_offsets, - joint_angles=baseline_angles, - )[0] - measurements.append( - JointAxisMeasurement( - joint=joint, - cycle=cycle, - axis_common_xyz=tuple(base_rotation.apply(axis)), - point_common_xyz_m=tuple( - base_rotation.apply(point) + base_translation - ), - condition_state_u8=tuple(state), - plane_rms_m=0.0002, - radial_rms_m=0.0002, - rotation_circle_axis_difference_rad=math.radians(0.1), - ) - ) - - result = solve_urdf_zero_offsets( - source_urdf=SOURCE_URDF, - measurements=measurements, - curves=curves, - motor_by_joint=motor_by_joint, - ) - - assert math.degrees( - math.acos( - np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0) - ) - ) > 1.0 - assert result.passed is True - static_policy = { - **zero.fixed_direct_zero_offsets_rad, - **zero.static_output_zero_offsets_rad, - } - for name, expected in true_offsets.items(): - if name in static_policy: - expected = static_policy[name] - assert result.direct_offsets_rad[name] == pytest.approx( - expected, abs=math.radians(0.05) - ) - assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS) - assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx( - 0.0, abs=math.radians(0.05) - ) - for target in INHERITED_ZERO_JOINTS: - assert result.all_active_offsets_rad[target] == pytest.approx( - 0.0, abs=1.0e-12 - ) - assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys() - - -def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None: - hand = get_hand_calibration_profile("right") - zero = get_zero_calibration_profile("right") - baseline = [255.0] * 20 - baseline[6:10] = [127.0] * 4 - curves = { - name: _synthetic_curve( - int(baseline[hand.joint_specs[name].motor_index]), - math.radians(50.0), - ) - for name in hand.measured_joints - } - motor_by_joint = { - name: spec.motor_index for name, spec in hand.joint_specs.items() - } - true_offsets = { - name: math.radians(value) - for name, value in zip( - zero.direct_zero_joints, - [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], - ) - } - model = UrdfKinematicModel(RIGHT_SOURCE_URDF) - base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) - base_translation = np.asarray([0.31, -0.19, 0.72]) - measurements: list[JointAxisMeasurement] = [] - for cycle in range(3): - states: dict[str, list[float]] = {} - lines: dict[str, tuple[np.ndarray, np.ndarray]] = {} - for joint in zero.axis_joints: - state = list(baseline) - if joint == "thumb_cmc_yaw": - state[5] = 145.0 - states[joint] = state - angles = _angles_from_state( - state, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=zero.inherited_zero_joints, - ) - lines[joint] = model.axis_line( - joint, - zero_offsets=true_offsets, - joint_angles=angles, - ) - original_lines = { - name: (axis.copy(), point.copy()) - for name, (axis, point) in lines.items() - } - for observer, parent in zero.phase_parent_joint.items(): - parent_axis, parent_point = lines[parent] - original_parent_axis, original_parent_point = original_lines[parent] - child_axis, original_child_point = original_lines[observer] - radial = original_child_point - original_parent_point - radial -= original_parent_axis * float( - radial @ original_parent_axis - ) - # Preserve angular phase while deliberately corrupting link radius - # and along-axis depth. These components must not move a zero. - lines[observer] = ( - child_axis, - parent_point + 1.25 * radial + 0.02 * parent_axis, - ) - for joint in zero.axis_joints: - axis, point = lines[joint] - measurements.append( - JointAxisMeasurement( - joint=joint, - cycle=cycle, - axis_common_xyz=tuple(base_rotation.apply(axis)), - point_common_xyz_m=tuple( - base_rotation.apply(point) + base_translation - ), - condition_state_u8=tuple(states[joint]), - plane_rms_m=0.0002, - radial_rms_m=0.0002, - rotation_circle_axis_difference_rad=math.radians(0.1), - ) - ) - - result = solve_urdf_zero_offsets( - source_urdf=RIGHT_SOURCE_URDF, - measurements=measurements, - curves=curves, - motor_by_joint=motor_by_joint, - hand_type="right", - ) - - assert result.passed is True - static_policy = { - **zero.fixed_direct_zero_offsets_rad, - **zero.static_output_zero_offsets_rad, - } - for name, expected in true_offsets.items(): - if name in static_policy: - expected = static_policy[name] - assert result.direct_offsets_rad[name] == pytest.approx( - expected, abs=math.radians(0.05) - ) - for target in zero.inherited_zero_joints: - assert result.all_active_offsets_rad[target] == pytest.approx( - 0.0, abs=1.0e-12 - ) - assert set(result.all_active_offsets_rad) == set(hand.active_joints) diff --git a/src/linkerhand_calibration/test/test_virtual_control_contract.py b/src/linkerhand_calibration/test/test_virtual_control_contract.py deleted file mode 100644 index f964df3..0000000 --- a/src/linkerhand_calibration/test/test_virtual_control_contract.py +++ /dev/null @@ -1,84 +0,0 @@ -"""A renamed rig and reordered SDK still use the shared capture/artifact chain.""" - -from dataclasses import replace -import hashlib - -import pytest - -from linkerhand_calibration.core.domain.profile import ProfileKey -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel -from linkerhand_calibration.profiles import dump_hand_profile, load_hand_profile -from linkerhand_calibration.runtime.adapters.base import HardwareHealth -from linkerhand_calibration.runtime.adapters.legacy_byte_sdk import LegacyByteSdkAdapter -from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session -from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper -from engine_capture_fixture import collect_with_engine -from test_profile_finalization import virtual_capture - - -def test_reordered_channels_views_tasks_and_same_sdk_command_to_exported_fk(tmp_path): - original, source, rows, _ = virtual_capture(tmp_path, "l6_right_8") - order = (4, 2, 0, 5, 1, 3) - channel = {old: new for new, old in enumerate(order)} - views = dict(zip(original.vision.view_names, ("inspection", "oblique", "overhead"))) - def vector(values): - return tuple(values[index] for index in order) if values else () - def groups(values): - return tuple(tuple(channel[index] for index in group) for group in values) - def commands(values): - return tuple((channel[index], value) for index, value in values) - layout = original.command - layout = replace(layout, - **{name: vector(getattr(layout, name)) for name in ( - "names", "baseline_u8", "baseline", "lower_bounds", "upper_bounds", - "feedback_lower_bounds", "feedback_upper_bounds", "sdk_to_joint_direction", "maximum_velocity")}, - command_index_by_joint={joint: channel[index] for joint, index in layout.command_index_by_joint.items()}, - disabled_indices=frozenset(channel[index] for index in layout.disabled_indices), - speed_slot_by_command_index={channel[index]: slot for index, slot in layout.speed_slot_by_command_index.items()}) - profile = replace(original, key=ProfileKey("VIRTUAL", "right", "reordered_interfaces"), - namespace="/virtual_interfaces", command=layout, - vision=replace(original.vision, - views=tuple(replace(view, name=views[view.name]) for view in reversed(original.vision.views)), - extrinsic_reference_view=views[original.vision.extrinsic_reference_view]), - measurement=replace(original.measurement, measurements={name: replace(spec, - view=None if spec.view is None else views[spec.view]) for name, spec in original.measurement.measurements.items()}), - motion=replace(original.motion, tasks=tuple(replace(task, - command_index=channel[task.command_index], view=views[task.view], - auxiliary_commands=commands(task.auxiliary_commands), preparation_groups=groups(task.preparation_groups), - entry_waypoints=tuple(commands(waypoint) for waypoint in task.entry_waypoints)) - for task in reversed(original.motion.tasks)), return_groups=groups(original.motion.return_groups), - resume_verification_waypoints=tuple(replace(waypoint, command=vector(waypoint.command), - tag_ids_by_view={views[view]: ids for view, ids in waypoint.tag_ids_by_view.items()}) - for waypoint in original.motion.resume_verification_waypoints))) - path = tmp_path/"virtual_profile.yaml" - path.write_text(dump_hand_profile(profile)) - profile = load_hand_profile(path) - observations = [{**row, "view": views[row["view"]], - "state_u8": vector(row["state_u8"]), - "command_vector_u8": vector(row["command_vector_u8"])} for row in rows] - _, records = collect_with_engine(profile, observations) - hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} - hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() - payload, _, correction = finalize_profile_session(profile=profile, session_dir=tmp_path/"session", - serial_number="VIRTUAL_ONLY", source_urdf=source, protected_inputs=hashes, - records=records, standard_loader=lambda _: None) - mapper = load_unified_mapper(tmp_path/"session/release_manifest.json") - model = UrdfKinematicModel(correction.path) - writes = [] - sdk = LegacyByteSdkAdapter(profile.command, publish=writes.append, - set_speed_callback=lambda *_: None, health_callback=lambda: HardwareHealth(True, True)) - for values in ((31, 63, 95, 127, 159, 191), (191, 159, 127, 95, 63, 31)): - sdk.publish_position(values) - assert writes[-1] == tuple(values) - feedback = sdk.parse_feedback(tuple(reversed(profile.command.names)), tuple(reversed(values))) - assert feedback == tuple(values) - angles = dict(zip(mapper.urdf_joint_names, mapper.map_positions(values))) - # The identical native SDK vector indexes the certified JSON and FK. - direct = {joint: table["angle_rad"][values[table["sdk_channel"]]] - for joint, table in payload["joints"].items()} - assert angles == pytest.approx(direct, abs=1e-9) - active = {joint: value for joint, value in direct.items() if model.joints[joint].mimic_joint is None} - resolved = model.resolve_angles(active) - assert {joint: resolved[joint] for joint in direct} == pytest.approx(direct, abs=1e-9) - for joint, index in profile.command.command_index_by_joint.items(): - assert payload["joints"][joint]["sdk_channel"] == index diff --git a/src/linkerhand_calibration/test/test_visual_motion.py b/src/linkerhand_calibration/test/test_visual_motion.py deleted file mode 100644 index 3069117..0000000 --- a/src/linkerhand_calibration/test/test_visual_motion.py +++ /dev/null @@ -1,266 +0,0 @@ -"""Explicit vision policy remains supported independently of O30's default.""" - -from dataclasses import replace -from types import SimpleNamespace -import copy -import json - -import numpy as np -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.adapters import HardwareHealth -from linkerhand_calibration.runtime.adapters.command_seed import read_o30_command, load_command_seed -from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution -from linkerhand_calibration.runtime.safety import SafetyPolicy, SafetySample -from linkerhand_calibration.runtime.visual_motion import VisualMotionObserver, validate_visual_motion_evidence - - -@pytest.fixture -def profile(): - return as_vision(load_bundled_hand_profile("o30_right_18", scope="full")) - - -def as_vision(profile): - return replace(profile, acquisition=replace(profile.acquisition, motion_observation="vision")) - - -def observe(observer, joint, now, value, command, *, version=1): - spec = observer.specs[joint] - parent = np.array([[100.,100.],[120.,100.],[120.,120.],[100.,120.]]) - corners = {spec.parent_role: parent, spec.child_role: parent+[40.+value/5., 0.]} - observer.observe(spec.view, int((now+100)*1e9), corners, - now=now, epoch=1, version=version, command=command) - - -def execution(profile, *, phase="prepare", start_value=0, target_value=80, returns=()): - initial = list(profile.command.baseline_values) - initial[0] = start_value - target = initial.copy() - target[0] = target_value - observer = VisualMotionObserver(profile) - observe(observer, "thumb_cmc_roll", 0., start_value, initial, version=0) - command = MotionCommand(phase, tuple(target), 200., command_index=0, - observed_joints=("thumb_cmc_roll",), visual_return_joints=returns) - assert observer.begin(command, initial, now=0., version=1) - segment = MotionExecution(profile, command, initial_command=initial, - initial_feedback=(), now=0., identity="vision", visual_observer=observer) - return observer, segment - - -@pytest.mark.parametrize("feedback", [(), (0.,)*20, (999.,)*20]) -def test_real_image_motion_completes_with_missing_stuck_or_bad_feedback(profile, feedback): - observer, segment = execution(profile) - safety = SafetyPolicy(profile) - for tick in range(240): - now = tick/30 - command = segment.sample(now) - observe(observer, "thumb_cmc_roll", now, command[0], command) - segment.observe(feedback, stamp=None, now=now) - assert safety.evaluate(SafetySample(now, None, command, feedback, - HardwareHealth(True, True, feedback_fresh=False))).safe - assert not observer.check(segment, now) - assert segment.arrived(now) - - -def test_command_alone_cannot_prove_actual_movement(profile): - observer, segment = execution(profile) - for tick in range(240): - now = tick/30 - command = segment.sample(now) - observe(observer, "thumb_cmc_roll", now, 0., command) - segment.observe((), stamp=None, now=now) - reason = observer.check(segment, now) - if reason: - assert reason == "visual_motion_not_observed:thumb_cmc_roll" - assert not segment.arrived(now) - break - else: - pytest.fail("static images passed a significant motion request") - - -def test_missing_tag_and_device_faults_still_stop(profile): - observer, segment = execution(profile) - assert observer.check(segment, 1.01).startswith("visual_motion_unavailable") - base = SafetySample(3., None, profile.command.baseline_values, None, HardwareHealth(True, True)) - for changed, code in [ - (replace(base, health=HardwareHealth(False, True)), "sdk_disconnected"), - (replace(base, health=HardwareHealth(True, True, active_faults=("overcurrent",))), "hardware_fault"), - (replace(base, competing_controller=True), "duplicate_controller"), - (replace(base, reference_moved=True), "fixed_reference_moved"), - (replace(base, operator_abort=True), "operator_abort"), - (replace(base, command=(999.,)*20), "physical_range_exceeded")]: - assert SafetyPolicy(profile).evaluate(changed).code == code - - -def test_group_witnesses_and_middle_only_segment(profile): - observer = VisualMotionObserver(profile) - start = list(profile.command.baseline_values) - start[2:6] = [0,80,0,0] - target = start.copy() - target[2:6] = [255]*4 - group = next(t for t in profile.motion.tasks if t.key == 'fingers_mcp_roll_front') - motion = MotionCommand("sweep", tuple(target), 200., observed_joints=group.joints) - names = observer.names_for(motion, start) - assert set(names) == set(group.joints) - for name in names: - if name != "ring_mcp_roll": - observe(observer, name, 0., 0., start) - assert not observer.available(names, 0.) - lower = start.copy() - lower[3] = 0 - independent = MotionCommand("sweep", tuple(lower), 200., observed_joints=("middle_mcp_roll",)) - assert observer.names_for(independent, start) == ("middle_mcp_roll",) - - -def test_visual_zero_return_does_not_release_avoidance_early(profile): - observer, segment = execution(profile, phase="task_exit", start_value=255, target_value=0, - returns=("thumb_cmc_roll",)) - pose = segment.command.target - observe(observer, "thumb_cmc_roll", .01, 0., pose) - observer.zeros[("thumb_cmc_roll", pose)] = observer.latest["thumb_cmc_roll"] - for tick in range(1, 470): - now = tick/30 - command = segment.sample(now) - observe(observer, "thumb_cmc_roll", now, max(40, command[0]), command) - segment.observe((), stamp=None, now=now) - observer.check(segment, now) - assert not segment.arrived(now) - for tick in range(470, 490): - now = tick/30 - observe(observer, "thumb_cmc_roll", now, 0., pose) - observer.check(segment, now) - assert segment.arrived(now) - - -def stable_records(profile): - observer, segment = execution(profile, phase="steady") - records = observer.drain() - for tick in range(220): - now = tick/30 - command = segment.sample(now) - observe(observer, "thumb_cmc_roll", now, command[0], command) - segment.observe((), stamp=None, now=now) - assert not observer.check(segment, now) - evidence = observer.settle_record(segment.finished_at, now, epoch=1) - records.extend(observer.drain()) - assert evidence and segment.steady_ready(now) - records.append(dict(kind="joint_sample", joint="thumb_cmc_roll", sample_phase="steady", - image_stamp_ns=int((now+100)*1e9), command_vector_u8=list(command), - visual_settled_evidence_id=evidence, feedback_u8=None, state_u8=None)) - return records - - -def test_stability_evidence_replays_original_pixels_and_rejects_tampering(profile): - rows = stable_records(profile) - validate_visual_motion_evidence(profile, rows) - wrong_image = copy.deepcopy(rows) - wrong_image[-1]["image_stamp_ns"] += 1 - with pytest.raises(ValueError, match="visual_sample_image_changed"): - validate_visual_motion_evidence(profile, wrong_image) - missing = [r for r in rows if r.get("kind") != "visual_motion_observation"] - with pytest.raises(ValueError, match="source_missing"): - validate_visual_motion_evidence(profile, missing) - wrong_command = copy.deepcopy(rows) - wrong_command[-1]["command_vector_u8"][0] = 81 - with pytest.raises(ValueError, match="visual_sample_command_changed"): - validate_visual_motion_evidence(profile, wrong_command) - - -def test_unstable_and_duplicate_images_cannot_open_steady_window(profile): - observer, segment = execution(profile, phase="steady", target_value=0) - for tick in range(100): - now = tick/30 - command = segment.sample(now) - observe(observer, "thumb_cmc_roll", now, 20*(tick % 2), command) - segment.observe((), stamp=None, now=now) - assert not segment.steady_ready(now) - latest = observer.latest["thumb_cmc_roll"] - observer.observe("front", latest.stamp_ns, {}, now=100., epoch=1, version=1, command=command) - assert not observer.available(("thumb_cmc_roll",), 100.) - - -def test_startup_reads_target_register_without_position_or_write_access(profile, tmp_path): - calls = [] - class Controller: - hand_info = {"产品型号": "O30", "左右手": "RIGHT", "设备唯一标识": "fixture"} - def __init__(self, **kwargs): - calls.append(kwargs) - def get_target_position(self): - calls.append("target") - return list(profile.command.baseline_values) - def close(self): - calls.append("close") - seed = read_o30_command(side="right", controller_factory=Controller) - assert calls[1:] == ["target", "close"] - assert calls[0]["probe_sensor"] is False - path = tmp_path/"seed.json" - path.write_text(json.dumps(seed)) - assert load_command_seed(path, profile) == (profile.command.baseline_values, "fixture") - seed["values"][0] = 999 - path.write_text(json.dumps(seed)) - with pytest.raises(ValueError, match="initial_command_seed_invalid"): - load_command_seed(path, profile) - - -def test_coordinator_starts_without_feedback_and_uses_only_target_seed(tmp_path): - from runtime_host_fixture import coordinator_fixture - host, clock = coordinator_fixture(tmp_path, model="o30", profile_transform=as_vision) - try: - for view in host.profile.vision.view_names: - host.cameras.matrices[view] = np.eye(3) - host.cameras.image_sizes[view] = (640, 480) - host.cameras.info_received_at[view] = clock.now - host.cameras.detections_received_at[view] = clock.now - assert not host.state_history - host.tick() - assert host.start().success - host.tick() - clock.now += float(host.parameters.speed_settle_seconds)+.01 - host.tick() - assert clock.positions[-1] == host.parameters.initial_command - assert not host.state_history - # An unavailable/invalid position packet is diagnostic only. - host.receive_feedback(host.command_names, [999.]*20, host.ports.clock_ns()) - host.tick() - assert host.state not in {"PAUSED", "FAILED"} - clock.now += 3.1 - host.tick() - assert host.state == "PAUSED" - assert "sdk_disconnected" in host.reason - finally: - host.close() - - -def test_image_capture_keeps_absent_feedback_explicit(tmp_path): - from runtime_host_fixture import coordinator_fixture - from linkerhand_calibration.runtime.capture import CaptureFrame - from linkerhand_calibration.runtime.image_capture import image_observation_record - host, _ = coordinator_fixture(tmp_path, model="o30", profile_transform=as_vision) - try: - task = host.profile.motion.tasks[0] - frame = CaptureFrame(task.view, 123, np.eye(3), {}, None, - host.profile.command.baseline_values, command_skew_ns=1000) - motion = MotionCommand("sweep", frame.command, 200., task_key=task.key, - command_index=0, cycle=0, direction="increasing") - row = image_observation_record(host.profile, frame, motion) - assert row["feedback_vector"] is None - assert row["state_image_sync_error_ns"] is None - assert row["command_image_skew_ns"] == 1000 - assert row["command_vector"] == list(frame.command) - finally: - host.close() - - -@pytest.mark.parametrize("model", ["o6", "o12", "l6", "g20"]) -def test_existing_models_keep_position_feedback_requirement(model): - # Registered layout names differ, so discover through the product contract. - from pathlib import Path - from linkerhand_calibration.product import load_product_config - package = Path(__file__).resolve().parents[1] - config = load_product_config(package/f"config/{model}_right_product.yaml", - workspace=package.parents[1], check_can=False) - profile = config.calibration_contract.typed_profile - assert not profile.vision_motion - sample = SafetySample(3., None, profile.command.baseline_values, None, HardwareHealth(True, True)) - assert SafetyPolicy(profile).evaluate(sample).code == "feedback_stale" diff --git a/src/linkerhand_calibration/test/test_witness_endpoint.py b/src/linkerhand_calibration/test/test_witness_endpoint.py deleted file mode 100644 index 9462845..0000000 --- a/src/linkerhand_calibration/test/test_witness_endpoint.py +++ /dev/null @@ -1,104 +0,0 @@ -"""A real terminal command gap must delay departure, not fabricate evidence.""" -from copy import deepcopy -from dataclasses import replace -import gzip -import json -from pathlib import Path -from types import SimpleNamespace - -import pytest - -from linkerhand_calibration.core.domain.capture_plan import configure_training -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.profiles.preparation import configure_preparation -from linkerhand_calibration.runtime.motion_execution import MotionCommand -from linkerhand_calibration.runtime.preparation_witness import ( - PreparationWitnessCapture, WitnessInput, stationary_witness_endpoint, select_witness_rows, -) - - -@pytest.fixture -def capture(): - rows = json.loads(gzip.decompress((Path(__file__).parent/'fixtures/o30_witness_terminal_gap.json.gz').read_bytes())) - profile = configure_preparation(configure_training(load_bundled_hand_profile('o30_right_18'), - 'staged_2_to_3'), 'occlusion_aware_witness_v1') - owner = SimpleNamespace(profile=profile) - witness = PreparationWitnessCapture(owner) - for row in rows: - witness.observe(row) - motion = MotionCommand('zero_approach', profile.motion.preparation_witnesses['pinky_mcp_pitch'].command, - 200., task_key='pinky_mcp_pitch_side', reference_joints=('pinky_mcp_pitch',), - reference_path_index=2, preparation_witness_joint='pinky_mcp_roll') - return profile, rows, witness, motion - - -def extra_frame(rows, index): - """Synthetic later receipt for the acquisition gate only, never release.""" - row = deepcopy(rows[-1]) - row['image_stamp_ns'] += index*33_333_333 - for tag in row['tags'].values(): - tag['candidate_diagnostics']['observation_stamp_ns'] = row['image_stamp_ns'] - return row - - -@pytest.mark.replay -def test_real_three_missing_commands_leave_only_two_terminal_images(capture): - profile, rows, witness, motion = capture - assert len(rows) == 226 - assert all(row['command_vector'] is None for row in rows[-5:-2]) - assert witness.endpoint_error(motion, 1, rows[-1]['image_stamp_ns']) == 'preparation_witness_stationary_endpoint_missing' - with pytest.raises(ValueError, match='stationary_endpoint_missing'): - stationary_witness_endpoint(profile, 'pinky_mcp_pitch', rows) - source = WitnessInput('pinky_mcp_pitch', 'pinky_mcp_roll', tuple(rows), (), (5, 10, 14)) - with pytest.raises(ValueError, match='stationary_endpoint_missing'): - select_witness_rows(profile, source) - - -def test_wait_collects_new_images_and_live_and_replay_use_same_window(capture): - profile, rows, witness, motion = capture - original = deepcopy(rows) - for index in range(1, 9): - row = extra_frame(original, index) - witness.observe(row) - rows.append(row) - if index < 8: - assert witness.endpoint_error(motion, 1, row['image_stamp_ns']) - assert witness.endpoint_error(motion, 1, rows[-1]['image_stamp_ns']) == '' - tail = stationary_witness_endpoint(profile, 'pinky_mcp_pitch', rows) - assert len(tail) == 10 - assert rows[:len(original)] == original - source = WitnessInput('pinky_mcp_pitch', 'pinky_mcp_roll', tuple(rows), (), (5, 10, 14)) - _, replayed_tail = select_witness_rows(profile, source) - assert replayed_tail == tail - - -@pytest.mark.parametrize('fault', ['stale', 'missing_command', 'unstable_feedback', 'epoch', 'tag']) -def test_terminal_gate_cannot_accept_invalid_or_old_evidence(capture, fault): - profile, rows, witness, motion = capture - extra = [extra_frame(rows, index) for index in range(1, 12)] - if fault == 'missing_command': - extra[-1]['command_vector'] = None - elif fault == 'unstable_feedback': - extra[-1]['feedback_vector'][5] += 10 - elif fault == 'tag': - del extra[-1]['tags']['pinky_pip'] - for row in extra: - witness.observe(row) - now = extra[-1]['image_stamp_ns'] - assert witness.endpoint_error(motion, 2 if fault == 'epoch' else 1, - now+2_000_000_000 if fault == 'stale' else now) - assert witness.endpoint_error(replace(motion, reference_path_index=1), 1, now) == '' - - -def test_coordinator_holds_with_fixed_deadline_and_never_starts_next_motion(capture): - from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator - profile, rows, witness, motion = capture - pauses = [] - host = SimpleNamespace(profile=profile, branch_initialization=SimpleNamespace(witness_capture=witness), - ports=SimpleNamespace(clock_ns=lambda: rows[-1]['image_stamp_ns']), - _observation_epoch=1, segment=SimpleNamespace(finished_at=10.), _pause=pauses.append) - assert not CalibrationCoordinator._witness_departure_ready(host, motion, 11.) - assert not pauses - assert not CalibrationCoordinator._witness_departure_ready(host, motion, 10.+profile.acquisition.steady_timeout_seconds) - assert pauses == ['preparation_witness_stationary_endpoint_missing'] - assert host.segment.finished_at == 10. diff --git a/src/linkerhand_calibration/test/test_zero_approach_path.py b/src/linkerhand_calibration/test/test_zero_approach_path.py deleted file mode 100644 index 537031a..0000000 --- a/src/linkerhand_calibration/test/test_zero_approach_path.py +++ /dev/null @@ -1,105 +0,0 @@ -"""Full pre-zero paths retain bounded, source-bound geometry evidence.""" -from copy import deepcopy -from dataclasses import replace -import json - -import pytest - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.runtime.branch_initialization import BranchInitialization, solve_initialization -from linkerhand_calibration.runtime.motion_provenance import _initializations -from test_branch_initialization import arrival - - -def path_fixture(): - profile = load_bundled_hand_profile('o6_right_8') - approach, zero, templates = arrival(profile) - name = zero.zero_joints[0] - spec = profile.motion.joint_zero_references[name] - channel = profile.command.command_index_by_joint[name] - def pose(value): - values = list(spec.command); values[channel] = value - return tuple(values) - spec = replace(spec, command=pose(96), approach_commands=(pose(0), pose(255), pose(0)), evidence_scope='path') - references = dict(profile.motion.joint_zero_references); references[name] = spec - profile = replace(profile, motion=replace(profile.motion, joint_zero_references=references)) - zero = replace(zero, target=spec.command) - stages, stamp = [], 1000 - for index, order in ((0, range(len(templates))), (1, range(len(templates))), - (2, reversed(range(len(templates)))), (3, range(len(templates)//3))): - rows = [] - for i in order: - row = deepcopy(templates[i]); stamp += 1 - row.update(image_stamp_ns=stamp, motion_version=10+index, - reference_path_index=index, sample_phase='zero_approach', - command_vector=list(pose(255*i/(len(templates)-1))), - feedback_vector=list(pose(255*i/(len(templates)-1)))) - for item in row['tags'].values(): - item['candidate_diagnostics']['observation_stamp_ns'] = stamp - rows.append(row) - stages.append((replace(approach, target=(*spec.approach_commands, spec.command)[index], - reference_path_index=index), rows)) - return profile, zero, stages - - -def collect(profile, zero, stages): - owner = BranchInitialization(profile, image_geometry=False) - for motion, rows in stages: - owner.begin(motion, session_epoch=1, motion_version=rows[0]['motion_version']) - for row in rows: - owner.observe(row) - owner.begin(zero, session_epoch=1, motion_version=14) - return owner, owner.request(stages[0][1][0]['view'], zero, session_epoch=1, motion_version=14) - - -def test_full_path_keeps_full_motion_and_separate_source_versions_after_short_arrival(): - profile, zero, stages = path_fixture() - owner, request = collect(profile, zero, stages) - assert len(request.frames) <= 129 - assert set(dict(request.source_motion_versions).values()) == {12, 13} - assert not set(frame.stamp_ns for frame in request.frames) & { - row['image_stamp_ns'] for row in stages[0][1]} - result = solve_initialization(request) - assert result.resolved, result.reason - _, report = owner.accept(request, result) - assert report['schema_version'] == 2 - sources = [row for _, rows in stages for row in rows] - # Serialized multi-segment provenance must pass the same final reader. - journal = json.loads(json.dumps([*sources, report])) - assert _initializations(journal) - changed = deepcopy(journal) - source = next(row for row in changed if row.get('image_stamp_ns') in report['source_image_stamps']) - source['motion_version'] = 10 - with pytest.raises(ValueError, match='source_image_missing_or_changed'): - _initializations(changed) - - -@pytest.mark.parametrize('mutation', ['skip_start', 'skip_leg', 'new_epoch', 'held_joint_moved', 'stale_frame']) -def test_path_cannot_mix_incomplete_different_epoch_or_stale_motion(mutation): - profile, zero, stages = path_fixture() - owner = BranchInitialization(profile, image_geometry=False) - selected = stages[1:] if mutation == 'skip_start' else stages - if mutation == 'skip_leg': selected = [stages[0], *stages[2:]] - for motion, rows in selected: - epoch = 2 if mutation == 'new_epoch' and motion.reference_path_index >= 2 else 1 - owner.begin(motion, session_epoch=epoch, motion_version=rows[0]['motion_version']) - for row in rows: - row = deepcopy(row) - if mutation == 'held_joint_moved' and motion.reference_path_index == 1: - row['feedback_vector'][0] += 5 if row['image_stamp_ns'] % 2 else 0 - if mutation == 'stale_frame': row['motion_version'] -= 1 - row['session_epoch'] = epoch - owner.observe(row) - owner.begin(zero, session_epoch=epoch, motion_version=14) - request = owner.request(stages[0][1][0]['view'], zero, session_epoch=epoch, motion_version=14) - assert not solve_initialization(request).resolved - - -def test_new_retry_discards_previous_preparation_evidence(): - profile, zero, stages = path_fixture() - owner, request = collect(profile, zero, stages) - assert request.frames - first, rows = stages[0] - owner.begin(first, session_epoch=2, motion_version=20) - assert owner.accept(request, solve_initialization(request)) is None - assert owner._frames == {} and owner.reports == {} diff --git a/src/linkerhand_calibration/test/test_zero_calibration.py b/src/linkerhand_calibration/test/test_zero_calibration.py deleted file mode 100644 index e457ec2..0000000 --- a/src/linkerhand_calibration/test/test_zero_calibration.py +++ /dev/null @@ -1,340 +0,0 @@ -from __future__ import annotations - -import math - -import cv2 -from linkerhand_calibration.zero_calibration import ( - build_trajectory_zero_angle_payload, - build_trajectory_zero_travel_payload, - circular_median_rad, - detect_reference_alignment_line, - fit_image_circle_trajectory, - measure_zero_from_circle, - signed_angle_difference_rad, - summarize_zero_frames, - validate_zero_angle_payload, - validate_zero_travel_payload, -) -import numpy as np -import pytest - - -def _detect_reference_line(image: np.ndarray, reference_y: float) -> dict | None: - return detect_reference_alignment_line( - image, - reference_y_px=reference_y, - roi_y_min_ratio=0.55, - roi_y_max_ratio=0.98, - minimum_length_ratio=0.30, - maximum_candidate_angle_rad=math.radians(15.0), - ) - - -def _frame( - table_angle_rad: float, - *, - state_u8: float = 255.0, - t0_xy: tuple[float, float] = (500.0, 300.0), - circle_xy: tuple[float, float] = (-120.0, 80.0), - radius_px: float = 90.0, -) -> dict[str, float]: - # table_angle_rad uses a y-up convention while image y grows downwards. - relative = np.asarray(circle_xy) + radius_px * np.asarray( - [math.cos(table_angle_rad), -math.sin(table_angle_rad)] - ) - t0 = np.asarray(t0_xy) - t3 = t0 + relative - return { - "state_u8": state_u8, - "t0_x_px": float(t0[0]), - "t0_y_px": float(t0[1]), - "t3_x_px": float(t3[0]), - "t3_y_px": float(t3[1]), - } - - -def _trajectory(noise_px: float = 0.15) -> list[dict[str, float]]: - rng = np.random.default_rng(7) - observations: list[dict[str, float]] = [] - for states in ( - np.linspace(255.0, 64.0, 70), - np.linspace(64.0, 255.0, 70), - ): - for state in states: - fraction = (255.0 - state) / (255.0 - 64.0) - angle = math.radians(10.0 + 65.0 * fraction) - shift = rng.normal(0.0, 0.35, size=2) - frame = _frame( - angle, - state_u8=float(state), - t0_xy=(500.0 + shift[0], 300.0 + shift[1]), - ) - frame["t3_x_px"] += float(rng.normal(0.0, noise_px)) - frame["t3_y_px"] += float(rng.normal(0.0, noise_px)) - observations.append(frame) - return observations - - -def _fit(observations: list[dict[str, float]] | None = None) -> dict: - return fit_image_circle_trajectory( - _trajectory() if observations is None else observations, - bin_size_u8=8.0, - minimum_frames=45, - minimum_bins=18, - minimum_state_span_u8=160.0, - minimum_radius_px=20.0, - minimum_arc_rad=math.radians(20.0), - maximum_radial_rms_px=2.0, - maximum_p95_radial_error_px=3.5, - ) - - -def _zero_summary(angle_rad: float) -> dict[str, float]: - frames = [ - _frame( - angle_rad + math.radians(index - 14.5) * 1.0e-4, - ) - for index in range(30) - ] - return summarize_zero_frames(frames) - - -def _measurement( - table_rad: float, - *, - radial_error_px: float = 0.2, -) -> dict[str, float]: - return { - "table_rad": table_rad, - "zero_radial_error_px": radial_error_px, - } - - -def _round( - table_rad: float, - *, - return_delta: float = 0.002, -) -> dict[str, dict[str, float]]: - return { - "zero_before": _measurement(table_rad), - "zero_after": _measurement( - table_rad + return_delta, - ), - } - - -def _travel_round( - zero_table_rad: float, - travel_rad: float, - *, - return_delta: float = 0.002, -) -> dict[str, dict[str, float]]: - round_value = _round( - zero_table_rad, - return_delta=return_delta, - ) - round_value["travel_endpoint"] = _measurement( - zero_table_rad + travel_rad, - ) - return round_value - - -def test_circular_statistics_cross_pi_without_jumping() -> None: - values = [math.radians(179.0), math.radians(-179.0), math.pi] - result = circular_median_rad(values) - assert abs(signed_angle_difference_rad(result, math.pi)) < math.radians(1.1) - - -def test_detect_reference_line_reports_signed_angle_and_offset() -> None: - height, width = 720, 1280 - reference_y = 0.90 * (height - 1) - expected_angle = math.radians(2.0) - image = np.zeros((height, width, 3), dtype=np.uint8) - half_span = 560.0 - vertical_change = math.tan(expected_angle) * half_span - cv2.line( - image, - (80, int(round(reference_y + vertical_change))), - (1200, int(round(reference_y - vertical_change))), - (255, 255, 255), - 5, - ) - - result = _detect_reference_line(image, reference_y) - - assert result is not None - assert result["angle_rad"] == pytest.approx(expected_angle, abs=0.004) - assert abs(result["vertical_offset_px"]) <= 5.0 - - -def test_detect_reference_line_rejects_non_horizontal_scene() -> None: - image = np.zeros((720, 1280, 3), dtype=np.uint8) - cv2.line(image, (640, 420), (640, 700), (255, 255, 255), 5) - - assert _detect_reference_line(image, 0.90 * 719.0) is None - - -def test_circle_fit_recovers_center_radius_and_rejects_anchor_translation() -> None: - fit = _fit() - assert fit["centre_relative_xy_px"] == pytest.approx( - [-120.0, 80.0], abs=0.8 - ) - assert fit["radius_px"] == pytest.approx(90.0, abs=0.8) - assert fit["arc_rad"] >= math.radians(60.0) - assert fit["radial_rms_px"] < 0.5 - assert fit["passed"] is True - - -def test_circle_fit_is_robust_to_sparse_bad_tag_centres() -> None: - observations = _trajectory() - for index in (9, 31, 57, 92, 121): - observations[index]["t3_x_px"] += 18.0 - observations[index]["t3_y_px"] -= 15.0 - fit = _fit(observations) - assert fit["centre_relative_xy_px"] == pytest.approx( - [-120.0, 80.0], abs=1.5 - ) - assert fit["radius_px"] == pytest.approx(90.0, abs=1.5) - - -def test_zero_uses_fixed_inward_radius_and_ignores_t3_rotation() -> None: - circle = _fit() - trajectory_angle = math.radians(10.0) - expected_table = math.radians(-170.0) - summary = _zero_summary(trajectory_angle) - summary_with_t3_forward = {**summary, "t3_rad": math.radians(31.0)} - summary_with_t3_flipped = { - **summary, - "t3_rad": math.radians(-149.0), - } - result = measure_zero_from_circle(circle, summary) - forward = measure_zero_from_circle(circle, summary_with_t3_forward) - flipped = measure_zero_from_circle(circle, summary_with_t3_flipped) - assert result["table_rad"] == pytest.approx(expected_table, abs=0.01) - assert forward == result - assert flipped == result - - -def test_circle_fit_rejects_short_state_span() -> None: - observations = [ - _frame(math.radians(10.0 + index * 0.1), state_u8=255.0 - index) - for index in range(50) - ] - with pytest.raises(ValueError, match="trajectory_state_span_too_small"): - _fit(observations) - - -def test_payload_remains_small_and_contains_circle_derived_angles() -> None: - rounds = [ - _round(-0.30), - _round(-0.299), - _round(-0.302), - ] - payload, report = build_trajectory_zero_angle_payload( - serial_number="G20_LEFT_TEST", - rounds=rounds, - trajectory_quality=_fit(), - maximum_round_difference_rad=math.radians(1.0), - maximum_return_error_rad=math.radians(1.0), - maximum_zero_radial_error_px=4.0, - detection_rate=0.99, - minimum_detection_rate=0.95, - ) - validate_zero_angle_payload(payload) - assert payload["schema_version"] == 2 - assert set(payload["zero_angles"]) == {"table_projected_zero_rad"} - assert payload["zero_angles"]["table_projected_zero_rad"] == pytest.approx( - -0.299 - ) - assert payload["quality"]["passed"] is True - assert report["measurement_method"] == ( - "t3_center_to_circle_centre_image_trajectory" - ) - - -@pytest.mark.parametrize(("detection_rate", "radial_error"), [(0.8, 0.2), (0.99, 8.0)]) -def test_quality_fails_for_detection_or_zero_circle_error( - detection_rate: float, radial_error: float -) -> None: - rounds = [ - { - "zero_before": _measurement( - -0.30, radial_error_px=radial_error - ), - "zero_after": _measurement( - -0.299, radial_error_px=radial_error - ), - } - for _ in range(3) - ] - payload, _ = build_trajectory_zero_angle_payload( - serial_number="G20_LEFT_TEST", - rounds=rounds, - trajectory_quality=_fit(), - maximum_round_difference_rad=math.radians(1.0), - maximum_return_error_rad=math.radians(1.0), - maximum_zero_radial_error_px=4.0, - detection_rate=detection_rate, - minimum_detection_rate=0.95, - ) - assert payload["quality"]["passed"] is False - - -def test_roll_payload_contains_zero_and_measured_travel() -> None: - rounds = [ - _travel_round(-0.30, 1.201), - _travel_round(-0.299, 1.199), - _travel_round(-0.302, 1.200), - ] - payload, report = build_trajectory_zero_travel_payload( - serial_number="G20_LEFT_TEST", - joint_name="thumb_cmc_roll", - zero_command_u8=255, - travel_endpoint_command_u8=0, - rounds=rounds, - trajectory_quality=_fit(), - maximum_round_difference_rad=math.radians(1.0), - maximum_travel_difference_rad=math.radians(1.0), - minimum_travel_rad=math.radians(20.0), - maximum_return_error_rad=math.radians(1.0), - maximum_zero_radial_error_px=4.0, - detection_rate=0.99, - minimum_detection_rate=0.95, - ) - - validate_zero_travel_payload(payload) - assert payload["joint"] == "thumb_cmc_roll" - assert payload["schema_version"] == 2 - assert payload["zero_command_u8"] == 255 - assert payload["travel_endpoint_command_u8"] == 0 - assert set(payload["zero_angles"]) == {"table_projected_zero_rad"} - assert payload["travel"]["signed_rad"] == pytest.approx(1.2, abs=0.003) - assert payload["travel"]["range_rad"] == pytest.approx(1.2, abs=0.003) - assert payload["quality"]["passed"] is True - assert report["signed_travel_rounds_rad"] == pytest.approx( - [1.2, 1.198, 1.199], abs=0.003 - ) - - -def test_roll_quality_rejects_inconsistent_or_short_travel() -> None: - rounds = [ - _travel_round(-0.30, travel) - for travel in (0.10, 0.12, 0.14) - ] - payload, _ = build_trajectory_zero_travel_payload( - serial_number="G20_LEFT_TEST", - joint_name="thumb_cmc_roll", - zero_command_u8=255, - travel_endpoint_command_u8=0, - rounds=rounds, - trajectory_quality=_fit(), - maximum_round_difference_rad=math.radians(1.0), - maximum_travel_difference_rad=math.radians(1.0), - minimum_travel_rad=math.radians(20.0), - maximum_return_error_rad=math.radians(1.0), - maximum_zero_radial_error_px=4.0, - detection_rate=0.99, - minimum_detection_rate=0.95, - ) - - assert payload["quality"]["passed"] is False diff --git a/src/linkerhand_calibration/test/test_zero_evidence_coverage.py b/src/linkerhand_calibration/test/test_zero_evidence_coverage.py deleted file mode 100644 index 7fb6378..0000000 --- a/src/linkerhand_calibration/test/test_zero_evidence_coverage.py +++ /dev/null @@ -1,58 +0,0 @@ -"""Missing absolute palm registration cannot be certified by flexion curves.""" -import json -from pathlib import Path -from types import SimpleNamespace - -import numpy as np -import pytest -from scipy.spatial.transform import Rotation - -from linkerhand_calibration.profiles import load_bundled_hand_profile -from linkerhand_calibration.core.fitting.session import compile_spatial_profile -from linkerhand_calibration.core.fitting.spatial import solve_urdf_zero_offsets -from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel - -SOURCE = Path(__file__).resolve().parents[1]/'urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf' -FLEXION = ('pinky_mcp_pitch', 'pinky_pip', 'pinky_dip') - - -@pytest.mark.parametrize('validation_cycle', [None, 3]) -def test_missing_axes_report_names_and_do_not_invent_training_or_holdout(validation_cycle): - profile = compile_spatial_profile(load_bundled_hand_profile('o30_right_18')) - # Coverage-only fixtures: execution must reject before touching geometry. - measurements = [SimpleNamespace(joint=j, cycle=c) for j in FLEXION for c in (0, 1)] - with pytest.raises(ValueError, match='axis measurements do not contain') as error: - solve_urdf_zero_offsets(source_urdf=SOURCE, measurements=measurements, - curves={}, motor_by_joint={}, zero_profile=profile, - training_cycles=(0, 1), validation_cycle=validation_cycle) - report = json.loads(str(error.value).split(':', 1)[1]) - assert set(report['training_missing']) == set(profile.axis_joints)-set(FLEXION) - assert set(report['training_missing_root_anchors']) == set(profile.root_anchor_joints) - assert report['training_unexpected'] == [] - assert set(report['validation_missing']) == (set(profile.axis_joints) if validation_cycle else set()) - - -def test_flexion_only_has_exact_mcp_zero_palm_registration_gauge(): - model = UrdfKinematicModel(SOURCE) - joint = FLEXION[0] - axis, point = model.axis_line(joint, zero_offsets={}, joint_angles={}, independent_mimic_angles=True) - offset = np.deg2rad(2.) - change = np.eye(4) - change[:3, :3] = Rotation.from_rotvec(axis*offset).as_matrix() - change[:3, 3] = point-change[:3, :3]@point - for q in (0., -.5, -1.): - angles = dict(zip(FLEXION, (q, q/2, q/3))) - for target in FLEXION: - actual = model.link_transform(target, zero_offsets={}, joint_angles=angles, - independent_mimic_angles=True) - shifted = model.link_transform(target, zero_offsets={joint: -offset}, joint_angles=angles, - independent_mimic_angles=True) - # Unknown palm registration absorbs an incorrect MCP zero exactly. - assert np.allclose(change@shifted, actual, atol=1e-12, rtol=0) - # With an independently fixed palm frame the same zero is detectable. - assert not np.allclose(shifted, actual, atol=1e-5, rtol=0) - # A fixed palm Tag is not a measured CAD mount. An unknown mount can absorb - # the registration change without changing its camera image either. - mount = np.eye(4) - mount[:3, 3] = [.02, .01, .03] - assert np.allclose(change@(np.linalg.inv(change)@mount), mount, atol=1e-12) diff --git a/src/linkerhand_calibration/test/test_zero_recovery.py b/src/linkerhand_calibration/test/test_zero_recovery.py deleted file mode 100644 index 2af489f..0000000 --- a/src/linkerhand_calibration/test/test_zero_recovery.py +++ /dev/null @@ -1,83 +0,0 @@ -"""Bounded fresh preparation, no replacement of already frozen joint zeros.""" - -from dataclasses import replace -from types import SimpleNamespace - -import numpy as np -import pytest - -from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import ImageMotionResolution -from linkerhand_calibration.runtime.branch_initialization import solve_initialization -from linkerhand_calibration.runtime.zero_recovery import ZeroRecovery -from runtime_host_fixture import coordinator_fixture, ready -from test_diagnostic_coordinator import run_until - - -def test_recovery_is_bounded_and_rejects_frozen_or_faulty_inputs(): - motion = SimpleNamespace(task_key="chain", zero_joints=("a", "b"), reference_reuse=False) - policy = ZeroRecovery() - reports = {"front": {"status": "unresolved", "reason": "image_motion_families_not_distinguishable"}} - kwargs = dict(geometry_error="ambiguous", reports=reports, referenced_joints={}) - assert policy.take(motion, **kwargs) is None # Identical motion adds no independent discriminator. - kwargs["reports"] = {"front": {"status": "unresolved", "reason": "image_motion_insufficient_independent_frames"}} - assert policy.take(motion, **{**kwargs, "referenced_joints": {"a": object()}}) is None - assert policy.take(motion, **{**kwargs, "reports": {"front": {"status": "unresolved", "reason": "motion_held_feedback_changed"}}}) is None - assert policy.take(motion, **kwargs) == "repeat_approach" - assert policy.take(motion, **kwargs) is None - assert policy.take(motion, geometry_error="", reports={}, referenced_joints={}) is None - - -@pytest.mark.parametrize("persistent", [False, True]) -@pytest.mark.parametrize("failure", ["model_frames", "source_frames", "source_candidates"]) -def test_transient_preparation_retries_locally_and_persistent_failure_stops(tmp_path, persistent, failure): - 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.]]) - requests = [] - def solver(request): - requests.append(request) - if persistent or len(requests) == 1: - if failure == "source_frames": - # Exercise the actual pre-solver validator's error vocabulary. - return solve_initialization(replace(request, frames=request.frames[:2], - image_frames=request.image_frames[:2])) - if failure == "source_candidates": - return ImageMotionResolution(False, "motion_candidates_missing_or_invalid") - return ImageMotionResolution(False, "image_motion_insufficient_independent_frames") - return solve_initialization(request) - host.initialization_solver = solver - assert host.start().success - try: - run_until(host, clock, lambda: host.state == "PAUSED" or bool(host.execution.zero_references)) - assert len(requests) == 2 - assert max(frame.stamp_ns for frame in requests[0].frames) < min(frame.stamp_ns for frame in requests[1].frames) - recoveries = [r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"] - assert len(recoveries) == 1 and recoveries[0]['failure_category'] == 'missing_samples' - if persistent: - assert host.state == "PAUSED" and not host.execution.zero_references - else: - assert host.state == "RUNNING" and "rh_thumb_cmc_yaw" in host.execution.zero_references - assert not host.finalization.started - finally: - host.close() - - -def test_zero_image_recovery_keeps_frozen_geometry_and_pose(tmp_path, monkeypatch): - 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.]]) - original = host._lock_joint_zeros - def delay_images(motion): - recovered = any(r.get("kind") == "joint_zero_recovery" for r in host.raw_records) - return original(motion) if recovered else False - monkeypatch.setattr(host, "_lock_joint_zeros", delay_images) - assert host.start().success - try: - run_until(host, clock, lambda: bool(host.execution.zero_references)) - recoveries = [r for r in host.raw_records if r.get("kind") == "joint_zero_recovery"] - assert len(recoveries) == 1 and recoveries[0]["action"] == "retry_zero_images" - assert len([r for r in host.raw_records if r.get("kind") == "motion_branch_initialization"]) == 1 - finally: - host.close() diff --git a/src/linkerhand_calibration/urdf/o30_left/linkerhand_O30i_left-V2_0819.urdf b/src/linkerhand_calibration/urdf/o30_left/linkerhand_O30i_left-V2_0819.urdf new file mode 100644 index 0000000..8304633 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o30_left/linkerhand_O30i_left-V2_0819.urdf @@ -0,0 +1,605 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o30_left/linkerhand_O30i_left-update.urdf b/src/linkerhand_calibration/urdf/o30_left/linkerhand_O30i_left-update.urdf new file mode 100644 index 0000000..6508de6 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o30_left/linkerhand_O30i_left-update.urdf @@ -0,0 +1,605 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/hand_base_link.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/hand_base_link.STL new file mode 100644 index 0000000..5b2192c Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/hand_base_link.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/index_distal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/index_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/index_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/index_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/index_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/index_middle.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/index_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/index_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/index_proximal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/index_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/middle_distal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/middle_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/middle_middle.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/middle_proximal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_distal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_middle.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_proximal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/ring_distal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/ring_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/ring_middle.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/ring_proximal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_distal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_distal.STL new file mode 100644 index 0000000..0dec965 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..af0e866 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..0a1a390 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_metacarpals_base2.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_proximal.STL b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_proximal.STL new file mode 100644 index 0000000..c517c21 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_left/meshes/thumb_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/o30_right_draft.urdf b/src/linkerhand_calibration/urdf/o30_right/o30_right_draft.urdf new file mode 100644 index 0000000..07febe4 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o30_right/o30_right_draft.urdf @@ -0,0 +1,1187 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file