diff --git a/config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml b/config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml new file mode 100644 index 0000000..90730bf --- /dev/null +++ b/config/o30_three_camera_extrinsics_20260919_front_recalibrated.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.8438350117109596 + - 0.0030839597011880146 + - 1.058604518460327 + quaternion_xyzw: + - -0.04630065903711772 + - 0.6864139609264401 + - -0.015210755676403294 + - 0.7255761546038824 + top: + translation_xyz_m: + - 0.008505305967195427 + - -0.5708652170072022 + - 1.1230571120977015 + quaternion_xyzw: + - 0.7264186042657113 + - 0.05091741831973884 + - 0.0330385816024251 + - -0.6845669288053643 +quality: + passed: true + reprojection_rms_px: 0.9332749561975657 + maximum_rotation_repeatability_deg: 0.05294934187781907 + maximum_translation_repeatability_m: 0.0004960858291558162 + 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/config/o30_three_camera_extrinsics_recalibrated.yaml b/config/o30_three_camera_extrinsics_recalibrated.yaml new file mode 100644 index 0000000..5f96e48 --- /dev/null +++ b/config/o30_three_camera_extrinsics_recalibrated.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.807055290135415 + - 0.007757362896197391 + - 1.109645424653359 + quaternion_xyzw: + - -0.038239828751376485 + - 0.7081714897073992 + - -0.008111842845009399 + - 0.7049574842983983 + top: + translation_xyz_m: + - 0.04396900884358515 + - -0.5702983486721814 + - 1.106333731407498 + quaternion_xyzw: + - 0.7207445525815062 + - 0.025265750150805705 + - 0.010996906531907304 + - -0.6926528710978753 +quality: + passed: true + reprojection_rms_px: 0.8851530873841623 + maximum_rotation_repeatability_deg: 0.02169207759215767 + maximum_translation_repeatability_m: 0.00023144069642531324 + 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/AGENTS.md b/src/linkerhand_calibration/AGENTS.md new file mode 100644 index 0000000..e52721d --- /dev/null +++ b/src/linkerhand_calibration/AGENTS.md @@ -0,0 +1,8 @@ +# 标定包协作约定 + +- 全程用中文；保留工作区已有修改。 +- 测试选择遵循 [TESTING.md](TESTING.md)。日常运行受影响用例及必要契约，常规验证目标 60 秒内。 +- 不默认运行全量 pytest、colcon test 或所有型号整手拟合。公共契约变更与发布节点再扩大范围。 +- 同一代码版本已通过的测试不无理由重复执行；修复失败后只重跑失败或受修改影响的用例，并记录耗时。 +- 优先复用不可变输入及已生成产物，在验收边界测试拒绝条件；不为每种错误重复生成整手标定。 +- 软件回放通过与实机连续通过分开报告；不能为缩短测试或减少暂停放宽精度、可观测性和发布门限。 diff --git a/src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md b/src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md deleted file mode 100644 index 1e413cd..0000000 --- a/src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md +++ /dev/null @@ -1,169 +0,0 @@ -# 统一标定需求代码审查（2026-09-10） - -## 结论与审查范围 - -审查当前工作区（包含未提交的统一重构、精简 JSON v2 和本会话启动参数修复）， -不是只审查 Git HEAD，也没有把历史 PASS 当作当前实机验收。 -通用采集、拟合、URDF 修正、最终文件验收和发布架构已经存在，但仍有迁移残留和精度验证缺口。 -本轮修正可由代码及隔离测试确认的问题，没有改变原始 CAD、机械限位、SDK、贴 Tag 配置或相机外参。 -没有启动 SDK、相机或发送机械手运动指令。 - -## 已修复的问题 - -| 问题 | 影响 | 修改 | -| --- | --- | --- | -| `vendor_sdk_config:=` 等空 launch 实参 | O6/L6/G20 可能在节点启动前退出 | runner 省略空参数，使用 ROS CLI 的实际解析器覆盖四型号 | -| 在线 IO 使用固定 front/side/top 外参加载器 | 新 Profile 即使静态校验通过，在线仍可能不能使用其他机位 | 直接用通用外参加载器，视图和参考机位来自 Profile | -| launch 固定三机位、型号文件名回退和过期参数 | 新型号需修改启动代码；部分开关没有实际作用 | 按 Profile 创建机位；要求受保护 Profile/原始 URDF/Tag 路径；移除旧诊断、速度、scope 和输出目录覆盖参数 | -| MVS 无图像时只显示“等待设备” | 操作者无法区分 SDK、相机或检测链路故障 | 统一显示每机位有效内参/检测消息等待项、SDK 条件和内参不匹配原因 | -| READY 后设备断流仍保留可开始状态 | 启动请求可能使用已失效的就绪条件 | 开始前可撤销 READY；Start 再次检查完整设备条件 | -| 实时 CameraInfo 只比较 P 的部分字段 | K、D、R 改变但 P 未变时，整流图可能与原外参不一致 | 实时校验宽高及 K/D/R/P 指纹；开始后不匹配按原有坐标变化故障停止继续采集 | -| 每个稳态采样点清空整方向统计 | 显示有效帧和覆盖率归零，误导进度判断 | 分离方向数据与当前稳态点数据，重扫/换方向才重置方向统计 | -| 标题只显示序列号 | 序列号不含型号时无法确认当前型号 | 所有型号统一显示 model、side、serial_number | -| 正式 finalizer 仍允许旧格式绕过指令拟合 | 新型号配置旧版本可能没有所需的 SDK 指令→rad 产物 | 正式 Profile 与 finalizer 均只接受统一输出版本 2，取消旧输出分派 | -| 完成回调读取精简 JSON 已删除的 `quality` | 文件可能已经发布，但节点在结束时抛 KeyError | 删除没有消费者的 `final_quality` 赋值，增加无 quality 字段的完成回调回归 | -| 视觉暂停与控制定时器之间的状态竞争 | 定时器等待锁期间已暂停，取得锁后仍可能继续动作 | 在控制锁内再次检查终止状态 | - -开始前等待相机消息的 2 秒窗口不用于扫描中的实时暂停；空检测数组算视觉链路存活。 -短时 Tag 丢失、低检测率等仍按方向结束的数据质量策略处理，同速重扫一次后仍不足才暂停。 - -清理的在线残留包括无人读取的回调计数、命令频率缓存/方法、型号质量标签、旧 tracker reset hook， -以及每次反馈回调不必要构造的备用 Adapter。没有删除 SDK 协议接口的抽象方法。 - -五个过渡期写出器已移出生产包并保留原文件： -`calibration_output/code_review_retired_serializers_umhxlepj/`。 -包括 `select.py`、`generic_v1.py`、`legacy_v4.py`、`legacy_v6.py`、`native_v7.py`。 -历史读取器和历史诊断工具仍在；此归档不包含或改写用户的历史标定产物。 - -## 视频复核后的补充修复：共用姿态分支筛选 - -视频复核发现旧配置把 `pnp_reprojection_tie_px` 和姿态拒绝阈值均设为 `1.5 px`， -导致通过质量门槛的 IPPE 候选都参加时间连续性比较，旧镜像分支可能压过明确更好的图像拟合。 -用现有候选姿态复现时，旧分支 `0.24 px` 会压过另一分支 `0.05 px`。 - -公共模块统一默认近似同误差阈值为 `0.03 px`；ROS 与离线采集继承该值。 -五份旧 YAML 的重复覆盖已移除，四产品的 `calibration_config_sha256` 已同步更新。 -新增校验拒绝非有限、负值或不小于图像拒绝阈值的分支容差，防止旧的 `1.5/1.5` 配置再次生效。 -保持实际近似同误差时的连续性选择、原有倾角/图像质量限制和方向结束重扫策略。 -因此前文“配置保持不变”仅描述首轮结构审查；这次补充修复修改了上述算法配置及其哈希。 - -覆盖五份配置和四型号默认采集的九个复现用例，在修复前全部失败，修复后全部通过。 -第一组姿态、配置、采集与加载实际参数文件的四型号隔离 ROS host 检查共 51 项通过。 -第二组公共采集/拟合与发布、O12 姿态解析、来源保护、四型号配置及启动检查共 141 项通过， -耗时 114.53 秒。两组为本次修改的定向回归，没有宣称本次又执行过全包测试。 -本次 `colcon build --packages-select linkerhand_calibration --symlink-install` 通过； -安装环境读取四产品配置、校验受保护哈希及公共默认值均通过，`git diff --check` 通过。 -这不代表所有平面双解已消除，也不替代下述相机时序和实机重复性验证。 - -## 仍不能由本轮软件审查保证的事项 - -### P1：真实采集时间与传输积压 - -`hikrobot_camera.py::_publish_frame` 使用主机取到帧时的 ROS 时间戳，未将设备曝光时间映射到统一时钟。 -SDK 帧结构有设备时间戳和帧号，但设备时钟单位、偏移、漂移与主机时钟同步还没有在当前硬件验证。 -`runtime/ros/io.py::_state_callback` 对无时间戳反馈也采用接收时间。 -因此，USB/驱动缓存可能把旧图像与新反馈配对；50 ms 的消息时间戳检查和“只取最新帧”不能排除该问题。 -本会话已经实测到三台相机共享 480 Mbps Hub 上行，且正面/上方取帧超时。 - -后续应在恢复 USB 3.x 链路后，核验设备时间戳、帧号、曝光与反馈时序，并用独立运动数据检查配对误差。 -不能猜测设备时钟单位、直接给时间戳减一个常数，或放宽质量阈值作为修复。 - -### P1：重复性和实机/仿真验收尚未完成 - -前三轮训练、第四轮独立验证、冻结 Tag 安装和双文件重读校验已接入。 -但同手多次独立重采、重新摆放和重新贴 Tag、不同保持姿态/多关节组合动作的实际验收仍缺证据。 -L6/O6 部分关节由小指参数迁移，不属于所有关节独立实测。 -发布报告仍应如实保留 `arbitrary_multiaxis_validated: false`。 - -外部仿真要加载本次修正 URDF，用同一 SDK 指令话题及配套 JSON 驱动；用实机反馈直接驱动仿真， -只能显示反馈对应姿态，不能证明指令映射正确。桥本身是消息转换器，不是独立精度测量工具。 - -### P2：数学模块和旧配置仍有维护负担 - -`core/fitting/spatial.py` 约 3,656 行，包含轴线拟合、基座位姿、零位优化、可观测性和验证， -其中 `solve_urdf_zero_offsets` 从约第 1,921 行开始，嵌套函数及共享局部状态较多。 -它是当前生产算法，不是可直接删除的废代码。后续拆分应按这些数学职责进行， -用固定输入的参数、残差、失败原因和产物回归验证等价性，不应复制成各型号独立算法。 - -当前约 58 个兼容模块仍用于旧导入路径、历史回放/迁移工具和旧回归；不能仅因目录名称或静态引用少就删除。 -几行的公开兼容导出不是第二套拟合算法。旧型号诊断、旧 URDF plan 和部分旧 YAML 参数仍需明确退休范围后继续收缩。 -受保护的历史配置在本轮保持不变；旧 YAML 中一些已不被在线节点读取的参数，不应当作当前算法的生效开关。 - -原始 G20/O12 的 mimic/限位冲突和 CAD 保留策略仍需机械资料确认；当前通用 URDF 修正 -主要处理零位、限位和线性 mimic，不估计连杆长度、轴位置、mesh 或惯量。 - -## 按需求核对 - -| 需求 | 当前判断 | -| --- | --- | -| 多型号共用拟合和 URDF 修正 | 已实现统一生产链；SDK 新协议仍需 Adapter 和 ROS 启动绑定 | -| 新型号只关心 SDK、贴 Tag、避让 | 基本成立，但必须声明关节绑定、可观测性、迁移/保留策略和可修改字段，不能从未知安装的单轴 Tag 自动猜出 CAD 零位 | -| 统一 SDK 指令→rad JSON 与修正 URDF | 正式仅生成统一 v2 指令表；u8 为 256 项，rad 为显式节点；被动表由标准 mimic 推导并验收，详细证据保留在报告 | -| 开始前可移动、开始后固定 | 每次 Start 丢弃预览参考，正式锁定；可见固定基准有漂移监测；遮挡对象是否移动不能实时保证 | -| 正确、可重复 | 有独立验证和来源保护，但仍受采集时序与未完成实机验收限制 | -| 少暂停且原因明确 | 统一策略保留；开始前明确缺失设备，不用短时视觉丢失实时打断扫描 | -| 统一进度且明确型号 | 已修正型号、设备等待原因及方向统计显示 | -| 同源指令实机/仿真验证 | 有统一转换桥；实际动态/组合动作对比尚未完成 | - -## 本轮验证 - -- 审查中完整标定包回归：**544 通过、1 跳过，383.25 秒**。跳过项要求显式提供 - `O12_REPLAY_RAW` / `O12_REPLAY_REFERENCE`，没有用合成数据替代历史实测。 -- 随后补充的启动清理、单一正式输出及归档改动：启动/runner/正式 Profile 拟合/精简产物定向组 - **74 通过，108.78 秒**，包含四型号与虚拟型号的实际公共拟合和发布路径。 -- 最后完成回调、暂停竞争、ROS host、等待诊断、实时内参保护：**25 通过，0.98 秒**。 - 与上一组有重叠，不相加冒充一次全量结果；没有宣称最后所有变更又跑过一次全量。 -- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。 - 安装入口读取四产品的受保护配置、ROS launch 实参解析均通过；167 个生产/兼容 Python 文件 AST 通过； - `git diff --check` 通过。 -- 新 launch 测试实际构造四型号及改名机位的 launch actions，但不执行这些动作。 - 隔离 ROS 节点/标准加载器测试不启动 SDK 或相机，不代表实机验收。 - -## 数学核心与 ROS 组合重构的验收记录 - -本轮以包含公共 `0.03 px` 姿态分支修复的工作区为基线，只拆分职责、明确数据和并发边界。 -没有删除基线中仍存在的历史入口，没有修改配置、拟合公式、优化初值/顺序、残差权重或验收阈值。 - -- `core/fitting/spatial.py` 保留 45 个原定义符号的显式导出；实际计算在 `spatial_solver/`。 - `solve.py` 的入口顺序为输入准备、训练求解、统计、独立验证及结果装配。 - 数据通过 `ZeroProblem`、`TrainingProblem`、`TrainingFit` 和各验收结果类型传递。 - 训练接口只接收训练观测，训练几何中的掌部姿态按训练轮筛选。 -- `core/geometry/pnp.py` 保留 14 个原定义符号；实现拆为 IPPE、单 Tag 跟踪、刚性组和轨迹选择。 - 跟踪默认值集中在 `tag_pose/parameters.py`，ROS 参数只负责加载和 deg/rad 转换。 -- `UnifiedCalibrationNode` 直接继承 ROS `Node`，组合消息 IO 与 `CalibrationCoordinator`。 - 协调器不加载 ROS 库，复用 `SessionExecution`、采集、运动、固定基准和断点组件。 - 运行参数、观测输入和快照使用明确类型；阶段只有 `CalibrationSession.phase` 一个来源。 -- `FinalizationController` 组合原 worker 与发布器，协调器接收阶段事件并授权提交。 - 状态锁先于 worker 锁；PnP 在状态锁外计算，提交时检查会话版本、采集/运动对象及稳态边界。 - 暂停、中止与提交互斥；失败不触发自动恢复运动，成功只提交一次。 - SDK 绑定显式接收反馈新鲜度、时钟、健康订阅和发布接口。 - -固定输入记录在本次工作区快照 `/tmp/calibration_structure_baseline_30qf3ozo/`。 -持久化的对比报告及压缩前后输出位于 `calibration_output/structure_refactor_20260910/`， -其中 `comparison.json` 记录输入摘要、差异、产物摘要和拒绝原因，`protected_inputs.json` 保存输入哈希。 -这些记录是合成数据的结构等价证据，不是实机精度验收。 - -| 固定输入 | 比较浮点值数量 | 最大绝对差异 | JSON/拟合结果/阶段事件及 URDF 文本 | -| --- | ---: | ---: | --- | -| G20 | 46,046 | 0 | 一致 | -| L6 | 15,304 | 0 | 一致 | -| O6 | 15,304 | 0 | 一致 | -| O12 | 14,494 | 0 | 一致 | -| 虚拟型号（任务重排） | 15,304 | 0 | 一致 | - -对五组输入分别移除第四轮或注入第四轮 Tag 姿态滑移，10 个拒绝原因和阶段事件与基线一致。 -167 个配置、Profile、原始 URDF/资源文件的哈希不变。 -另一个虚拟型号同时重排 SDK 通道、机位名称和扫描任务，经过真实公共采集、finalizer、 -最终 JSON/URDF 验收和发布，再检查同一 SDK 指令经过模拟传输、查表和标准 mimic 图的一致性。 - -验证结果： - -- 整个标定包：581 passed，1 skipped，337.43 s。 -- SDK 绑定时钟补充后，协调器、四型号 ROS 构造和安全策略回归：27 passed。 -- 跳过项是 `test_o12_recorded_replay.py`：未提供 `O12_REPLAY_RAW` 与独立参考模型；不计作实机通过。 -- 改动模块通过 Pyflakes，`git diff --check` 通过。 -- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。 -- 安装后的 12 个 console entry 可导入；两个产品 CLI 的 `--help` 正常。 - 两个 ROS 节点入口均通过四型号受保护 Profile 的转交检查；安装环境下四型号 ROS 构造/Start 回归 4 passed。 - -本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。 diff --git a/src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md b/src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md deleted file mode 100644 index ea78d21..0000000 --- a/src/linkerhand_calibration/CALIBRATION_PIPELINE_REVIEW.md +++ /dev/null @@ -1,937 +0,0 @@ -# 四轮采集与图像拟合的修改前评估 - -日期：2026-09-16。目标：稳定完成采集，从可信标定 JSON 重建修正 URDF。 - -## 当前结论（2026-09-16） - -当前 O6 按重新实测的 16.5 mm Tag 完成新采集，完整数据经正式离线流程验收通过， -已生成合格 JSON、修正 URDF 和发布清单。本次在线收尾发生系统内存耗尽，不能把离线 -成功表述为在线全过程无中断。已修正收尾读取原始日志时的整文件副本开销，独立验证见后文。 -没有增加 O6 专用容错分支或降低物理精度门限，也不据此宣称其他型号实机均已通过。 - -| 环节 | 当前证据与结论 | -| --- | --- | -| 四轮连续采集与必要稳态点 | 新会话 `20260916_105528` 完成 36/36，1 个小指方向自动重扫后通过；采集约 6 分 47 秒。随后收尾时发生 OOM，反馈超时暂停；完整数据已用于正式离线验收。此前两次 16 mm 会话也完成采集，但空间验收失败。 | -| 反馈时间 | 使用真实 CAN 接收时间；相同缓存不得伪装成新测量。逐通道时间保留至图像插值。 | -| 相机时间 | 三台实际设备的曝光事件与帧计数器吻合；设备锁存换算曝光中点，替换约晚 16.5 ms 的取帧时间。 | -| 调度 | ROS 控制/反馈顺序处理，图像独立有界队列；实机对照消除了共享锁造成的反馈排队。 | -| 映射输入 | 当前报告的分段线性反馈曲线保留曝光时刻的小数输入；实际字节命令与旧整数表保持量化。 | -| 图像证据 | 同一运动保存全部机位的原始角点，不依赖任务主机位或 PnP；最新会话保存 16,822 个图像记录。没有增加运动或停点。 | -| 图像验收 | 独立三维测量对原始角点仍须 ≤1.5 px；最终文件按原角度/位置门限检查，额外视角只提供物理容差内的图像相容性证据。已知真值反例和反向拒绝测试通过。 | -| 平行转轴零位 | 已修正参考点沿轴移动导致零位变化的公共公式；四型号源 URDF 的已知真值及任意轴上参考点检查通过。数学规则不再由型号开关选择。 | -| JSON → URDF | `20260916_105528/offline_165mm` 已生成通过验收的文件与 manifest；另外调用重建 CLI，仅从保存 JSON 和源 URDF 得到的文件与发布 URDF 逐字节相同。 | -| 整链空间精度 | 新 16.5 mm 会话已通过原门限。主观测运动/稳态独立验收中，各角色最差角度 P95 为 1.873°，位置 P95 为 2.122 mm；最终文件空间与图像检查通过。精度适用范围是观测关节、扫描方向与固定的其他关节姿态；未独立观测的关节仍按配置迁移。 | -| 尺度输入核验 | 用户实测更正为 **16.5×16.5 mm** Tag、**27×27 mm** 棋盘，与独立图像检查基本吻合。当前 O6 的 Profile、检测器和标定节点尺寸已同步并记录新指纹；未修改相机参数，未将旧 16 mm 数据冒充新尺寸采集。 | - -先前的分机位空间不一致已在修正实测尺寸后的新数据上通过验收，未靠增加拟合参数 -或放宽门限达成。当前稳定性问题集中在已确认的收尾内存峰值；保留完整采集数据直接 -验证收尾修复，不为重复计算重新运动机械手。其余型号仍需对应硬件实测，O6 当前 Tag -布局中的其他三指曲线迁移不等于各关节独立标定。 - -上述两次未验收候选的三个主动零位差均小于 0.03°，说明当前分段求解可重复， -不能据此证明其绝对空间精度。正式发布仍必须通过整链与最终文件验收。 - -以下按发生顺序保留评估依据；早期 CAN 未连接等现场状态已由上述新实采结果更新。 - -## 已核实的问题 - -1. 当前生产采集要求准备运动先确定唯一 PnP/铰链姿态；不唯一时整场暂停，后续三轮训练数据尚未获得。 -2. 准备阶段的自由铰链几何与最终保留骨长、轴向的 URDF 使用不同参数空间。 -3. 后续姿态已受准备模型约束，不能把其重复性直接解释为原始图像上的整链正确性。 -4. JSON 保存后读取并重建 URDF 已有独立字节一致性验证，应保留这条文件链。 - -## 修改前的离线实验 - -没有改动生产算法、现场硬件、原始采样或原始 URDF。以 `20260915_150015` 保存的原始 -角点进行诊断：前三轮抽取 480 个 Tag 观测，第四轮 160 个观测仅用于冻结后的检查。 -直接按源 URDF 的运动链拟合一个共同基座、三个允许修改的零位和每个 Tag 的固定安装； -另一个实验允许带固定基准的双向曲线修正。两个实验均使用记录时的相机参数。 - -- 固定曲线：39 个参数，32 次迭代收敛，约 0.60 秒。 -- 联合曲线：119 个参数，23 次迭代收敛，约 1.90 秒，曲线最大修正约 1.38°。 -- 带 0.05 px 噪声的已知真值实验，三个初值均收敛，三个零位最大误差约 0.023°。 -- 实采第四轮仍有不合格残差；小指末节最大约 4.15 px。上述实验不是实机通过证据。 -- 使用后来核验的相机参数进行的私有反事实诊断也没有消除全部残差，不能将旧数据改挂新参数发布。 - -脚本、参数、雅可比和报告位于 -`calibration_output/O6_RIGHT_001/20260915_150015/review/architecture_assessment/`。 -原型只证明统一参数空间可以计算、在所测合成条件下可恢复；没有证明全型号实机精度。 - -## 采用的边界 - -- 采集保存原始角点、相机身份、SDK 指令/反馈、方向、轮次与基准图像；固定基准漂移、 - SDK 故障、时间同步和必要采样覆盖检查仍然有效。 -- 稳定基准图像先冻结，三维解释后求解；未求解的图像不得标成已测准的角度或零位。 -- 训练只使用前三轮；第四轮与独立稳态验证不参与几何、安装、曲线或候选的选择。 -- 图像拟合使用与输出 URDF 一致的拓扑和可修改参数；不自动改变骨长、转轴、相机参数， - 不新增 CAD 零位假设。显式复制关节继续保留来源声明。 -- 像素残差小不是角度/位置精度合格的充分条件。原物理门限、参数可确定性和最终文件读取验证需保留。 -- 模型尚未通过回归与实测验收前，不凭局部收敛结果替换正式发布逻辑，不降低门限以生成 PASS。 -- 采集完成后的拟合失败应保留完整数据并支持离线重算，区别于运动中暂停。 - -## 第二轮评估：不采用未经验证的整体替换 - -对同一会话进一步测试直接从原始角点初始化、共同拟合 33 节点双向单调曲线的版本。 -共 359 个参数、6160 个训练残差，150 次迭代、43.39 秒后仍未收敛；拇指末节 -第四轮最大误差约 45.18 px，小指末节约 7.77 px。这个结果说明，前一个实验依赖 -既有拟合初值的局部收敛，不足以证明新流程具备独立初始化和实机鲁棒性。 - -因此该原型**未接入生产**，相关采集/基准/拟合草稿已移出程序源码，连同源代码快照保存到 -上述评估目录的 `unadopted_prototype/` 和 `prototype_source_snapshot.tar.gz`。 -`monotonic_urdf_image_replay.json` 保留失败数据。下一步不能通过增加迭代次数、减小残差权重 -或放宽门限将其直接上线。 - -经过四型号 FK 对照测试的投影基础 `core/fitting/urdf_image.py` 和对应测试也归档到 -`unadopted_prototype/`，便于复查。生产源码不保留未接入的备用算法。 - -## 本次实际接入的流程修改 - -1. 四轮及稳态采集完成、数据同步落盘后，显式关闭图像采集，撤销尚未返回的图像回调。 - 拟合使用已经保存的图像与相机参数；此后相机消息/Tag 位置变化不再改变这份输入或触发 - 采集中暂停。运动采集期间的固定基准漂移检查保持生效。 -2. 反馈接收、SDK 故障保护和用户中止在后台拟合期间持续生效。 -3. 数值拟合、文件验收和后台阶段协议错误统一结束为 `FAILED`，保存失败阶段、原始数据路径、 - 完成/计划采集单元数和异常堆栈；不再写成采集中 `PAUSED`。 -4. 状态输出始终提供原始数据路径，拟合失败提供诊断路径，提示先离线复算。 - 失败结果不会填写已通过的 JSON/URDF 路径，也不会被迟到观测或清理用中止覆盖。 -5. 修复反馈曲线整链回放中丢失非活动关节到达方向的问题。比如下游关节正在弯曲， - 上游关节保持不动时，上游仍应使用其到达方向对应的回差曲线，不能自动改用双向均值。 - 反馈回放和稳态指令回放现在使用同一个方向解析函数；记录了不完整或非法方向时明确拒绝。 - 独立两轴合成数据证明：旧处理会使固定 Tag 看起来有超过 4° 的安装误差，修复后 - 训练和第四轮最终 URDF 回放恢复真值。当前旧 O6 数据的对应影响小于 0.03°，不是 - 该次实采大误差的主要原因。数值见 `held_joint_direction_effect.json`。 -6. 用户确认 O6 顶部 ID7 与正面 ID2 固定在同一末节后，明确设置其安装连杆为 - `rh_thumb_distal`，不再从侧摆任务名称推断为侧摆根部连杆。Profile 哈希同步更新； - 旧会话的配置身份保持原样。独立合成场景中的 O6 安装真值同步修正，其他型号不变。 - 单独侧摆且弯曲角固定时，错误连杆可以被固定安装矩阵吸收，因此不能声称该错误 - 一定解释先前全部残差；必须在正确绑定及完整关节状态下继续验证。 - -这组流程修改解决采集完成后继续受实时视觉影响和失败状态误导的问题；准备段的几何 -不一致另按下面的评估处理。保持原文件生成与发布验收链路。 - -## 第三轮评估：使用输出 URDF 已保留的几何约束 - -源 URDF 中相邻平行转轴的平行关系及垂直轴距，在任意关节零位修正下均不变。 -旧准备模型却允许这些量自由变化，求出的几何无法由最终保留骨长和轴向的 URDF 表达。 -因此只对真实 Tag 安装连杆、观测拓扑和源 URDF 均满足条件的相邻平行轴，消去两个 -自由轴倾角和一个自由轴距参数。相机、Tag 安装与逐帧角度仍由图像估计；不新增 -CAD 零位约定、不使用 SDK 角度作为拟合先验、不引入额外惩罚权重。 - -适用性由共同编译器判断，没有 O6 专用分支；不满足条件时保留原图像模型。 -冻结模型记录新的策略版本、约束及源 URDF 哈希，最终处理前从源文件重新推导验证。 -旧模型的序列化字节结构保持不变，避免破坏旧证据哈希。内存记录与磁盘记录均保留会话头。 -断点策略升级为 `confirmed_branch_v6_source_geometry`；旧自由几何会话仍可离线读取， -不能作为新约束流程的断点混入新采集。 - -对 `20260915_160040` 保存的小指准备段 104 帧原始角点： - -- 原自由模型返回 `image_motion_families_not_distinguishable`。 -- 使用源平行关系与 36.9995936 mm 轴距后，四个候选均在原 150 次迭代上限内收敛。 -- 最优两个候选收敛到相同完整 Tag 姿态，其他候选仍用原统计检验排除。 -- 最优候选留出帧最大误差约 0.696 px，低于原 1.5 px 门限；正式求解器约 4.69 秒返回成功。 -- 原像素、角度/轴心不确定性、候选等价性及显著性门限均未放宽。 - -独立合成图像验证同向及反向平行轴的真值恢复；四型号源几何验证约束在独立零位变化下 -仍成立。这证明该准备段可以使用与输出一致的模型解算，**尚不等于新四轮整手实机通过**。 -结果保存在 `20260915_160040/review/cad_geometry_resolution.json` 及相邻诊断文件。 - -随后经实际 `BranchInitialization` 入口回放该会话三个准备段的原始图像，拇指侧摆 -98 帧、拇指弯曲 88 帧、小指 104 帧均通过，耗时约 0.45 / 3.78 / 4.94 秒。 -该回放使用用户已确认的当前 Tag 绑定，仅保存诊断；没有改写旧会话或发布新标定。 -见 `all_preparations_source_geometry.json` 和同目录可复现脚本。 - -## 验证顺序 - -1. 受约束图像模型的已知真值、多初值、旧证据兼容和错误几何反例。 -2. 四型号共同采集逻辑与数据身份；反馈/固定基准保护及中止持续生效。 -3. 前三轮训练身份与第四轮隔离；完整性、重复图像、篡改与旧会话身份检查。 -4. JSON 落盘后独立重建 URDF、标准 ROS 加载及最终文件验收。 -5. O6 当前相机配置下重新实采；只有通过实际验收才更新发布结果。 - -## 本轮验证结果及实机阻塞 - -- 最终几何、真实图像入口、采集边界、来源身份、JSON 独立重建回归：87 项通过（39.81 秒）。 -- 后台进程、诊断采集、四型号产物与 launch 相关集成检查：80 项通过（157.08 秒）。 -- 新旧断点策略、恢复与原始图像凭据检查：92 项通过（4.85 秒）。 - 各组有交叉，不将数量相加作为独立测试总数。 -- 已知真值覆盖两/三关节链和正/反向平行轴；故意把 37 mm 轴距改为 80 mm 时， - 求解器因原重投影门限不合格而拒绝，不能仅凭源几何声明强行通过。 -- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。 -- 用户确认现场为 O6 后，清理了仍订阅同一控制话题的 O30 残留 SDK/GUI。 - 实际执行新的 `calibrate_hand --no-resume` 命令时，启动检查报告 `CAN interface does not exist: can0`。 - 系统仅有 lo/eno1/wlo1；采集未开始、未发送电机指令、没有发布本轮新标定。 - 需恢复 O6 原通信适配器后继续四轮实测；当前不能宣称整手实机精度或全部型号实机通过。 - 启动报告保存在上述 review 目录的 `o6_cad_geometry_startup.json` 和日志中。 - -## 第四轮评估：有限恢复、精简停点与最终原始角点验收 - -本次优化以保留四轮连续扫描、独立验证集和原精度门限为前提。没有采用直接用运动中 -角度代替静态指令映射的办法，因为反馈滞后和方向回差会使两种角度不同。 - -### 准备段恢复 - -`runtime/zero_recovery.py` 为每组尚未冻结零位的关节提供一次局部恢复额度。 -图像数量不足或候选暂时不确定时，复用该组原归零路径补采；几何已确认而零位图像 -不足时，仅保持当前姿态重新打开图像窗口。恢复撤销旧观测回调，保留真实日志； -不替换已冻结几何/零位、不改变保持条件，不将设备故障当成视觉缺样处理。 -两种恢复共享一次额度，持续失败仍暂停，不无限重做整场四轮运动。 - -### 训练停点精简 - -`core/fitting/command_sampling.py` 只读取前三轮连续数据，在原静态训练网格上求最少 -保留节点，使每关节、每方向在原网格节点的插值变化不超过 0.25°。端点、baseline、 -中间支撑点和显式非线性补充节点必须保留；训练不足或单调修正过大时保留完整网格。 -该准则决定采样位置，不证明实际静态误差已经达标。每个保留点仍需采集新稳态图像； -原独立稳态验证点全部保留，只有这些验证与最终产物验收通过才可发布。 - -计划记录训练来源及自身哈希，采集、恢复、离线回放和拟合共同消费同一声明，禁止 -使用第四轮或稳态验证点反过来选择训练节点。断点策略升级为 -`confirmed_branch_v7_final_image_sampling`，旧数据不会混入新流程冒充新证据。 - -另一项优化只省去重复准备等待：上一段已经完成到位检查、下一目标的完整指令向量 -相同、一秒内反馈未变化时复用该结果；真实位移、反馈变化、记录过期或归零准备段 -仍执行原运动检查。原运动速度、避让和采样稳定性门限保持不变。 - -对 O6 `20260915_150015` 的已保存数据离线比较，稳态训练点由每方向 9/9/10 个变为 -7/6/10 个；独立验证仍为 10/10/11 个，双向总停点由 118 次降至 108 次。 -小指因训练曲线噪声保留原网格，不以强行减点换取精度风险。 -原方案和精简方案均通过原独立稳态角度验证，全部关节最大误差仍小于 1.69°。 -这是旧数据的训练点选择对照，不能当作新四轮实机或最终空间精度通过。 -原始文件哈希保持不变，脚本及结果为该会话的 -`review/assess_static_sampling.py`、`review/static_sampling_assessment.json`。 - -### 最终文件到原始图像 - -`runtime/artifacts/image_evidence.py` 从原始记录绑定图像身份、四角、Tag 尺寸以及 -受哈希保护的相机外参和 CameraInfo。`core/urdf/image_acceptance.py` 读取最终文件， -使用冻结的安装和基座，把 JSON＋URDF 的 FK 直接投影到第四轮及独立稳态验证四角。 -这里不调用 PnP、优化器或重新配准；同一最终产物还须通过原来的 3D 和角度门限。 -每帧四角重投影 RMS 沿用 1.5 px 门限，超限保存 `final_image_diagnostics.json`。 -原始证据缺失、相机身份变化、训练/验证重叠及重复图像都会被拒绝。 - -新软件的自动测试与离线数据检查不代表四型号已经逐一实测通过。 -O6 仍需恢复 `can0` 后运行新的完整四轮，并通过上述最终文件验收。 - -本次新增链路的验证结果： - -- 四型号精简采样策略及实际 JSON→URDF→角点验收共 11 项通过（96.41 秒）， - 包含静态验证异常仍被拒绝、第四轮不影响训练点选择等反例。 -- 局部恢复、真实定时器流程和重复等待边界共 24 项通过（21.62 秒）。 - 持续缺少第十张零位图像在一次补采后仍停止，不能少采冒充合格。 -- 最终角点、真实相机来源、混合实测/复制产物及后台进程共 23 项通过（25.13 秒）。 -- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过（1.81 秒）。 -- 构建后再次执行 O6 正式 `calibrate_hand --no-resume`，仍在配置检查阶段报 - `CAN interface does not exist: can0`，没有进入运动采集；日志保存为 - `20260915_160040/review/o6_optimized_flow_startup.log`。 - -这些组存在覆盖交叉，不相加作为独立总数；四型号整链使用独立构造的合成观测， -真实来源绑定另用 O6 已保存角点检查，两者均不能冒充新实机结果。 - -全包回归中同步修正两处旧测试约定：零位不足的用例须观察到唯一一次恢复结束， -而非要求第一次超时立即暂停；历史侧面准备图像须调用带源 URDF 约束的当前生产入口， -不能继续要求旧自由几何模型必然可解。针对这两处的 7 项复测通过（11.49 秒）。 -同一份侧面历史数据 121 帧使用当前入口通过，训练/验证仍为互不重叠的 61/60 帧， -候选统计门限与绝对像素门限保持不变；没有为了通过测试修改原始数据或生产门限。 - -全包首次运行共 1497 项：1494 项通过、上述 2 项旧测试断言失败，另有 1 项因未提供 -O12 本地实采与独立参考文件而跳过（1175.15 秒）。两处失败修正后的 7 项复测通过， -没有剩余已知失败。完整日志、专项回归和构建日志保存在 -`20260915_160040/review/common_flow_validation/`；不将跳过的 O12 实采检查视为通过。 - -## 第五轮评估：训练选择与准备验证的边界 - -2026-09-15 19:32 使用新相机外参进行正式 O6 采集，拇指两任务完整采完， -小指准备阶段两次均报 `image_motion_families_not_distinguishable`。原始数据与修改前 -评估保存在 `20260915_193254/review/solver_assessment/`；先完成诊断实验，再修改生产代码。 - -整体链路保留：准备几何 → 冻结零位 → 四轮连续扫描 → 必要稳态映射 → JSON → 回读生成 -URDF → 独立图像验收。本次问题位于准备模型的选择和验证，不涉及末端非线性存储格式。 -独立稠密 Jacobian 求解证实增加迭代无效；四个初值有三个收敛到同一解，剩余解与其 -相差约 19°、9 mm，不能把真实空间差异当作浮点误差。 - -审查发现原实现按验证误差选择优胜模型，再用同一验证集证明它较好；拟合使用平方误差， -统计比较却使用 RMS 差；逐个立即返回还保留了最保守的固定多重比较惩罚。 -现统一为训练选择、验证否决，候选所用的训练资格不受验证结果或验证异常影响； -验证失败不得改选。公共 `image_model_selection.py` 按同一平方损失比较，完成全部候选 -比较后使用 Holm 校正，原始与校正后的概率均写入诊断。 - -1.5 px、0.03 px、1°/1 mm 等价界、原角度和空间验收门限均保留，名义族错误水平仍为 0.01。 -同帧等价检查按角色名对齐，并拒绝缺失或重复角色；不再依靠 `zip` 恰好有相同顺序。 -姿态策略升级为 `confirmed_branch_v8_training_model_selection`，旧会话仅供诊断，不能 -自动续接为新策略采集。新增策略适用于公共流程，不增加 O6 专用回退或停点。 - -修改前的候选策略诊断中，两次失败数据均可区分（概率约 0.00373、0.00576），拇指保持通过， -真正歧义的合成数据保持拒绝；这些仅是修复依据，不代表整手新实机验收完成。 -统计假设和条件性边界参考 [SciPy Wilcoxon 文档](https://docs.scipy.org/doc/scipy/reference/generated/scipy.stats.wilcoxon.html)， -多重比较方法参考 [Holm 方法文档](https://www.statsmodels.org/stable/generated/statsmodels.stats.multitest.multipletests.html)。 - -新策略实机 `20260915_195619` 完整采集 36/36 个方向单元，0 次暂停、0 次局部补采； -小指准备一次通过。146 项准备求解与产物链专项检查通过。最终空间验收仍失败， -候选 JSON/URDF 未发布；这与采集是否完整是两个不同结论。 - -## 第六轮评估：先修复测量时间，再判断空间求解 - -该会话的原始角点、生产重放和参数对照保存在 `20260915_195619/review/spatial_assessment/`。 -轴线投影开关没有改善整体误差；共同 URDF 的 39 参数图像求解虽然收敛，第四轮仍有 -4.124 px 误差，因此没有采用这一替换。相同反馈和相同方向的第四轮图像还能构成 -与任何确定性查找表不相容的反例：两帧像素距离 3.753 px，最大误差必有一帧不低于 -1.877 px，超过现有 1.5 px 门限。该反例不依赖拟合算法，也不证明硬件损坏。 - -SDK 代码确认了一个时间来源错误：发布线程给低频轮询缓存反复填上当前时间。 -相机与发布消息的时间差很小，并不能证明相机与真实反馈同步。修复位于共同数据边界： - -- CAN 接收端原子保存位置和接收时间，O6/L6 单帧、G20 按各指分帧保留。 -- SDK 的标准状态保留真实接收时间；标定专用接口携带每通道时间，仅发布新增测量快照。 -- 标定按每通道实际时间插值。重复、乱序、过期或同一接收身份对应不同值的记录不能刷新 - 稳态样本或反馈新鲜度；原始反馈快照落盘，便于复核。 -- 共用采集策略升级为 `unified_engine_v6_measured_feedback_time`，四型号 Profile 同步声明。 - 源 URDF、相机和精度门限保持原样；旧采集不能改挂新时间策略发布。 - -时间契约、缓存、分帧、回归与稳态检查 150 项通过；四型号配置、ROS 接入、断点与产物链 -集成检查 69 项通过；SDK 与标定两包构建通过。不运动的实机接口核验中，486 条标定反馈 -全部匹配独立旁路收到的原始 CAN 帧，0 条无来源、0 条重复身份。真实测量频率约 25 Hz， -同一时段标准接口发布 1147 条消息，说明之前约 58 Hz 的发布频率不能作为测量频率。 -该核验启动 SDK，SDK 仍执行其初始化速度/力矩设置；没有发送位置运动命令。 -完整实机产物仍须用新的测量时间重新采集后验收。 - - -## 第七轮评估：控制消息顺序处理，图像任务独立执行 - -真实时间采集 `20260915_202203` 暴露了图像缺样：一段约 0.30 秒的有效图像空白 -内仍有 15 条真实 CAN 测量，不应扩大同步窗口来掩盖。最初加入每视角有界图像工作线程， -但 ROS 端仍保留多线程执行器；`20260915_203104` 恢复导入期间仍因反馈超过一秒暂停。 -这项中间实现没有完成目标，不能算作修复成功。 - -进一步评估保存在 `20260915_203104/review/dispatch_profiling/`：真实 64 条记录编码 -约 6 ms，不支持直接减小导入条数。实机锁计时和线程栈显示，多个 ROS 回调争用同一把 -会话锁，图像队列还在持有自己的锁时反向查询会话。仅在诊断入口替换为单线程 ROS -执行器后，反馈进入回调的延迟由初段中位约 113 ms、P95 247 ms，降到约 9 ms、17 ms； -同一精度、采样和时间策略下已成功复用 8 个合格单元。 - -据此整理公共 ROS 接入，而不更换拟合器或增加型号特例： - -- 控制、反馈、相机消息解析和状态更新由一个 ROS 执行器顺序处理，删除冗余回调组。 -- 每个视角只保留一项执行中的图像任务和一项待处理任务，过载时保留最新待处理图像。 -- 只有 coordinator 接受的新测量才能推进图像队列的时间水位；多帧协议使用最旧通道时间。 - 队列持锁时不调用 coordinator，开始新会话时清空旧水位和未执行图像。 -- 原 50 ms 同步界限继续有效；图像估计仍在锁外，提交结果仍检查会话和运动版本。 -- 图像线程异常传回控制入口，不将线程停止当成正常无图；检查点落盘规则保持原样。 - -调度、ROS 接入、真实测量时间、coordinator 和断点恢复共 51 项针对性检查通过； -图像元数据并发、过期结果提交和隔离计算共 28 项检查通过；标定包构建通过。 -单线程对照运行及后续正式运行的终态以各会话记录为准；这里的延迟改善不能代替 -四轮完整性、独立角点和 JSON＋URDF 的最终空间验收。 - - -## 第八轮评估：相机曝光时间与取帧时间分离 - -单线程对照会话 `20260915_203750` 完成 36/36 单元（复用 8 个已验证单元）、0 次暂停， -生成候选 JSON 和由该 JSON 重建的 URDF，但顶部 ID7 的训练安装位置 P95 为 18.285 mm， -超过 3 mm，产物未发布。其余四个实测 Tag 角色通过该项空间检查。共同 URDF 的 39 参数 -原始角点拟合虽然收敛，第四轮仍达 4.779 px，因而没有替换生产拟合器。 - -随后检查图像时间来源，发现相机驱动在 `MV_CC_GetImageBuffer` 返回后使用当前 ROS -时间，不能表示曝光时刻。实测三台相机的设备时钟、40 次独立时钟锁存，以及曝光开始/ -结束事件，确认： - -- 三台取帧时间分别比设备帧时间晚约 16.44 / 16.52 / 16.49 ms；不应靠拟合器吸收该延迟。 -- 帧计数时间与独立曝光开始事件相差约 150 ns；固定曝光时可使用曝光中点时间配对。 -- 当前固件的 `DeviceTimestampIncrement` 返回 100,000,000，实测约 10 ns/tick。 - 设备 XML 的单位说明与此不一致，因此程序启动必须用独立经过时间核验计数单位。 - -`camera_timing.py` 单独负责设备/主机时钟对应和曝光时间换算。每 0.5 秒重新锁存， -选择往返区间最短的有效测量；偶发缓慢事务丢弃后有界重试，不能刷新原两秒有效期。 -错误单位、时钟跳变、计数器重置、重复帧和过期对应均不能伪装成新图像。 -相机驱动在图像及 CameraInfo 中使用同一曝光中点时间，保留每次锁存和每帧换算记录 -`camera_timing_.jsonl`。固定外参和内参不因软件时间修正而改变。 - -共用采集策略升级为 `unified_engine_v7_camera_device_time`，四型号 Profile 及其受保护 -哈希同步更新；旧会话保留诊断用途，不能直接续接到新时间规则。改动没有增加停点、 -调整非线性查找表语义或放宽最终精度门限。 - -时钟、真实三相机记录、ROS 接入、配置、启动图和断点/队列共 57 项检查通过；构建通过。 -随后实际驱动对三台相机各连续发布 242 帧、执行 16 次校时，无时序或时钟错误。 -O6 运动期间另旁路监听 CAN：370 条位置命令的响应均不是原命令回显，仍可作为真实状态 -观测；没有把 L6 的回显过滤规则未经核实套到 O6。 - -完整新策略实采会话为 `20260915_205704`，不复用断点、不使用调度诊断包装。 -共完成 36/36 个单元，0 次人工暂停；拇指弯曲第四轮增加方向和小指第三轮增加方向 -各自动重扫一次。候选 JSON/URDF 已生成，Tag 安装刚性训练检查失败，未发布 manifest。 -正式失败时位置 P95：小指末节 3.108 mm、拇指末节 6.476 mm、拇指弯曲 3.875 mm、 -顶部拇指 10.086 mm。会话中 `review/spatial_assessment/live_run_summary.json` 保留统计。 - -## 第九轮评估：反馈插值契约与空间模型边界 - -反馈拟合使用曝光时刻的连续插值值，报告声明 `piecewise_linear`；原 `JointMapping` -和 `SerializedJointMapping` 却对所有 `_u8` 输入再次取整，使训练和读取语义不同。 -统一规则为：现代反馈曲线连续插值，实际 `command_u8` 量化，历史整数表保持原兼容行为。 -序列化读取在量化前检查输入支持域，不能将越界小数取整后当成合法测量。 -此修改不改变非线性曲线本身、不重拟合零位、不调整采集版本或原始记录。 - -先用相同 39 参数整链诊断做对照：保留原 URDF 骨长、轴向和已拟合曲线，前三轮拟合 -共同基座、允许的零位及固定安装；独立第四轮不参与优化。取消反馈取整后，小指末节 -最大像素误差由 3.390 降至 2.449 px，但仍超过原 1.5 px，因此该诊断模型未接入生产。 -另加有界双向曲线修正的 119 参数实验，第四轮小指末节最大仍为 3.136 px，且出现 -非单调局部曲线；增加参数并没有提供足以替换现有生产求解器的证据。 - -正式公共回放也验证：反馈取整修复不能消除空间不一致。位置 P95 为小指末节 3.080 mm、 -拇指末节 6.489 mm、拇指弯曲 3.877 mm、顶部拇指 10.088 mm；保持失败且未发布。 -原始会话与在线候选文件未覆盖；新回放位于 `20260915_205704/review/spatial_assessment/` -下的 `continuous_production_replay/`。同目录保留诊断脚本、参数、Jacobian 和测试日志。 - -补充整链可行性对照仍只使用前三轮、相同 39 个参数：将训练目标换成按原 2°/3 mm -尺度归一化的三维位姿残差，局部收敛后拇指弯曲零位触及原 20° 边界；第四轮顶部 -位置 P95 仍为 5.148 mm，原始图像最大误差增至 28.718 px。此结果没有证明全局无解， -但否定了将这一局部三维优化结果直接用于替换现有求解器的做法。 -原始保存位姿经受保护的相机变换投回同帧角点，各角色最大误差均小于 0.64 px； -没有发现这条原始公共坐标变换的方向或图像身份错配。检查见 -`continuous_spatial.json`、`common_pose_projection_check.json`。 - -反馈输入、双映射、被动非线性与读取兼容检查 57 项通过；补充真实时间插值、四型号 -合成产物链、JSON 重建、独立角点和标准 URDF 检查 49 项通过，两组有交叉。 -SDK 和标定包最终构建通过，`git diff --check` 通过。 -合成验证不等于其他型号实机通过；本轮 O6 的最终空间精度问题仍未解决。 - -## 第十轮评估：保留跨运动的原始图像证据 - -此前原始角点随任务主机位的姿态采集写入，缺少顶部 ID7 在弯曲中、正面 ID2 在侧摆中 -的完整原始图像。这两块 Tag 已确认属于同一末节；只保存各自任务下的图像不足以独立 -核查共享运动链。公共采集器现独立保存所有机位的有效原始角点，保留原检测质量、 -窗口与会话身份，不依赖 PnP 是否成功。主采样门限、运动计划和稳态点数量均不变。 -同机位图像时间去重只在成功提交时推进；迟到或窗口已撤销的图像不能推进水位。 - -采集策略升级为 `unified_engine_v8_all_view_images`，四型号 Profile 及产品哈希同步更新。 -旧会话不修改哈希、不补造新策略记录。新增采集与公共并发检查 43 项通过，补充采集、 -四型号配置与断点身份检查 58 项通过，两组有交叉；标定包构建通过。 - -新会话 `20260915_213337` 完成 36/36，0 次人工暂停；小指第二轮反向扫描自动重试一次。 -三机位共保存 16,822 个原始图像记录。正式算法仍在 Tag 安装空间一致性处失败， -未发布 manifest。主动零位与上一会话差均小于 0.03°，不能以重复性替代正确性。 - -保持当前源 URDF、相机参数和 Tag 尺寸，前三轮拟合共享基座、零位和每块 Tag 的固定安装， -第四轮检查的新全视角对照仍未通过。39 参数固定曲线模型的最大像素误差约 3.52 px； -359 参数双向单调曲线模型最大约 4.32 px。后者按 SDK 输入均匀选择训练图像， -不再按停留时间重复加权；非负曲线增量和参考输入零约束仍不足以消除残差。 - -静止姿态也出现约 1%～3% 的投影边长偏差，因此另做一次受限的统一骨长尺度诊断。 -其训练最优尺度约 1.0327，第四轮最大误差仍约 4.37 px；没有采用该尺度，没有修改 -源 URDF 或相机文件。原始公共位姿可重投回其源角点，暂未发现转换方向错误。 -上述原型和数值结果均仅保存在 `20260915_213337/review/whole_chain/`，未接入生产求解器。 - -仅用稳态共同视野核查 ID2/ID7 刚体关系时，训练 6 个姿态、留出 7 个姿态全为侧摆， -不能独立确定完整外参。连续扫描保存了两种动作的共同视野，可在原曝光时间上对齐后 -进行独立于 URDF 和曲线的外参一致性诊断；诊断结果不能自动覆盖已记录的相机参数。 - -## 第十一轮评估：尺度一致性与全机位最终图像验收 - -对 ID2/ID7 同末节约束做独立检查，不使用 URDF 尺寸、轴向或 SDK 查找表。 -连续图像按曝光时间对齐，线性/局部三次及留一插值的一致性误差不超过 0.1 px； -训练前三轮 96 个姿态，第四轮 32 个姿态仅检查冻结的参数。每帧刚体位姿作为局部未知量， -两个 Tag 之间始终只有一个固定安装变换。多初值只按训练平方误差选择。 - -| 诊断假设 | 第四轮 front/top 最大误差 | 结论 | -| --- | --- | --- | -| 记录的外参与两个 16 mm Tag | 1.28 / 1.69 px | 与原始图像仍有不一致。 | -| 自由估计 front/top 外参，Tag 均 16 mm | 0.44 / 0.48 px | 外参改变量约 2.43°/23.72 mm；条件三倍标准差约 1.96°/22.24 mm，不能作为精确外参替换。 | -| 记录外参，单独估计 ID7 尺度 | 0.84 / 0.49 px | 固定 ID2 为 16 mm 时 ID7 约 16.52 mm；不是两块 Tag 实测尺寸差的独立结论。 | -| 记录外参，同时估计两个 Tag 尺度 | 0.42 / 0.52 px | 有效边长 ID2 约 16.38 mm、ID7 约 16.52 mm；依赖现有相机模型和刚体/正方形假设，尚不能替代物理尺寸。 | - -自由外参不能解释此前保存的棋盘格图像：其最大误差约 12.93 px，原外参约 0.56 px。 -因此不能因为手指图像误差变小就认定相机移动或自动重写外参。只读采集当前静止画面后， -正面背景匹配显示约 3 px 变化，侧面约 1 px 内；顶部旧图被棋盘格遮满，没有足够共同背景， -不能判断顶部机位是否变化。该检查没有启动 SDK 或驱动手指。 - -另将整链中的所有相机基线或所有 Tag 尺寸分别只增加一个公共尺度参数：前三轮最优值 -约为 0.97226 和 1.03073。两者都明显降低拇指正面误差，但第四轮小指末节最大仍约 -4.29/4.27 px、顶部最大仍约 2.66/2.44 px；没有采用任何尺度修正。 -条件协方差未包含相机、打印尺寸和模型的系统误差，不能用其很小的数值宣称物理尺寸测准。 -已请求分别精测 ID2/ID7 黑框宽高及棋盘格跨五格长度，现有整数尺寸记录不足以区分尺度来源。 -结果在 `review/whole_chain/rigid_*_assessment.json`、`board_camera_comparison.json`、 -`static_background_comparison.json` 和 `all_view_*_scale.json`。 - -程序结构上，只有任务主机位参与最终图像验收会漏掉同一 Tag 在其他动作下的不一致。 -现将相机身份核验和原始图像索引集中在 `ImageEvidence`，反馈和稳态指令各自使用独立窗口。 -前三轮、未通过尝试和无同步反馈的原始图像不能成为最终验收测量；主观测必须在新原始记录 -中找到完全一致的角点、SDK 值和方向。其他动作中的原始角点也直接进入冻结 JSON/URDF 投影， -不伪造 PnP 位姿，不参与训练，不增加运动或停点。历史主图像读取接口保留原证据语义。 - -实际 O6 输入核对：反馈原有 1,818 个主观测全部一致，增加 3,270 个其他观测； -稳态指令原有 312 个主观测全部一致，增加 677 个观测。此结果仅证明证据绑定正确， -不代表最终空间精度通过。专项和现有产物流程 36 项通过，补充最终检查、四型号合成产物、 -JSON 重建及非有限输入反例 34 项通过；两组有交叉，不相加为独立测试总数。 -标定包构建通过；实际数据证据核对和测试日志保存在 -`review/whole_chain/all_view_final_source_verification.json` 与 `final_image_validation/`。 - -### 剩余运动残差的解释边界 - -固定上述未启用的候选相机尺度与整链几何，仅逐帧调整活动关节角度进行定位。 -小指两关节最大改变量约 0.63°/0.49°，侧摆约 0.38°；图像最大残差分别可降至 -0.43/0.94 px 和 0.47 px。最大原始残差集中在 SDK 约 244 的运动区间； -同一 Tag 静止、其他手指运动时的误差显著更小。 - -这项实验在每个被检查图像上估计局部角度，明确**不是独立第四轮验收**，也不证明 -其条件几何正确。它提示剩余像素误差中包含运动映射误差，不能据此继续修改骨长或零位。 -前三轮相同输入位置的角度改变量有约 0.1° 的轮间差，增加曲线节点也未必能消除。 -后续需分别评估反馈分辨率、允许的运动映射误差与图像测量误差；不能直接把 PnP 的 -像素测量门限等同于整条运动链的角度/位置精度要求。本轮没有修改任何验收门限。 -完整逐帧结果见 `review/whole_chain/local_angle_residuals.json`。 - -## 第十二轮评估：分离图像测量误差与运动精度 - -先用完全独立的已知真值验证规则：单关节运动存在恒定 0.5°、0.873 mm 偏差， -满足原角度和位置要求。仅将相机焦距从 1000 改为 3500 px，最终 FK 的最大投影误差 -就从 1.10 变成 3.84 px；原规则因此对相同物理精度作出不同结论。测量位姿对原始角点 -的误差均近于零。这证明 PnP 的 1.5 px 图像测量门限被误用于含运动映射误差的总投影量。 - -公共验收现在分为以下职责： - -1. 发布器原有的独立三维反馈、指令验收仍先执行，角度 MAE≤1°、P95≤2°、最大≤3°， - 位置 P95≤3 mm。非线性被动关节仍读取完整 JSON，不以 mimic 近似替代。 -2. `image_acceptance.py` 将独立位姿与原始角点、SDK 输入、方向和图像身份直接绑定。 - 测量位姿重投影 RMS≤1.5 px；最终 FK 的总投影误差保留为单独统计，不据此改变物理精度要求。 -3. 额外视角无独立三维测量时，`image_consistency.py` 查找一个能解释原始角点的局部刚性位姿， - 直接核验其像素残差及相对最终 FK 的物理位移。数值见证仅说明图像与容差相容， - **不是真实位姿测量，不证明最优，不替代独立三维精度**。求解器返回成功也不能单独使其通过。 - 不回写或重新估计共享的相机、安装、基座、零位和曲线；按运动任务分别统计，防止静止帧稀释误差。 -4. manifest 明确记录 `independent_measurement_and_physical_consistency_v2`， - 独立精度与额外机位相容性分开报告。缺失独立位姿不能由数值见证补齐。 - -解析投影 Jacobian 已用独立有限差分检查。已知真值、不同焦距、物理超差、角点/相机篡改、 -证据身份和静止图像稀释等 42 项专项检查通过；四型号合成产物、非线性、JSON 重建及原物理 -验收共 36 项通过；最后的规则版本/任务身份补充检查 15 项通过。测试集合有交叉，不相加。 -标定包构建通过，`git diff --check` 通过。没有增加实机轮次或停点。 - -最新真实 O6 的 1,818 个第四轮及 312 个稳态主观测均核验了真实保存位姿与原始角点， -最大误差分别为 0.613/0.616 px。此检查没有拟合任何参数，属于测量证据核验，不是产物 PASS。 -复算原训练安装检查仍得到拇指位置 P95 为 5.371/3.591/11.352 mm，仍失败，没有 manifest。 -尺度/相机输入尚待现场精测区分，未采用之前任何诊断尺度，也未直接替换整链求解器。 -已知真值脚本使用保存的旧规则源码复现反例，当前规则由生产测试验证；全部记录位于 -`20260915_213337/review/whole_chain/image_error_budget/`。 - -## 第十三轮评估：轴线参考点不能决定零位 - -一条转轴可用轴上任意一点表示；沿轴移动该点不会改变物理几何。当前 O6 的旧零位公式 -先把两轴参考点的差直接投到图像平面，没有消除沿轴任意坐标。相机斜看转轴时，这个 -任意坐标被误解释成弯曲零位。旧代码已为 O12 增加可选开关，但 O6/L6/G20 仍使用原公式。 - -在真实 O6 的相同轴线输入上，仅将被动轴参考点沿轴移动 ±20 mm：拇指弯曲零位从 -−9.8736° 变成 +1.0529° 或 −16.1650°，三个结果竟都通过原轴线角度检查。整个实验没有 -改变相机、Tag 尺寸、运动图像或真实轴线，足以证明程序中存在几何表示相关的错误。 - -公共 `ObservationGeometry.phase_error` 现始终先取两轴的垂直间距向量，再投影到相机平面。 -移除了 `project_axis_gauge_before_image` 型号开关及其编译参数；O12 删除已无必要的 YAML -声明并同步产品哈希，其数学行为保持原先已启用的正确规则。O6 的采集配置与受保护输入 -不变，可以用原四轮数据离线复算，无须增加运动。 - -独立测试使用四型号的原始 URDF、自行生成已知 0.07 rad 零位，改变相机视角、共同坐标系， -并任意移动预测和观测的轴上参考点；所有平行轴对均恢复相同真值。源码专项、四型号完整 -合成产物链、配置与 O12 安装/机位变化共 24 项检查通过；源代码的轴线/求解顺序 9 项通过。 -构建后另核对实际加载的安装包与源码字节一致，安装包的轴线、图像与物理门限检查共 -51 项通过，避免用旧安装文件检验新修改。各测试集合有交叉，不相加。 - -正式回放保留原始输入身份检查，输出到 `review/whole_chain/axis_gauge_production_replay/`。 -拇指弯曲零位约 −0.5384°、小指约 −1.8044°，不再依赖轴上参考点；侧摆仍约 14.5179°。 -JSON 已先保存并被读取生成候选 URDF；空间安装检查仍失败，位置 P95 如页首所列， -未生成 manifest。这项数学修正没有消除后续空间不一致，也不能替代尚待核实的尺度输入。 - -对最近两次已保存的轴线输入使用同一修正公式，侧摆、拇指弯曲、小指零位差分别为 -0.02955°、0.01044°、0.02942°。这是条件零位的重复性对照，不是绝对精度通过； -没有改写旧 v7 会话的采集策略或将其伪装成 v8 发布。结果见 `corrected_zero_repeatability.json`。 - -旧公式及配置已备份到 `axis_gauge_before/`，`assess_axis_line_gauge.py` 从备份源码复现 -历史反例；不将旧公式或型号条件保留在生产求解器中。数值对照见 `axis_line_gauge_assessment.json`， -源码、安装包测试及构建日志均在同一 `review/whole_chain/` 目录。 - -## 第十四轮评估：独立双目尺度检查 - -先使用已冻结的 ID2/ID7 刚体关系，在第四轮 32 个共同观测姿态上仅估计逐帧刚体位姿， -不使用 URDF 或 SDK 曲线。保持原相机与两个 16 mm Tag 时，front/top 图像最大误差仍为 -1.280/1.693 px；与原主机位三维测量的位置 P95 分别相差 26.84/19.09 mm。 -此前“调整外参”或“调整 Tag 尺寸”的条件模型均能把最大图像误差降至约 0.5 px， -但得到不同的三维位置，且部分与原主测量相差约 20～33 mm。因此不能将其中任意一个 -条件重建当成独立真值，亦不能用小图像残差证明最终空间精度。共享参数均未在第四轮更新。 -对照见 `stereo_primary_measurement_comparison.json`。 - -只读保存的静止画面还发现：侧面相机可以同时看见正面 ID1、ID2。用两机位对同一个 Tag -的四个对应角点直接三角测量，不预设 Tag 尺寸、刚体安装、URDF 或 LUT。两帧曝光相差 -6.55 ms，保存时没有 SDK 控制进程、机械手静止。尺度仅来自记录的 front/side 外参基线。 - -- ID1 两组对边平均长度为 **16.496 / 16.503 mm**；两机位重投影 RMS 为 **0.536 / 0.443 px**。 - 这项独立对照进一步提示当前尺度与“16 mm”的记录不一致。 -- ID2 两组对边约 **16.593 / 16.713 mm**，但重投影 RMS 为 **1.583 / 1.412 px**， - 最大单角误差约 2.28 px，不能将该尺寸估计作为已通过质量检查的测量。 - -此检查仍依赖棋盘格确定的相机基线尺度，不能独立区分打印 Tag 偏大与棋盘格尺度偏差。 -没有把 ID1 的尺寸复制给其他 Tag，没有更改原相机或尺寸配置，没有增加整手运动。 -原始 PNG、相机元数据、逐角三维坐标及误差均可复查，见 `current_camera_images/`、 -`same_tag_stereo_assessment.json` 与相应同名诊断脚本。 - -下一项必要现场证据仍是已请求的精确黑框尺寸与棋盘格跨五格长度。现有图像能发现 -尺度不一致，但不足以无假设地决定应修改哪一项物理输入。尺寸证据到位后，应先核实输入、 -评估与原始数据的兼容性，再复算整链；不能把某个局部拟合最优值直接写入生产配置。 - -## 第十五轮评估：固定已确认尺寸，排除图像坐标处理与局部拟合误导 - -2026-09-16 用户再次明确：全部 Tag 黑色码区为 16×16 mm，棋盘格为 27×27 mm。 -这解除上一轮的尺寸信息等待；本轮按用户确认值作为固定物理输入，不把双目条件估计 -当作新的 Tag 尺寸。以下诊断记录在 `20260915_213337/review/whole_chain/confirmed_dimensions_audit/`。 - -### 1. 原始图像与去畸变图像的数值核验 - -仅启动相机，机械手不运动；保存同一曝光时间戳的原始和去畸变 PNG。按 CameraInfo 的 -K、D、R、P 独立执行 OpenCV remap，front/side/top 三机位与 image_proc 输出的最大、平均 -灰度差均为 **0**。程序以 P 解释 image_rect、棋盘物点间距为 0.027 m；没有发现单位错误、 -缩放裁剪、重复去畸变或原始 K 与去畸变图像混用。该证据只核验软件坐标契约，不证明 -K/D 本身就是准确物理内参。结果见 `raw_rect_consistency.json`、`raw_rect_capture/`。 - -新静止图像中 ID1 的两组对边平均为 **16.5057/16.5078 mm**，两机位重投影 RMS 为 -**0.830/0.687 px**。ID2 的重投影 RMS 为 1.836/1.635 px，继续不作为合格尺度测量。 -因此上一轮发现的矛盾可再次观测，并非一次检测结果；见 `fresh_same_tag_stereo.json`。 - -### 2. 棋盘低像素误差不能单独决定新相机参数 - -使用已有原始角点，保持 27 mm 间距，对正面/侧面及正面/顶部训练对分别联合估计内参。 -两组的留出误差均下降，但同一 front 的主点横坐标分别约 866/1078 px，明显不同。 -又将单机位训练集分为互不重叠子集：side 的部分子集可给出明显不同焦距/主点，同时 -在同一留出图像上仍保持约 0.45～0.48 px 的 RMS。不能凭这个像素指标选择物理参数。 -结果见 `joint_intrinsics.json`、`intrinsic_stability.json`；未修改相机配置。 - -进一步以 OpenCV object-releasing 仅在训练图像中估计一个固定棋盘角点形状，保持 -第一行两端 7×27 mm 间距。三个机位独立估计的形状均存在亚毫米偏差；其中 front、side -估出的形状相近。冻结形状后，未参与拟合的单机位图像 RMS 分别从 -**0.276/0.465/0.317 px** 降为 **0.133/0.168/0.138 px**。 -这提示理想平面棋盘假设可能影响内参，但仍是条件模型证据，不等于已测准棋盘变形。 - -为避免“各相机自由拟合都能变好”的误判，另只采用 front 训练图像估出的同一形状， -重新估计 side/top 内参，再用 19:25 棋盘原角点训练新的条件外参（每四组留一组验证）。 -side 的留出双目 RMS 最大约 0.395/0.383 px；随后检查从未参加相机拟合的新静止 Tag， -ID1 两组对边仍为 **16.5038/16.5058 mm**。棋盘误差改善没有解决目标尺度矛盾，故明确 -拒绝启用这组候选参数。见 `board_shape.json`、`shape_camera_transfer.json`。 - -### 当前下一步 - -已请求保持手和相机不动，将棋盘置于手旁，使 front/side 同时看到完整棋盘及 ID1， -采集同一时段的静止画面。这可直接核查已确认的两种尺度，减少先标棋盘、后拍手之间 -场景变化的影响。另询问棋盘载体是否刚性平整，作为几何假设的现场证据。 -本轮不增加整手运动、不启用候选相机参数、不改变 Tag 尺寸或精度门限；原数据和受保护 -配置保留。整手发布条件仍未达成，无新的合格 manifest。 - -### 当日现场反馈与贴平后检查 - -用户补充“棋盘没有完全弯曲变形，tag贴纸有弯曲不平整”，随后确认“已贴平固定”。 -因此不能把之前的平面 Tag 位姿当作已证实准确的物理测量，也不能把棋盘条件拟合 -直接解释成全部误差的原因。正式复标需使用处理后的新图像，而非沿用旧 Tag 安装结果。 - -10:09 的贴平后静止对照，front/side 曝光差为 1.763 ms。ID2 的双目重投影 RMS 为 -1.349/1.201 px，较处理前改善；ID1 为 2.254/1.872 px，仍不满足图像质量门限。 -两者的条件尺度仍约 16.5 mm，不能据此宣布测量链修复完成。数据保存在 -`confirmed_dimensions_audit/flattened_tags_20260916_100921/`。 - -用户表示棋盘已摆好后进行了实际拍摄，并持续开启无机械手控制的双目预览；至 10:10 -两路画面实际均未出现棋盘。已提供正面/侧面画面帮助摆放，没有将空缺的棋盘观测标成 -成功采集。新的同帧棋盘/Tag 核验脚本已用已知真值验证：正确相机下恢复 16 mm 各边， -误差小于 1e−5 mm；该合成检查不是现场相机验证。下一步仍需让棋盘实际进入两个机位， -目前未重新启动整手运动或输出新的合格产物。诊断汇总与受保护输入哈希见 -`confirmed_dimensions_audit/audit_summary.json`。 - -## 第十六轮评估：分时核验棋盘与 Tag，取消不必要的同框限制 - -固定相机外参时，棋盘负责估计相机之间的刚体变换；Tag 的位置和姿态在检查时独立 -估计。因此棋盘与机械手不必同时出现在画面中，手在两次拍摄间也不必保持原位置。 -此前把同框设为必要条件，给现场带来了不必要的摆放限制，现已取消。只需相机及镜头 -设置在两段采集中保持固定。这一修改属于诊断采集流程，不改变整手正式运动流程。 - -10:21～10:22 保存四组正面/侧面完整棋盘原图： -`board_placement_preview_20260916_102131`、`102213`、`102215`、`102217`。 -原始双目曝光差 10.79～10.99 ms；每组以已确认的 27 mm 棋盘估计条件相机变换， -棋盘重投影 RMS 为 front 0.281～0.364 px、side 0.438～0.571 px。 -四次相对首帧的变化最大为 0.0272°、0.458 mm。姿态相近的四组只说明条件重复性， -不能充当完整多姿态外参标定，也没有写入活动相机配置。 - -临时预览的自动保存器还曾设置 20 ms 配对条件，而一次启动后的相机帧相位差约 -24.66 ms，造成没有进入检测。这是诊断脚本的问题，已采用原相机核验的 50 ms 配对 -范围，并保留真实曝光时间；正式图像与空间精度门限不变。另一个临时触发条件要求 -全部棋盘角点连续 2 秒变化均小于 0.4 px，未触发自动保存。上述四组是另行直接保存的 -原始画面，经离线图像核验后使用；不能把预览中的 `saved_count=0` 改记成自动触发成功。 -后续共面检查直接保存可见原图，在原图上评估几何质量，不先用这种触发条件丢弃证据。 - -用户移开棋盘、放回 O6 后，10:24 保存新的 Tag 图像，曝光差 11.75 ms，位于 -`joint_board_tag_20260916_102403/`。分别使用上述四组棋盘估计的相机变换，得到： - -- ID1 两组对边平均约 **16.487～16.500 mm**；自由角点双目重投影 RMS 为 - front 1.040～1.107 px、side 0.878～0.935 px。 -- 强制 ID1 为 16 mm 平面方形时，front RMS 仍为 **1.629～1.677 px**，超过 1.5 px。 -- ID2 两组对边约 **16.499～16.585 mm**；自由角点的双目 RMS 约 0.41～0.51 px。 - -新旧手位姿分别估计，不参与棋盘相机参数的拟合。使用已知真值的另一个位置/姿态 -Tag 验证过这一分时检查方法，两种来源均恢复 16 mm，误差小于 1e−5 mm。 -结果记录在各棋盘目录的 `board_tag_consistency_joint_board_tag_20260916_102403.json`。 -这仍不足以单独判定应改 Tag 尺寸、棋盘尺寸还是相机模型；没有因此运行新一轮整手运动。 - -### 共面长度比例的独立检查 - -用户提供同批备用 Tag，并把 ID1 固定在棋盘同一块板上。10:27 的三个原始正面图像 -位于 `coplanar_capture_20260916_102710/frame_00` 至 `frame_02`。图中左侧的备用 ID1 -与棋盘共面；右侧手掌 ID0 不共面，不将其按棋盘平面换算的数值解释为尺寸。 - -使用棋盘格交错的 20 个角点拟合平面单应变换，另 20 个角点检查投影误差，不使用 -相机外参、URDF、SDK 或关节曲线。原始像素直接计算的 ID1 两组对边约 -**16.474～16.478 / 16.595～16.602 mm**，留出棋盘角点 RMS 约 0.186～0.191 px。 -用当前内参去畸变后约 **16.680～16.685 / 16.593～16.600 mm**；旧内参、未采用的 -棋盘形状内参仅作预先规定的敏感性对照，结果也记录在同一报告中，不按 Tag 尺寸选模型。 - -该 ID1 接近原图左边缘，畸变模型之间仍有约 0.2 mm 的差异，所以未据此改配置。 -随后请用户将备用 ID1 移到棋盘上方中间，再核验边缘畸变的影响。 - -### 中央位置复核与实测尺寸的矛盾 - -用户将备用 ID1 贴到棋盘中间上方后，10:43 保存三个新的原始正面图像，位于 -`coplanar_capture_20260916_104349/frame_00` 至 `frame_02`。图中 ID1 位于同一底板上方， -纸张棋盘完整可见；手掌 ID0 仍不与棋盘共面，不解释其尺寸换算值。 - -沿用预先规定的棋盘交错训练/留出划分，三个图像的结果为： - -- 原始像素直接计算的 ID1 两组对边为 **16.511～16.513 / 16.594～16.602 mm**； - 留出棋盘角点 RMS 为 **0.188～0.197 px**。 -- 当前内参去畸变后为 **16.521～16.523 / 16.636～16.645 mm**，相较原始像素仅变化约 - 0.01 / 0.04 mm，不能解释相对 16 mm 的约 0.5 mm 差异。 -- 另用传统棋盘角点检测加亚像素细化，并分别使用全部角点、靠近 Tag 的三行或两行 - 拟合平面变换；不同检测方法和网格子集的边长变化小于 0.1 mm，比例差异仍存在。 - 这是同一图像上的方法敏感性检查，不是新增独立实物样本，也不是新的合格标定。 - -结果见各帧的 `coplanar_scale_assessment.json` 及汇总目录中的 -`central_coplanar_sensitivity.json`。上述长度全部以 27 mm 棋盘间距、平面棋盘及 -Tag 与棋盘共面为条件；图像本身不能无条件证明 Tag 的绝对毫米尺寸或两者物理共面。 -这项检查未使用外参、URDF 或关节拟合，不能通过增加关节拟合参数解决其比例矛盾。 - -用户随后确认 16 mm 和 27 mm 均为打印后实物测量值。保留这两个生产输入，不能将 -图像条件换算直接盖过实测值并断定打印错误。已请求测量工具、Tag 黑框宽高及棋盘连续 -五格的未取整总长度，以区分测量精度与几何假设的问题。此次仅进行静止图像与离线 -检查，没有新增整手运动，没有修改相机、尺寸或验收门限，没有生成合格发布 manifest。 - -## 第十七轮评估：依据实物复测修正 Tag 尺寸 - -用户再次量取后明确：“tag 是 16.5×16.5 mm，棋盘还是 27×27 mm”。这提供了独立的 -实物输入修正依据，与先前双目约 16.49 mm、中央共面约 16.51×16.60 mm 基本一致。 -不是从 URDF 拟合残差反推并选择一个易于通过的尺寸。 - -原先按 16 mm 计算 16.5 mm 的方形 Tag，会在同一单目姿态解下将平移距离缩小到 -16/16.5，约低估 3.03%；跨相机变换中的基线平移来自 27 mm 棋盘，并不会随之缩放。 -这构成明确的输入尺度不一致，不能通过增加运动曲线参数来合理消除。 - -修改范围仅为当前 O6 的以下配置及指纹： - -- `profiles/o6_right_8.yaml`：8 个 Tag 均显式声明 `size_m: 0.0165`。 -- `o6_right_8_tags.yaml`：检测器默认尺寸和每个 ID 的尺寸均为 0.0165 m。 -- `o6_three_camera_calibration.yaml`：标定节点尺寸及全部 ID 覆盖值同步为 0.0165 m。 -- `o6_right_product.yaml`：更新以上三个受保护文件的 SHA-256。 - -程序已有 Profile、检测器和标定节点的尺寸一致性检查，本次无需增加型号特判或 -修改拟合算法。其他型号/打印批次没有新的实测依据，不自动替换其配置。棋盘尺寸未变， -没有因此修改相机内外参、源 URDF、旧采集文件或验收门限。非线性关系仍由完整 JSON -和修正 URDF 配合表达。改前配置与改后指纹见 -`20260915_213337/review/whole_chain/tag_size_correction_20260916/`。 - -28 项配置加载、实际 launch 参数和 PnP 参数测试通过；已重新构建安装，核实四个 -配置文件与源码逐字节一致，`--validate-only` 通过。用户移开棋盘及备用 ID1 并确认 -O6 可运动后，启动新会话 `20260916_105528`，显式 `--no-resume` 完整采集。 -最终是否合格须以该会话的独立验收及发布文件为准，不能仅凭尺寸改正确就声明成功。 - -### 新会话结果与收尾内存问题 - -36 个采集单元全部通过，小指第三轮 increasing 方向第一次存在反馈分箱空白 42， -按原最大空白 16 的门限自动重扫一次后通过。完整采集约 6 分 47 秒；保存 15,167 个 -原始图像记录、6,828 个关节样本及 15,629 个 SDK 反馈样本。11:02:20 完成最后采集单元， -随后开始安全回位与收尾。11:05:28 系统内核明确记录 OOM、4 GiB swap 耗尽，终止了 -一个 VS Code 进程；三相机同时出现长时间时钟读取中断，SDK 反馈超时，终端随后发生 -BrokenPipeError。不能把该现象归因于机械手硬件，也不能将此会话记为在线无暂停完成。 - -原始日志完整保留，正式 `--offline-raw` 流程验证了配置指纹、全部采集单元、运动来源、 -独立第四轮与稳态验证数据，生成 `offline_165mm/` 下的 JSON、URDF、报告和 -`release_manifest.json`，退出码 0。JSON/URDF 的 SHA-256 与清单相同；已从保存 JSON -再独立调用重建 CLI，得到逐字节相同的 URDF。原来的角度 MAE/P95/max 1°/2°/3°、 -位置 P95 3 mm 及对应图像门限没有修改。 - -同时发现 `read_journal_prefix` 先整文件读取、再 `splitlines()`，在解码对象之外还保留 -两份原始文本。这份日志为 318,772,511 字节，额外副本会显著增加相机仍运行时的收尾 -峰值。已改为按冻结字节边界逐行解析，保留全部记录、顺序和不可越过边界的校验； -不通过丢数据或修改安全超时规避问题。新增 16 MiB 日志的临时缓冲内存上限回归检查， -并验证短文件与非对象记录拒绝；独立进程、取消、真实 finalizer 等共 9 项测试通过。 -已重新构建安装，进一步用同一实采数据检查独立收尾进程与已验收文件的一致性。 - -修改后的已安装独立进程以 `require_motion_evidence=True` 处理同一份完整原始数据， -约 **55.21 秒**完成，子进程峰值 RSS 为 **1,344,164 KiB（约 1.28 GiB）**； -JSON、URDF 两个文件的 SHA-256 均与正式离线验收版本相同。该检查没有启动相机或 SDK， -也没有重新运动机械手，不能单凭它宣称修复后在线全过程已经完成第二次实机验证。 -报告为 `20260916_105528/isolated_finalization_verification.json`。 - -通过已验收文件的字节/哈希与 JSON→URDF 对应关系复核后，使用现有发布器将相同文件 -发布到产品根目录的 `20260916_105528_verified_165mm/`，并更新 `latest_partial_passed`。 -原始中断会话、离线验收目录与独立进程检查目录全部保留；没有把原始 PAUSED 状态改为 -COMPLETE。正式目录中的 `release_origin.json` 记录已验收来源；完整结论和实际采集范围 -见 `20260916_105528/verification_summary.json`。当前布局直接测量拇指和小指共 5 个关节， -食指、中指、无名指共 6 个关节的运动曲线按原配置从小指迁移，不宣称这些关节独立实测。 - -## 第十八轮评估：被动关节由 JSON 提供角度，消除线性导出冲突 - -用户指出拇指 IP 在修正 URDF 中明显弯得更小。复核确认，原 URDF 的线性倍率为 1.86， -旧修正产物的倍率为 1.16556243135602，而实测 JSON 的 IP/pitch 比例随行程约从 1.71 -变化到 1.94。父子下限分别为 -0.00172483145889436、-0.0020103987491093 rad， -旧“共同零点＋整段不越界”约束把这两个小负端点的比值变成全行程倍率上限。 -这不是非线性 JSON 的拟合失败，而是将独立实测曲线再次约束为一条直线的导出设计问题。 -旧完整 JSON＋自定义 FK 验收虽通过，标准 ROS mimic 仍会覆盖被动角，因此显示与验收不一致。 - -评估后采用统一的 JSON 角度来源规则，不修补 O6 的某个倍率： - -- 新 v3 修正 URDF 移除原被动关节的 ``，全部关节角由 JSON 明确给出。 - 父子链、转轴、连杆、零位与限位保持原有受检语义；原被动关节没有新增实机驱动通道。 -- `urdf_correction` 升为版本 2，保存 `joint_angle_source: calibration_json`、 - `passive_joint_sources` 和明确的删除操作。只有完整覆盖运动关节、来源与原 URDF 一致、 - 被动关节与来源关节共享 SDK 通道时，才能授权移除；其他数值编辑仍使用 Profile 原授权。 - 导出版本与采集配置分开，未修改 Profile 指纹或原始记录来绕过保护。 -- JSON 先保存，重新读取后重建 URDF；普通 FK、角度/位置验证及原始图像验收使用该最终文件。 - 标准 `robot_state_publisher` 可直接接收全部关节状态，不需要忽略 mimic 的定制算法。 -- 线性拟合仅保留报告诊断，标记 `diagnostic_only_not_exported`，不再按限位限制拟合倍率， - 不参与导出和运行角度。旧 correction v1 的精确重建和旧读取方式继续兼容。 - -专项回归覆盖微小负限位、非线性行程、普通 FK、JSON 重建字节一致性、错通道、缺失曲线、 -非法来源、未经授权删除与拓扑改动拒绝。真实 ROS TF 测试在独立命名空间验证三个弯曲点， -未连接 SDK；末节角度不再被覆盖。O6/G20/L6/O12 的合成完整收尾、独立图像验收、迁移来源、 -双向映射及旧格式检查通过；这不是其他三型号的新增实机精度证明。 - -已构建安装，并用 `20260916_105528/raw_samples.jsonl` 的完整 16.5 mm Tag 实采数据 -执行正式离线回放与发布，退出码 0。新目录为 -`calibration_output/O6_RIGHT_001/20260916_123347_json_driven_165mm/`， -`latest_partial_passed` 已更新。原来的角度、位置和原始图像门限全部通过，没有放宽。 - -对新旧发布文件逐项比较：11 个关节的全部指令表、反馈映射、零位和限位不变； -URDF 的结构差异仅为移除 5 个线性 mimic。重新读取新 JSON 重建得到逐字节相同的 URDF。 -相同输入的拇指结果如下（单位：度）： - -| SDK 指令 | CMC pitch | 旧 URDF 线性 IP | 新 JSON＋URDF IP | -| --- | ---: | ---: | ---: | -| 192 | 9.3781 | 10.9307 | 16.0523 | -| 128 | 18.8914 | 22.0191 | 33.0907 | -| 64 | 27.2887 | 31.8067 | 49.6742 | -| 0 | 34.2391 | 39.9078 | 66.3515 | - -复核脚本与结果保存在原会话的 `verify_json_driven_export.py`、 -`json_driven_export_verification.json`，正式回放日志为 `json_driven_replay.log`。 -旧发布三文件的哈希未变。本次没有重新运动机械手，沿用原 5 个独立观测关节、6 个迁移关节 -及 CAD 零位假设的精度边界。使用新产物时须通过 JSON 同时驱动全部关节；单独拖动 URDF -父关节滑条不再自动联动末节，外部仿真也须移除与 JSON 冲突的旧线性更新逻辑。 - -## 第十九轮评估：按用户要求保留标准 mimic 近似联动 - -用户明确使用 `https://viewer.robotsfan.com/` 直接预览 URDF，并进一步说明： -非线性关节也必须保留 `` 自动联动，单独 URDF 尽量接近实机； -需要实测非线性结果时再由标定 JSON 查找表控制。上一轮删除 mimic 的正式导出方向 -不符合这一使用要求。已核对网站当前构建及官方 URDFAdapter：滑条直接设置 URDF 关节， -没有本项目标定 JSON 的读取逻辑，不能期望它自动获得 JSON 中的非线性曲线。 - -### 方案评估与实现 - -标准 mimic 使用 `q_child = a*q_parent+b`。将 `b` 强制为 0，再约束父子全行程， -使拇指的两个微小负端点决定全局倍率上限，这是旧版 1.16556 倍率失真的原因。 -仅恢复 ``、手填倍率或扩大限位都不能解决模型与约束之间的矛盾。 - -现在使用两端子关节角度作为拟合变量： - -- 父关节输出范围 `[p0,p1]` 保持不变，拟合 `c0=q_child(p0)`、`c1=q_child(p1)`。 -- 两个变量分别受子关节原输出限位约束；区间内的线性插值自然不会越界。 -- 由 `a=(c1-c0)/(p1-p0)`、`b=c0-a*p0` 导出标准 mimic 倍率和偏移。 -- 沿用前三轮成对视觉样本与稳健损失拟合，第四轮只验证；不改变 SDK→角度表、 - 坐标零位、Tag 安装、相机参数或训练支持的限位。 -- `b` 是线性近似在零点的误差，不是新测出的机械零位。报告同时保存 - `baseline_error_rad`、`physical_zero_correction: false` 和实际残差。 - -生产 `urdf_correction` 使用版本 3，并声明 -`mimic_policy: range_bounded_affine_approximation`、`joint_angle_source: calibration_json`。 -仍先保存 JSON，再读取生成 URDF。所有原 mimic 来源与 XML 拓扑保留； -数值修改继续使用已有 Profile 授权，没有更改采集指纹。版本 1、2 产物保留重建兼容， -不原地覆盖。完整 JSON 回放显式使用非线性被动角，URDF 单独回放使用 mimic； -两者分别报告精度，线性近似不能冒充 JSON 的实测精度。 - -专项检查覆盖微小负限位不压低全程倍率、正负运动方向、全行程限位、留出样本禁止参与拟合、 -重建字节一致性及缺失/降级导出规则拒绝。真实标准 ROS TF 检查只给父关节角度， -验证被动关节按导出 mimic 联动；历史无 mimic 版本的独立关节测试也继续通过。 - -### O6 实采数据复验与发布结果 - -相关 121 项检查通过，覆盖四型号合成产物、迁移来源、双向表、历史格式及标准 ROS 联动。 -构建安装后，以同一份完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0， -生成并发布 `20260916_124718_mimic_json_165mm/`，更新 `latest_partial_passed`。 -JSON＋URDF 的角度、位置及独立原始图像验收通过，原精度门限不变。 - -新旧三版的全部 11 关节指令/反馈曲线、几何零位、限位逐项一致；5 组 mimic 来源与 -原始 CAD 相同。拇指 IP 倍率为 **1.86604745134131**，偏移为 -**0.00120821859875384 rad（0.06923°）**。小指及三个迁移末节的倍率为 -**0.870364582419037**，偏移为 **0.0348981257618717 rad（1.99952°）**。 -这些偏移是近似关系在零点的残差，完整 JSON 的 baseline 仍为 0。 - -| SDK 指令 | CMC pitch | 旧 URDF IP | 新 mimic IP | JSON IP | -| --- | ---: | ---: | ---: | ---: | -| 255 | 0.0000° | 0.0000° | 0.0692° | 0.0000° | -| 192 | 9.3781° | 10.9307° | 17.5691° | 16.0523° | -| 128 | 18.8914° | 22.0191° | 35.3214° | 33.0907° | -| 64 | 27.2887° | 31.8067° | 50.9912° | 49.6742° | -| 0 | 34.2391° | 39.9078° | 63.9611° | 66.3515° | - -相对 JSON 的 256 项指令网格，拇指线性近似最大差约 **2.3905°**，小指及对应迁移末节 -最大差约 **4.1965°**。这是两个表示之间的差异，不是新增的实机独立精度测量； -独立留出结果分别保存在 manifest 的 `full_json_urdf_holdout` 和 `urdf_mimic_approximation`。 -因此网站中的联动预览仍是近似，需要实测非线性结果时必须使用 JSON。 - -另行读取新 JSON 重建得到逐字节相同的 URDF。所有旧发布文件哈希不变， -没有重新运动机械手，也没有改写旧采集结果。复核脚本、结果及正式回放日志分别为 -原始会话下的 `verify_affine_mimic_export.py`、`affine_mimic_export_verification.json`、 -`affine_mimic_replay.log`。本次仍只独立观测拇指、小指 5 个关节，其他 6 个关节沿用原迁移关系。 - -## 第二十轮评估：全型号保留原始 mimic，实测非线性由 JSON 提供 - -用户确认统一采用原始 mimic 作为基础联动，暂时取消正式标定中的 mimic 参数优化。 -该规则替代第十九轮新产物的拟合策略；历史产物保持原样及原有读取语义。 - -### 全型号依据与边界 - -检查 L6、O6、O12、G20 的左右手原始 URDF，以及当前四套右手产品配置。 -L6/O6 部分曲线从小指迁移，O12 包含两级 mimic 链和无名指曲线迁移，G20 配置覆盖全部实测关节。 -左右手系数不能互相套用：O6 拇指左右分别为 2.29/1.86，G20 为 1.02/1.03。 -原始文件也不等于限位自洽：按原始右手父关节全行程，L6 拇指约超出子限位 3.6795°， -O12 无名指中节约 0.7162°，G20 四个 DIP 约 0.4297°。这些是文件内冲突，不是硬件越限结论。 -O6 的已声明实测范围策略不能自动授予其他型号放宽 CAD 硬限位的权利。 - -### 统一实现 - -- 新 correction schema v4：`mimic_policy=preserve_source_mimic`，`joint_angle_source=calibration_json`。 -- measured 正式拟合及收尾不再调用 mimic 参数优化，原 mimic 来源、倍率、偏移及省略属性全部保留。 -- `source_mimic.py` 统一计算负倍率、非零偏移及串联关系的可达范围，不含型号分支。 -- `mimic_envelopes` 单独记录基础范围、联动可达范围和最小导出包络；不会改写实测范围证据。 -- 包络扩展只适用于已有实测范围策略；默认 CAD 硬限位和独立安全限位继续生效，冲突明确报告关节及范围。 -- 零位仍修正 origin。原始 mimic 只作为修正坐标中的预览近似，精确 FK 使用 JSON 提供的各关节角。 -- 新 JSON 禁止请求 mimic 参数修改或删除；最终验收对照受保护原文件核对 mimic 元素和可信范围证据。 -- correction v1/v2/v3 继续兼容重建与读取，既有原始记录和配置指纹不变。 - -### 验证与 O6 发布 - -两组相关检查分别 **68 项、72 项通过，共 140 项**。覆盖八份左右手源文件、负倍率、零倍率、 -非零偏移、多级链、独立安全界限、非法 JSON 修正、历史版本重建及标准 ROS 的父关节联动。 -四型号合成完整流水线同时验证：禁止调用 mimic 优化函数，仍能完成 JSON→URDF→独立角点验收→发布→读取。 -这些合成检查不代表其他型号已经完成实机标定。 - -构建后以完整 `20260916_105528/raw_samples.jsonl` 正式离线回放，退出码 0，发布： -`20260916_134154_source_mimic_165mm/`，`latest_partial_passed` 指向该目录。 -5 个 mimic 元素与原始 O6 URDF 一致，拇指 1.86、其余四个末节 0.89，offset 全为 0。 -所有 11 关节指令/反馈查找表、几何零位、实测范围证据与前三版逐项一致，旧产物哈希不变。 -相对上一版 URDF，除 mimic 参数外仅 IP 下限改变： -`−0.0020103987491093 → −0.00320818651354352 rad`，约增加 **0.068628°** 的联动兼容范围。 - -SDK 指令 0 时，拇指 pitch 为 34.2391°，原始 mimic 预览 IP 为 63.6848°，JSON IP 为 66.3515°。 -完整 JSON＋URDF 的拇指 IP 独立角度 P95 为 0.43466°、位置 P95 为 2.11692 mm； -小指 DIP 分别为 1.87341°、1.94512 mm，原精度门限未改。 -独立图像验收的门限、样本数量和通过结论保持不变；重复数值优化的末位浮点值及诊断项可能变化。 -读取保存的 JSON 重建得到逐字节相同 URDF；本次没有重新运动机械手。 - -复核材料位于原始会话目录：`verify_source_mimic_export.py`、 -`source_mimic_export_verification.json`、`source_mimic_replay.log`。 -当前 8 Tag 布局仍只独立测量 5 个关节，其余 6 个按既有声明迁移；在线采集曾有 OOM 的边界保持原记录。 diff --git a/src/linkerhand_calibration/CALIBRATION_STABILITY_REVIEW_20260920.md b/src/linkerhand_calibration/CALIBRATION_STABILITY_REVIEW_20260920.md new file mode 100644 index 0000000..bfa5ebb --- /dev/null +++ b/src/linkerhand_calibration/CALIBRATION_STABILITY_REVIEW_20260920.md @@ -0,0 +1,71 @@ +# O30 右手标定稳定性与产物正确性审查 + +本次只分析并修改软件，没有启动 SDK、相机或机械手运动。实机指令下发后反馈不变的问题按硬件问题排除。原始日志、断点、Profile、保护输入哈希及运动顺序均未改写。 + +## 结论 + +流程确有软件层面的稳定性缺陷，不能把历次暂停全部归因于标签或硬件。本次修复准备观测准入、短暂缺帧的恢复分类，并统一在线与离线的完整采集验收入口。已有的姿态可观测性、独立留出验证及 JSON/URDF 回读要求继续保留。 + +“少暂停”与“结果正确”需要分别处理：短暂坏帧应在采集层过滤或使用有次数上限的重试；真正无法区分的三维姿态、移动的固定参考、改变的标签安装或不完整数据，应明确拒绝生成通过产物。相同机位重复同一动作并不一定能提供区分姿态所需的新信息。 + +## 已定位并修改的问题 + +| 问题 | 原有行为及影响 | 本次修改 | +| --- | --- | --- | +| 无效观测覆盖有效准备帧 | 每个指令区间保存最后一帧；后来的空候选或不合法候选可以覆盖先前有效帧，随后整个运动求解被拒绝 | 在写入区间缓存前，复用求解器已有的候选质量检查；图像求解所需的根参考必须已冻结。只有完整有效帧才能替换缓存，原始坏帧仍保存在日志中 | +| 缺帧错误未进入重试策略 | 底层返回 `insufficient_image_frames` 或 `motion_candidates_missing_or_invalid`，已有恢复策略却只识别另一部分错误名称 | 将这些暂态错误纳入同一个恢复策略，每个尚未建立零位的任务/关节组最多重新执行一次准备动作；持续失败仍暂停 | +| 在线与离线的收尾检查边界不一致 | 离线入口检查完整扫描单元，在线直接进入最终拟合；若恢复数据只有旧通过标记或最新尝试不完整，缺少同一个入口检查 | 抽出 `runtime/artifacts/capture_validation.py`，最终生成入口统一检查配置身份、固定参考、相机内参、每个单元最新尝试及实际样本质量；两种产品入口均要求采集证据 | +| 大日志的重复扫描和额外内存 | 每个单元都遍历完整日志；离线计算日志哈希时额外一次性读取整个文件 | 按单元、任务各建一次索引，复用原有质量规则；日志哈希改为流式计算 | + +候选准入只使用预先规定的检测质量条件，不根据拟合后的误差选择保留帧；保留的两种候选均进入原有比较，不填补不存在的另一种候选，也不改变最终精度阈值。准备记录增加 `preparation_observation_rejections`，可查到被拒绝观测的原因与数量。 + +若一个最新尝试同时存在通过与失败标记，拒绝通过；若只有较早尝试完成、较新尝试未完成，也拒绝通过。失败原因写入 `capture_validation.json`，并在拟合前返回，不能更新 `latest_passed`。 + +## 真实日志验证 + +### 无名指 MCP 准备阶段 + +来源：`calibration_output/O30_RIGHT_001/20260919_204235/raw_samples.jsonl`，侧面机位、运动版本 159。 + +- 原报告选择了 105 帧，其中 8 个区间的最终帧存在空候选，整体报 `motion_candidates_missing_or_invalid`。 +- 从原日志抽取该段 430 条侧面观测，保存为 `test/fixtures/o30_ring_preparation_admission.json.gz`；不修改其角点、指令、反馈或候选。 +- 新准入规则拒绝 14 条不合格候选观测，保留 97 个有效运动区间，求解不再因整段混入空候选而失败。 +- 这段历史数据还存在候选族不完整和图像重投影失败，最终仍未授权模型。该验证证明准入缺陷已消除，不能用来宣称这段实机标定已经通过。 + +### 已保存的 96 个单元 + +来源：`calibration_output/O30_RIGHT_001/20260919_214048/raw_samples.jsonl`。 + +- 保存的会话具有唯一启动记录、唯一固定参考和三个相机模型；与当前产品的保护输入哈希及采集策略兼容。 +- 离线提取日志中实际用于覆盖率和稳态质量检查的字段，执行相同单元验收：前 96 个单元通过，第 97 个 `middle_dip_side / cycle=0 / increasing` 因未采集而拒绝；没有将历史断点视为完整产物。 +- 该检查不拟合历史图像，不修改断点；不是完整运动来源与最终图像验收的替代品。 +- 正式断点启动仍使用 `passed` 恢复，直接复用通过单元。整套数据的完整检查放在最终生成阶段，不加入恢复启动路径。 + +## 目前合理且继续保留的边界 + +1. 几何求解与当前零位图像采集分别计时。已有实现会在参考复用后开启独立采集窗口，避免求解或加载耗时吃掉零位采样时间；此次未重复修改这一已修复逻辑。 +2. 已冻结零位不能被局部重试替换；已成功安装部分机位模型、保持关节发生移动或固定参考冲突时，不自动重走准备动作。 +3. 三维候选族无法区分时仍不授权。之前对正面、侧面共享标签、测得的父关节几何及其不确定度的修正继续保留；没有把父几何当作绝对真值。 +4. 运动顺序仍是拇指、四指屈伸、四指侧摆。恢复不回到整手起点；本次没有增加未声明的辅助动作。 + +## JSON 与修正 URDF 的正确性检查 + +正式产物必须依次通过： + +1. 保护输入、相机和固定参考身份检查；所有应采集单元的最新尝试完成，样本覆盖率、稳态节点和训练/验证输入范围合格。 +2. 零位、运动模型、跨会话来源及共享标签证据一致；训练数据与独立验证图像不重叠。 +3. 方向分别拟合的指令映射和零位通过独立留出检查，不能用训练拟合误差替代验收。 +4. 先写标定 JSON，再从落盘 JSON 重建修正 URDF，回读核对两份文件对应关系。 +5. 检查授权修改范围、运动学、角度和最终文件对应的独立像素重投影；标准 ROS URDF 加载检查通过后才允许发布。 +6. 发布控制器复核候选文件哈希和取消状态，最后更新通过指针。采集完整性检查通过不等于整套产物已通过。 + +O30 当前契约有 20 个独立主动关节；末节 DIP/IP 的五个 CAD 零位假设必须在产物中明确保留其来源，不能写成五个已经独立实测的绝对零位。本次不改变这一契约。要声称这些绝对零位也经过实测，还需要相应独立观测。 + +## 软件验证范围 + +- 新增回归覆盖坏帧覆盖、全部坏帧、未冻结根参考、保留两种候选、真实无名指准备数据，以及在线/离线收尾对失败单元、未完成新尝试、配置和内参变化的拒绝。 +- 恢复回归覆盖实际底层缺帧错误、一次重试后成功和持续失败后暂停；继续检查已通过单元复用、异步求解和零位窗口。 +- 产物回归覆盖 O30 20 通道非线性映射、独立角点验证、JSON 重建 URDF、授权字段约束、进程收尾与发布取消。 +- 共 198 项不同的相关回归测试通过，其中新增 18 项用例/参数组合。首批 28 项通过，综合批次 181 项通过（504.86 秒），去除重叠后为 198 项；最终收尾检查修改后又单独复跑 20 项，全部通过。Python 编译与 `git diff --check` 通过。 +- 软件回归及合成数据验收不代表实机精度已达标；当前仍没有完成整手最终产物验收。 +- 安装环境解析到当前源码目录，新增的共享验收模块可直接导入；没有启动实机程序验证。 diff --git a/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md b/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md deleted file mode 100644 index 6f41fa6..0000000 --- a/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md +++ /dev/null @@ -1,230 +0,0 @@ -# 按关节零位与实测/复制 JSON v3 实施记录 - -更新：2026-09-15。范围：`linkerhand_calibration`。 - -## 当前收敛结果 - -- G20、L6、O6、O12 右手共用四轮连续扫描，随后执行独立的稳态指令映射训练与验证。原精度门限保留。 -- 正式入口在启动硬件前检查运动覆盖与绝对零位来源。O12 的 `thumb_mcp`、`pinky_pip`、`ring_pip` - 按用户确认沿用原始 URDF 零位，记录为 `assumed_source_cad_zero`；运动继续实测或按声明复制。 -- 收尾先保存 JSON，再由 `runtime/artifacts/urdf_from_json.py` 读取它重建 URDF。 - 发布器检查修正元数据能否逐字节重建最终文件；可独立使用 `rebuild_calibrated_urdf` 命令。 -- 四型号的合成原始观测均已通过实际拟合、文件生成、验收、发布及读取器加载。 - 合成夹具使用明确的几何真值与范围，这不代表四种实机均完成验证。 -- O6 会话 `20260915_134211` 实采 36/36 单元一次通过，无重扫、无采集中断，采集耗时 412.35 秒。 - 同一原始日志已生成候选 JSON 与修正 URDF，独立 JSON 重建得到相同字节；整链几何验收仍失败，未发布。 - 最大 Tag 位置 P95 为 15.32 mm，小指末节姿态 P95 为 7.76°，不能称为整手精度合格。 -- 随后用棋盘格重新测得 O6 三相机外参：两组各 15 个姿态，最大联合重投影 RMS 为 0.928 px， - 最大重复平移差 0.341 mm、旋转差 0.043°，通过原门限。新配置为 - `config/o6_three_camera_extrinsics_20260915.yaml`；旧数据保留原外参身份，不混用。 -- 新外参会话 `20260915_144514` 完成拇指 24/36 单元后，小指准备求解有两个候选耗尽 150 次迭代。 - 同一批 120 帧离线复现确认是内部 LSMR 线性求解精度不足；调整 `atol/btol` 为 `1e-10` 后， - 四候选均在原 150 次预算内收敛。外层收敛条件、分支统计和物理精度门限未变。 - `test_recorded_pinky_alternatives_converge_within_original_budget` 保存该实采回归；相关 34 项测试通过。 -- 恢复会话 `20260915_145437` 暴露旧日志在运动回调中校验导致的反馈超时报错：离线复现读取约 - 1.41 秒、完整凭据/单元校验约 13.88 秒。`PreparedResume` 将静态校验移到回调启动前； - 当前固定基准与逐关节安装仍在现场独立验证。后续 `20260915_145748` 暴露一次性导入大日志 - 仍会阻塞；改为每个控制周期最多导入 64 条记录，全部持久化后才确认复用。 - 恢复相关 73 项测试通过，包含导入期间持续处理反馈、接收中止及落盘前不得跳过单元；原 1 秒反馈保护未变。 -- 图像模型零位恢复还需同步导入该零位的 `pnp_candidate_frame`，不能只复制 `joint_zero_sample`。 - 已修复该来源闭合问题；真实中断恢复会话离线核对发现恰好漏掉 10 帧零位图像，按新导入逻辑保留后 - 原凭据检查通过。原始日志未修改。相关 74 项测试通过，包含连续两次恢复的零位图像回放。 -- 新外参完整会话 `20260915_150015` 已完成 36/36 单元，采集中无暂停，两次自动重扫均通过。 - 生成的候选 JSON 可独立重建出字节相同的修正 URDF，并通过标准 ROS URDF 加载。 - 整链训练阶段的刚性 Tag 安装检查仍失败：拇指 IP 位置 P95 为 9.73 mm，小指 DIP 姿态 P95 为 9.40°。 - 这不是第四轮整链精度通过的结果，未生成发布 manifest，原始 URDF 未覆盖。 - 用户已确认 ID4/ID5 安装在平整硬片上，Tag 黑框为 16×16 mm，棋盘格单格为 27 mm。 - 尚不能据这些残差判定硬件故障。 -- 随后完成三相机原始棋盘格内参核验：正面 34、侧面 25、顶部 24 个不同姿态； - 每第 4 张预先留作独立验证。验证重投影 RMS 从旧内参的 0.880/0.955/0.484 px， - 降至 0.276/0.465/0.317 px。新内参保存在 `config/o6_camera_intrinsics_20260915/`。 - 配套外参使用正面/侧面 25 组同步观测和补采的正面/顶部 15 组同步观测，均无剔除； - 最大 RMS 0.575 px、旋转重复差 0.024°、平移重复差 0.269 mm，通过原门限。 - O6 产品配置已指向 `config/o6_three_camera_extrinsics_intrinsics_20260915.yaml`， - 并通过无运动的配置及受保护输入校验。旧相机文件与旧整手记录保留原身份。 - 原图、角点、训练/验证划分、拟合报告及导出哈希保存在 - `calibration_output/O6_RIGHT_001/camera_intrinsics_audit_20260915/`。 - 相机核验通过后仍须新采整手数据，不能将旧整手会话改挂新相机参数发布。 -- 新相机完整重采 `20260915_154743` 的拇指 24/36 单元全部完成，无重扫；小指准备的四候选 - 均在 26–36 次迭代内收敛，但候选区分证据不足，停在 `image_motion_families_not_distinguishable`。 - 随后发现统一 launch 将四型号受保护 Tag 配置中的 `detector.decimate=1.0` 强制覆盖为 `1.5`。 - 已移除这层未声明在产品配置内的覆盖，检测设置统一来自受保护 YAML；不修改候选区分或物理精度门限。 - 新回归实际展开 ROS 组件参数，修复前四型号均复现 `1.5 != 1.0`；修复后启动及 runner 的 55 项测试通过。 - 构建和 `git diff --check` 通过；旧降采样记录不混入新结果。 -- 原分辨率完整重采 `20260915_160040` 已从三台运行节点读回 `detector.decimate=1.0`。 - 拇指 24/36 单元一次通过，无重扫；小指准备的 104 帧、四候选均在 21–35 次迭代内收敛， - 仍因 `image_motion_families_not_distinguishable` 暂停，未生成本次完整候选 JSON/URDF。 - 离线回放中两个未分开的 ID4 姿态旋转差 P95 为 18.64°，不能当作同一解； - ID4 法线与相机光轴夹角在两解下分别约 6–8°、10–11°，运动中一直接近正视。 - 已请求保持相机、手掌和 ID3 不动，将 ID4 硬片垫斜后重新采集；这不是硬件故障结论。 - `review/all_view_detections/` 保留本次后 200.7 秒的三机位检测与 SDK 消息。 - 其中侧摆期间同时看到 ID1/ID2 的匹配样本仅 9 帧、反馈范围 206–255， - 不足以判断整个侧摆行程是否引起被动关节联动。所有精度门限保持原值。 -- 上述修复后的四型号文件链、JSON 重建、恢复凭据和图像求解定向回归共 55 项通过（76.55 秒）， - 构建通过，`git diff --check` 通过；142 个原始 URDF/mesh 文件仍与 HEAD 相同。 - -本次生成测试见 `test_all_model_artifact_pipeline.py` 和 `test_json_urdf_rebuild.py`。 -O6 最新采集及暂停证据在工作区 `calibration_output/O6_RIGHT_001/20260915_160040/review/`； -最近一次完成采集并生成候选文件的证据在 `calibration_output/O6_RIGHT_001/20260915_150015/review/`； -旧外参会话证据保留在 `calibration_output/O6_RIGHT_001/20260915_134211/review/`。 -下方分批测试数字为各次改造的历史记录。 - -2026-09-15 最后一次全包回归覆盖 1424 项：1408 项直接通过、15 项旧夹具兼容问题、1 项缺少实测记录跳过。 -这些失败项已修复，按失败清单重跑 15/15 通过，没有遗留失败；这是全量运行加失败项复测的结果。 -O12 使用当前 CAD 假设的完整生成/验收测试另行通过。构建与四型号安装入口检查通过， -142 个原始 URDF/mesh 文件与 HEAD 内容一致。 -完整日志和复测日志保存在上述 O6 `review/` 目录的 `linkerhand_final_full_tests.log`、 -`linkerhand_final_rechecks.log`；`software_validation.json` 汇总了验证范围。 - -## 实现与坐标约定 - -`q_CAD = q_output + delta`。每个关节自己的实机 baseline 对应 `q_output=0`， -不把某次局部参考的完整手姿态解释为全手零位。 - -生产流程仍为统一 runner、ROS 接线、协调器、任务执行器、公共拟合与发布器。 -新增型号通过 Profile、SDK Adapter、Tag 观测关系与避让动作接入，没有新增型号专用算法。 - -| 职责 | 实现 | -| --- | --- | -| 显式关节零位配置、已知几何依据 | `core/domain/profile.py`、`profiles/loader.py`、`profiles/validator.py` | -| 不可变零位证据及持久化校验 | `core/domain/reference.py` | -| 避让后按声明姿态采集零位 | `runtime/execution.py`、`runtime/motion_execution.py`、`runtime/capture.py` | -| 会话/运动版本核对及冻结提交 | `runtime/coordinator.py` | -| 原零位核验后逐任务恢复数据 | `runtime/joint_resume.py` | -| 冻结参考下的相对运动与指令映射 | `core/fitting/motion_fit.py`、`command_mapping.py`、`anchored_mapping.py` | -| 明确声明的运动复制、零位迁移及近似来源 | `core/fitting/motion_transfer.py` | -| 独立线性 mimic 近似 | `core/fitting/mimic_approximation.py` | -| 空间零偏及授权 URDF 修正 | `core/fitting/session.py`、`spatial_solver/`、`core/urdf/result_plan.py` | -| v3 序列化、读取和最终文件验收 | `runtime/artifacts/`、`core/urdf/measured_acceptance.py`、`tag_acceptance.py` | - -Start 清空预览后只锁固定参考。运动 Tag 轮到相应关节时才成为必需观测。 -Profile 必须声明完整零位指令和非零的最终到达动作；扫描起点不等于 baseline 时先访问零位。 -参考保存完整指令、反馈、到达方向、父子相对/公共位姿、图像身份、触发任务和会话/运动版本。 -其他手指的非零避让姿态保持在证据中。无法分离同通道多个关节影响的观测在启动检查时拒绝。 - -同一关节只冻结一次。重复任务、方向、轮次、重扫及恢复不替换参考。 -恢复先验证整场指纹，再在各关节声明姿态验证旧安装，保留原参考身份，通过后才导入已完成单元。 -原生 rad 输入的第一轮行程统计也随已验证单元恢复，避免跳过第一轮后丢失覆盖依据。 -迟到的预览、暂停前观测或旧运动结果不能进入当前参考。 -零位稳定性判断使用各机位最近的足量独立帧，短暂异常帧会随新帧退出判断窗口； -原始记录保留，稳定性门限不变。 - -JSON 关节来自自己的视觉样本，或 Profile 明确声明的对应实测来源；同一 SDK 通道允许绑定多个关节。 -接收关节保留自己的 SDK 通道，复制来源的指令/反馈节点及角度，不再拟合。 -接收关节没有独立零位帧和第四轮指标，报告标记 `independently_measured=false` 并引用来源证据。 -运动曲线只用前三轮，第四轮分别验证两方向；独立稳态分区用于指令映射训练与验证，约束 baseline=0。 -O6 侧摆按已确认策略输出正反向指令曲线；其他关节沿用单值指令曲线。两方向共用同一参考。 -验证数据不能用于重新拟合曲线、零位、Tag 安装或基座。 -序列化只导出冻结结果；u8 必须实测支持 0–255 才能生成 256 项，rad 只在 `input_values` 范围内插值。 - -修正 URDF 保留各型号、本侧的原始 mimic，单独使用时提供近似联动；完整非线性角度由配套 JSON 提供。 -`urdf_correction` v4 明确 `preserve_source_mimic`，正式标定不再拟合倍率或偏移， -不复制其他手指的 mimic 参数，也不因修正 origin 而重写原 mimic offset。准确 FK 显式使用 JSON 的各关节角。 -`mimic_envelopes` 与实测 `motion_range_evidence` 分开：只在明确声明的实测范围策略下， -取实测范围与原始联动可达范围的最小包络；禁止突破独立安全界限及默认 CAD 硬界限。 -多级 mimic 使用父关节真正的联动可达范围递推，不将其更宽的 JSON 运动范围继续放大。 -JSON 重建与最终文件验收均核对原 mimic 元素、最小包络、通道一致性及非线性表。 -旧 correction v1、移除 mimic 的 v2、拟合近似的 v3 保持原有重建/读取语义。 -既无运动观测也无显式复制声明，或缺少绝对零位来源时，保留相对运动诊断并拒绝完整发布。 -旧 mimic 及旧迁移字段不能自动授权 v3 复制，不用默认零偏补全未知几何。 -接受原 CAD 末节零位的前提单独记录在 `cad_zero_assumptions`，不作为独立实测证据； -仅明确列出的关节取 δ=0，保持其自身 `origin`；O12 的三个额外关节假设见上方当前结果。 - -## 验收与消费 - -发布器从最终文件回读并分别检验： - -1. 最终 JSON 对第四轮原始视觉角度；逐实测关节、逐方向检查；复制表另核验来源与一致性。 -2. 最终 JSON＋修正 URDF 的完整 FK 对第四轮 Tag 位姿；显式使用所有被动角。 -3. 相同主动映射配合标准 mimic 的独立近似损失，单独报告。 - -前两项精度门限仍为角度 MAE ≤1°、P95 ≤2°、最大 ≤3°，Tag 位置 P95 ≤3 mm。 -三文件哈希、保护输入、观测覆盖和通道绑定一并检查；拟合失败不自动运动，取消后不能提交。 -混合产物的 manifest acceptance 为 `measured_and_transferred_json_urdf_v3`， -明确记录观测关节、复制关节和 CAD 零位假设。未观测接收关节不具有独立精度结论。 - -公共读取器和 `calibrated_joint_state_bridge` 支持 v3 的完整关节映射及历史 v1/v2 原语义。 -完整 JSON 回放使用 `independent_mimic_angles=True`，防止线性近似覆盖实测被动角。 -标准 `robot_state_publisher` 和普通 URDF 网页查看器提供线性联动预览，不据此宣称实测非线性精度。 -本轮未修改 MuJoCo、相机时间同步和接触动力学；现有 MuJoCo 脚本尚不能视为支持 v3。 - -## 当前型号的发布边界 - -| 型号 | 独立运动观测 | 声明的运动复制 | 绝对几何边界 | -| --- | --- | --- | --- | -| G20 | 21 / 21 个运动关节 | 无 | 五个末节的原 CAD 零位为明确假设 | -| L6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | MCP 零偏明确迁移；五个末节 CAD 零位为假设 | -| O6 | 5 / 11 | 其余三指 MCP、DIP ← 小指对应关节 | 同上 | -| O12 | 16 / 19 | 无名指 MCP/PIP/DIP ← 小指对应关节 | pinky_pip、ring_pip、thumb_mcp 及末节沿用已确认的 CAD 零位假设 | - -`--validate-only` 的 `independent_measurement_coverage` 返回具体关节列表。 -配置检查通过表示合同可执行，不能解释为全关节精度通过。 -上述运动策略已经覆盖各型号全部运动关节；完整发布仍要求实测来源验收通过、零位有来源且范围合法。 -四个产品配置的零位来源均已明确；L6/O12/G20 的原 CAD mimic/限位冲突保留诊断， -收尾保留限位约束内的线性近似；所有 JSON 查表角度和 URDF mimic 全行程都必须处于合法范围内。 -现有 Tag 布局没有被补写成未经验证的新布局，新增参考动作也没有标为实机已验证。 - -## 原按关节零位改造的软件验证记录 - -修改前在原工作区测试路径上加载备份代码：**582 passed、1 skipped**。 -固定基线保存在 `/tmp/calibration_v3_baseline_ck_q_c99/`，包含原包、输入哈希和运行日志。 -旧数值回归继续显式使用 v2 语义，未通过重写黄金输出或放宽验收门限消除差异。 - -最终代码全量回归：**614 passed、1 skipped，485.43 秒**。 -日志：`/tmp/calibration_v3_final_verified.log`。 -包含最后补入的零位异常帧恢复用例；未放宽原有空间求解或精度门限。 - -已完成的额外检查： - -- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。 -- 安装后的 12 个 console entry point 均能导入；四个型号的安装入口 `--validate-only` 均以 0 退出。 -- 两种合成布局生成的 v3 修正 URDF 另经实际 ROS `robot_state_publisher` 隔离加载通过。 -- 对比修改前备份，142 个原始 URDF/mesh 文件与 16 个其他相机、Tag、设备配置文件内容不变。 -- 四份 Profile 新增参考动作、输出版本及采集策略，对应 product 仅更新 Profile 哈希。 -- `git diff --check` 通过；新实施记录随包安装，安装后的 README 文档链接检查通过。 - -主要回归场景： - -| 场景 | 测试 | -| --- | --- | -| 四型号先锁固定参考、逐关节仅一次归零，食指在其他手指第四轮后开始 | `test_joint_zero_runtime.py` | -| 起点与 baseline 不同、非零保持姿态、错误到达/禁用通道、跨任务复用、rad 断点行程恢复 | `test_joint_zero_runtime.py` | -| 零位只采自身观测、短暂缺 Tag 丢帧、固定 Tag 缓存、姿态候选不重选冻结参考 | `test_observation_capture.py`、`test_o12_observation_resolution.py` | -| 任意固定 Tag 安装旋转、冻结零位、非线性被动曲线、第四轮滑移、共零点约束 | `test_measured_zero_mapping.py` | -| 最终非线性组合达标但 mimic 超差、文件篡改、通道错误、迁移/几何缺失禁止发布 | `test_measured_finalization.py` | -| 更换通道顺序、机位名、任务顺序的虚拟型号走同一完整发布链 | `test_measured_finalization.py`、`measured_capture_fixture.py` | -| 旧格式读取、标准 URDF 授权和结构、READY 后断流、迟到观测、后台反馈、中止与提交竞争 | 原有相关回归及扩展的 `test_calibration_coordinator.py` | - -## 本轮观测策略扩展 - -全量回归：**700 passed、1 skipped，495.34 秒**，日志 `/tmp/calibration_transfer_full_verified.log`。 -全量收集后补入的两项反馈范围一致性用例另行通过（`/tmp/calibration_transfer_feedback_domain_verified.log`）， -本轮共覆盖 **702 项通过、1 项缺少实测数据而跳过**。未放宽任何空间求解或最终精度门限。 -构建成功；安装后 12 个 console entry point 均能导入，四型号 `--validate-only` 均以 0 退出。 -对照当前提交检查 142 个原始 URDF/mesh 文件，内容全部不变；四份产品配置仅更新对应 Profile 哈希。 - -新增回归见 `test_observation_strategy.py`、`test_motion_transfer.py` 和 `test_transfer_artifact_contract.py`。 -覆盖三种观测策略、虚拟型号改名/重排、接收通道、非线性被动表、来源与假设篡改拒绝、 -O12 PIP 不补零、末节自身 origin 和各指 CAD 几何保持、机械范围及混合产物读回。 -历史全实测合成夹具显式清空新的产品 CAD 假设，继续使用原非零合成真值；未修改其数值期望。 -`test_measured_transfer_finalization.py` 进一步覆盖 O6 及重排后的虚拟 O6： -由 5 个关节的原始合成记录实际拟合，展开为 11 个关节，经过最终文件验收、发布及 manifest 读回； -6 个复制关节保留自己的通道，且没有被制造为独立观测。完整组合通过时仍报告 mimic 近似损失。 - -合成数据的已知 baseline 几何明确标注 `synthetic_fixture_truth`，不写入产品 Profile。 -完整 v3 非线性回放覆盖合成 G20、O6 拓扑及重排后的虚拟型号；四型号执行器测试不等于四型号都有完整实测回放。 -原 G20 两种布局的合成回放均得到：最终关节映射最大误差约 0.079°，完整 FK 的 Tag 姿态最大误差约 0.078°、 -最差 Tag 位置 P95 约 0.025 mm；标准 mimic 的姿态最大误差约 5.465°，仍被如实报告。 -这些数字用于检验软件是否分开评价非线性映射与线性近似，不是相机或实机测量结果。 -O12 实测回放因未提供 `O12_REPLAY_RAW` 和 `O12_REPLAY_REFERENCE` 跳过。 -没有运行实机动作，没有整只手精度、USB 延迟或多关节接触工况的实测结论。 - -复现命令： - -```bash -source /opt/ros/jazzy/setup.bash -source install/setup.bash -PYTHONPATH=src/linkerhand_calibration:$PYTHONPATH python3 -m pytest -q src/linkerhand_calibration/test -colcon build --packages-select linkerhand_calibration --symlink-install -ros2 run linkerhand_calibration calibrate_hand --config src/linkerhand_calibration/config/o6_right_product.yaml --validate-only -``` diff --git a/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md b/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md new file mode 100644 index 0000000..6dfb36d --- /dev/null +++ b/src/linkerhand_calibration/O30_RIGHT_CALIBRATION.md @@ -0,0 +1,713 @@ +# O30 右手：标定与原始 URDF 修正 + +入口为 `calibrate_hand --config src/linkerhand_calibration/config/o30_right_product.yaml`。 +Profile 为 `O30/right/o30_right_18/v1`，20 个关节全部主动；原始 URDF 与外部 SDK 不被标定器改写。 +当前产品配置绑定正面相机移动后重新采集的 `config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml`。 +该外参的 SHA256 为 `59f587e8e371f09456043c17cd0268fe1f170f1b7efdf9d2cb744c73c194953c`； +三相机序列号及内参指纹均与现有文件一致。 +软件测试包含合成数据和已记录的真实图像回归，不能作为整手现场精度报告。 + +## 正常摆放与固定基准 + +O30 使用 `acquisition.fixed_reference_mode: stationary_image`。机械手可正常摆放， +固定基准 ID0、ID3、ID16 可以正对各自相机；不再为获得唯一 PnP 法线而要求倾斜基准标签。 +标签仍须平整、牢固、完整可见，手掌与相机在同次采集中保持固定。 + +固定基准的两个用途已分开：原始角点用于确认图像中的安装位置没有变化；计算转轴时使用 +方向与相机一致的坐标系。该坐标系是明确的计算约定，不是对标签真实朝向的测量，原始两组 +PnP 候选仍保留。坐标原点由至少十张静止图像的候选中心对称计算，仅改善数值条件； +结果回到公共坐标系后，不依赖选择哪一组固定标签法线。 + +运动 Tag 继续通过运动图像和已声明的串联几何约束判别转轴；固定坐标系不能消除运动 Tag +本身的不可观测性。图像质量、可观测性、三轮训练/第四轮独立验证以及最终 JSON/URDF 验收 +保持原要求。固定基准当前角点相对冻结图像的 RMS 必须不超过 1.5 px，缺失或变化的图像不能 +作为有效采样；全局超过 5 px、持续十帧的移位停机机制也保留。此实现不包含手掌晃动的动态补偿。 + +原始记录明确标记 `stationary_image_reference`、`reference_frame_is_tag_pose: false`， +保存初始图像、相机参数、坐标约定及内容指纹；准备、采集、文件回读均绑定同一参考。 +调度版本为 `unified_schedule_v11_stationary_image_reference_feedback_interleaved`。 +旧记录仍按原含义读取，不能作为这次新模式的断点混入；切换后应重新采集。 + +软件复核包含本次真实正面 ID0 角点、独立投影的转轴数据与采集/回读测试。 +离线对照验证的是坐标变换不改变解算结果，不代表现场标定已通过。 + +## 当前范围与顺序：20 关节，四指侧摆最后 + +当前 `scope.default_scope` 为 `full`，共 17 个任务、144 个方向/分段扫描单元。顺序为： + +1. 拇指 roll、MCP、IP、yaw。 +2. 四指屈伸：小指、无名指、中指、食指，分别完成 MCP pitch、PIP、DIP。 +3. 四指侧摆 `fingers_mcp_roll_front`，先完成四指联动，再补足中指下半行程。 + +侧摆准备先恢复拇指及四指屈伸到既定基准，再执行侧摆归零路径。指令速度、避让、 +图像质量、独立验证及停机门槛保持原要求。全部完成并通过验收后，才发布整手 JSON 和修正 URDF。 + +产品配置使用 `resume_mode: passed`，前提是操作者确认机械手、相机和标签安装位置未动。 +侧摆暂停后重新运行相同产品命令，直接复用前面采集检查已通过的 16 个任务（128 个扫描单元）， +不再回放旧图像、不重新计算旧采样质量，也不复核已完成关节的零位。 +未完成的侧摆会重新确认自己的四个零位并重跑整组；若侧摆整组也已通过，则直接复用。 +准备阶段只执行当前设备就绪、必要归位和新图像采集；通信、故障、堵转保护与最终验收保持原要求。 +未通过的侧摆不会发布为完整标定。需要完整恢复复核时，可将产品 `resume_mode` 改回 `verify`。 + +已通过模式的扫描样本和逐帧图像证据采用磁盘索引，导入时每批最多读取 64 条完整 +记录；原始内容逐条绑定，源文件被改写时拒绝导入。零位和模型来源证据仍完整保留， +最终验收继续读取全部图像。`20260919_213352` 曾因整份断点展开导致内存压力并在 +节点首条状态前超时；改用分批读取后，同一份 237,514 条记录实测准备耗时 29.26 秒， +峰值进程内存约 2.04 GiB，保留 88 个通过单元，下一任务为中指 PIP。 + +`20260919_193652` 已按此模式直接复用四个拇指任务的 32 个扫描单元，并进入小指 MCP。 +当前小指 MCP 的 129 帧准备图像出现两组无法区分的姿态解，验证 RMS 分别为 +0.578627 px 和 0.595443 px，程序按原门槛暂停；这不是拇指断点复核失败，未发布完整标定。 +记录见该会话 `passed_resume_attempt_summary.json`。断点准备实测约 10.2 秒， +历史逐帧回放次数为 0；索引在实时回调开始前完成，导入仍分批持久化。 + +### 侧面 MCP 沿用正面已验证的解算流程 + +四指 MCP pitch 改用中节 Tag（ID4、6、8、10），与同指 PIP 共用；DIP 继续使用 +中节→末节的相对观测。Tag 的实际安装链接保持原声明，MCP 扫描时 PIP/DIP 保持既定姿态， +中节 Tag 因而可以观测 MCP 运动。这与正面 ID1 同时观测 roll 和 MCP 的方式一致。 +固定基准仍使用三机位通用的 `stationary_image`，候选仍由原有运动图像求解器判别。 + +对 `20260919_193652` 同时记录的 129 帧离线重放：末节 ID5 的两组解无法区分； +中节 ID4 的验证 RMS 为 0.360377 / 0.526887 px，校正后比较 p=0.0000062452， +满足原有 0.03 px 差异与 0.01 检验门槛。真实图像回归夹具为 +`test/fixtures/o30_side_mcp_observers.json.gz`。这是准备阶段的验证，不代表四轮采集和最终 URDF 已验收。 +其余三指采用相同观测结构，仍须各自通过现场图像检查。 + +四指 DIP 同时接入与拇指 IP 相同的 `parent_reference` 机制：使用本指 MCP pitch +在原零位建立的中节 Tag 姿态，以及本指 PIP 测得的 Tag—轴线关系；从原始 URDF +提取 PIP/DIP 平行轴与轴间距约束。两项来源都必须先通过各自的采集检查，DIP 的曲线 +仍独立采集。这里只复核新任务所需的当前父 Tag 图像，不重新扫描 MCP/PIP。 + +恢复模型的固定图像参考按原模型身份保留：每个模型继续使用它建立时的坐标原点、 +参考图像声明和哈希，用当前同帧固定 Tag 角点确认参考仍有效。新会话的固定参考 +另行监测现场移动，不能替换旧模型的坐标声明；不同任务可以分别使用各自的参考。 +同一帧的模型依赖链若声明互相冲突的固定参考，拒绝采样。恢复时只加载已通过的 +不可变模型，不重放历史图像、不重新扫描已通过关节。当前角点缺失、移动或内参 +变化仍会拒绝采样，保存的图像证据保留原参考，供最终产物回读验证。 +父姿态模型、父关节轴线和共享 Tag 姿态统一通过 `passed_pose_models_restored` +记录绑定跨会话来源；记录必须匹配原模型哈希、证据身份、任务、观察代次和时间顺序。 +缺少或篡改恢复记录时仍拒绝引用旧来源，不改写旧模型的观察代次。 + +`20260919_202726` 已通过 48 个扫描单元，暂停在 `pinky_dip_side` 父 Tag 检查。 +`test/fixtures/o30_resumed_parent_reference.json.gz` 保存该次拒绝的十帧真实图像角点、 +旧模型和两次固定参考：原逻辑复现 `pose_image_motion_reference_binding_changed`， +修正后可完成十帧父姿态检查及序列化回读。超时诊断改为显示当前参考关节的缺帧数 +和实际 Tag 拒绝原因，不沿用上一关节的零位错误。本项离线回归不代表 DIP 实机采集 +或整手 URDF 验收已完成。 + +本次没有增加辅助关节运动。已通过的拇指观测定义不变；Profile 哈希变化通过显式的 +`capture_observation_revision` 派生断点记录，原始数据不改写。迁移只允许修改尚未取得 +有效零位/扫描结果的观测项，保留已通过的 32 个拇指扫描单元，按 `passed` 模式恢复， +不重放旧图像或重新检查旧零位。小指 MCP 使用新观测重新准备和采集。 +该次派生断点为 `20260919_200132_observer_revision`。 +离线复核与迁移记录位于 `calibration_output/O30_RIGHT_001/software_review_pinky_ambiguity/`。 + +### 中指 DIP：已测父关节几何的误差传播 + +`20260919_214048` 已完成中指 PIP，累计保留 96 个通过单元；随后中指 DIP +初始化因图像误差暂停。检查发现，旧传递模型在拟合时把上一关节实测的轴线和 +安装位置当作精确值，仅在拟合后追加来源不确定度。这会让测量误差变成无法调整的约束。 + +新模型 `image_hinge_v4_uncertain_source_geometry` 在来源记录的不确定度范围内 +拟合当前轴向与轴间距，仍使用原始 URDF 的平行关系和距离。两个原始姿态候选分别拟合， +当前训练图像与验证图像保持分离；拟合后恢复完整几何自由度计算图像不确定度，再叠加 +来源误差。1.5 px 图像门槛、3° / 3 mm 几何门槛及候选区分规则保持不变。 +来源模型、来源误差与当前拟合误差均写入证据，回读检查来源绑定及约束范围。 +旧版 v3 模型继续按原策略读取，不重写已通过单元，也不触发恢复时的旧图像重算。 + +对该次 129 帧真实准备图像离线重放，两候选验证 RMS 从 1.579 / 1.642 px +降至 0.215 / 0.220 px，最大单帧 RMS 从 2.820 / 2.885 px 降至 0.348 / 0.327 px。 +但两种三维姿态仍不能可靠区分，结果仍为未通过，不能仅选取误差稍小的候选。 +暂停提示 `OBS-POSE-AMBIGUOUS-117` 明确要求补充当前关节的独立观测；原样重复运动 +不保证解决歧义。用户暂时不能调整 ID9，故本轮仅完成程序修复与离线验证，未启动硬件。 +诊断和测试记录保存在 `calibration_output/O30_RIGHT_001/software_review_middle_dip_transfer/`。 + +修复后的 `20260919_220723` 恢复尝试使用 `ROS_DOMAIN_ID=99`，选中上述 96 单元断点， +但在初始基准恢复阶段暂停，尚未进入 DIP。346 帧记录显示无名指/小指 MCP 的指令 +分别从 253/251 降到 155/154，反馈始终为 253/251；同组其他关节有反馈变化。 +SDK 无活动故障，但不能据此否定机械或反馈异常。已停止运行栈，等待现场确认后继续。 +停机诊断现在分别记录实际指令、反馈和反馈进展判据，避免将原 `goal=250.5` 误认为 +发送目标；保护逻辑不变，相关回归 67 项通过。原断点保留，本次未发布 JSON/URDF。 + +### 共享 Tag 的零位姿态传递 + +#### 完整共享零位参与新关节拟合（v4，当前流程） + +`20260919_210955` 完成无名指 PIP、DIP 和中指 MCP，累计保留 88 个通过单元， +随后中指 PIP 暂停。仅共享方向的 v3 拟合与已测零位相差 5.228 mm，超过原有 +5 mm 容差；其中相机深度方向占 5.219 mm。固定基准身份相同，原 MCP 模型对 +本次十帧静止图像的最大姿态差为 0.146° / 0.167 mm。这支持独立拟合的深度偏差 +解释，不能据此认定机械手实际移动了 5 mm。 + +当前 `training_shared_zero_pose_holm_v4` 将同一物理 Tag、同一姿态的已测方向和 +位置共同用于新关节拟合。新转轴、枢轴及各帧角度仍从本次运动图像估计；Tag 安装 +平移由 `t0 = pivot + R0 * mount` 确定，不复制前一关节的轴线、枢轴或安装参数。 +共享零位只是测量约束，不是新的真值证明：仍先用原模型检查当前独立静止图像，再 +检查新模型的独立运动验证图像与静止图像。原有 2° / 5 mm、1° / 1 mm 和 1.5 px +门槛全部保留。静止见证图像不参与拟合，SDK 数值不被解释为几何角度。 + +不确定度计算恢复全部方向、位置自由度，以完整图像 Jacobian 计算局部敏感度， +避免固定共享位置后低估转轴和枢轴的不确定度。新旧策略使用不同证据版本；最终 +回读还检查模型是否确实使用了声明的共享方向和位置。旧 v2 / v3 证据保持原样读取， +`passed` 恢复仍直接保留已通过任务，不重新求解旧图像或采集旧零位。 + +真实中指数据夹具 `test/fixtures/o30_shared_tag_middle_pip.json.gz` 下，两组初值 +均收敛，独立运动验证 RMS 约 0.370 px，当前静止姿态差约 0.356° / 0.164 mm。 +这些是离线软件验证结果，尚未代表剩余实机采集或整手 URDF 验收完成。 + +#### v3 共享方向改进记录 + +`20260919_205013` 已完成无名指 MCP，累计 64 个通过单元；无名指 PIP 暂停于 +`image_motion_shared_pose_unresolved`。独立拟合的两个候选相对 MCP 零位分别偏转 +2.53° / 11.47°，前者几乎全是转轴外倾斜。将本次十帧静止图像交给原 MCP 模型， +相对原零位最大变化仅 0.046° / 0.05 mm。这支持独立模型倾斜偏差的解释，不能把 +两个模型的姿态差直接当成机械手实际移动量。 + +新增 `training_shared_zero_orientation_holm_v3`：先用已通过的原模型检查本次同姿态 +静止图像，再让已测共享零位方向参与新转轴拟合。只共享方向；新关节的轴线、枢轴、 +Tag 平移和每帧角度仍由本次运动图像估计。初始图像的角度也独立求解，避免误把 +准备运动第一帧当成共享零位。所有原始 IPPE 初值仍运行，收敛到同一物理解时按 +原有等价性规则处理，不强选某一平面法线。 + +十帧当前静止图像不进入拟合，运动图像仍划分独立训练/验证集；源模型当前姿态和 +新模型共享零位都须通过原来的 2° / 5 mm 检查，静止散布和 1.5 px 图像门槛不变。 +不确定度检查恢复全部可辨识的方向自由度，按完整图像 Jacobian 计算局部敏感度， +不因共享方向而报告虚假的零不确定度。该量仍是模型的局部图像敏感度，不能代替 +现场精度验收或证明原模型没有系统偏差。 + +真实无名指夹具 `test/fixtures/o30_shared_tag_ring_pip.json.gz` 下，两组初值收敛到 +同一模型，当前静止方向差约 0.11°，独立验证 RMS 约 0.312 px。方向约束的来源、 +原零位和当前图像绑定写入证据；最终回读重新检查来源以及模型是否使用了声明的方向。 +旧 v2 证据继续按原规则读取，已通过模型和旧扫描不重新拟合；恢复启动也不重放旧图像。 +本项为离线软件验证，未启动实机或完成剩余关节及整手 URDF 验收。 + +#### v2 共享姿态验证记录 + +`20260919_200527` 实机完成小指 MCP 的四轮往返，累计保留 40 个通过单元； +小指 PIP 在准备初始化处暂停。PIP 两组解的验证误差为 0.408063 / 0.401892 px， +但其归零姿态与同一 ID4 已确认的 MCP 零位分别相差约 1.57° / 20.99°。 +两任务在该姿态的相关通道指令和反馈一致。此前每个任务独立判别，未使用这项已有约束。 + +现由通用共享姿态模块连接同机位、同父/子物理 Tag、同基准姿态的前后任务。 +程序从本次归零准备末端保留至少十张、跨度至少 0.2 秒的图像；这些图像与转轴训练/ +验证图像不重叠。旧任务的零位和模型只读加载，新任务仍独立拟合转轴、安装位置和角度。 +两任务不满足共享姿态条件时，保持原有解算流程；有可用来源但当前姿态检查失败时明确暂停。 + +姿态一致性沿用 2° / 5 mm 容差，静止散布沿用 1° / 1 mm 门槛。 +为避免把边界误差误判成错误分支，只有偏差超过两份姿态容差之和才排除候选； +位于中间区域时保持未确定。这是保守的一致性余量，不声称统计置信区间。 +该约束先决定候选资格，再按原有训练误差选择模型和执行独立图像验证； +1.5 px 绝对图像门槛、0.03 px 差异门槛以及第四轮验收保持不变。 + +`shared_tag_pose_bridges` 绑定原零位、原模型身份、相关反馈通道和新图像内容哈希； +最终回读重新核对来源、姿态和 URDF 推导的通道依赖。`passed` 断点模式记录模型恢复的 +当前观测 epoch，直接加载已通过模型；不会在启动时回放旧扫描或重采旧零位。 +真实回归夹具为 `test/fixtures/o30_shared_tag_pip.json.gz`，包含原始 129 帧准备证据和 +独立的十张末端图像。该数据在原流程下重现暂停，在共享姿态约束下通过初始化。 + +本次修改不改变 Profile、运动顺序、轨迹或受保护配置，无需迁移旧数据。 +该次断点为 `20260919_200527`，恢复时保留拇指及小指 MCP，从小指 PIP 继续。 +上述新逻辑已做离线回归，尚未完成新逻辑下的实机续采和整手 URDF 验收。 + +### 从原 16 关节断点扩展 + +普通断点恢复仍拒绝不同范围或保护配置。已有 `without_finger_roll` 断点只能通过显式离线操作扩展： +`python3 -m linkerhand_calibration.runtime.checkpoint_extension --source-profile <原受保护Profile> +--target-product <当前产品配置> --source-checkpoint <原raw_samples.jsonl> --output-dir <新目录>`。 +该操作要求旧任务完整保留为新流程前缀；旧 Profile 扩展到目标范围后，除了原来未执行任务的位置， +整个声明必须与新 Profile 一致。相机、URDF、Tag 配置等其余输入必须同哈希。 +旧合同和新合同都须逐帧校验通过，且可复用单元、零位和行程检查结果完全一致。 + +工具新建派生断点，原始样本行逐字保留，原文件不改写；记录原始采集头、固定基准、 +源文件 SHA256、前后 Profile 快照及扩展审计信息。后续按产品的 `resume_mode` 执行恢复， +`passed` 模式另记录安装未变声明及实际跳过的复核步骤。 +可选的 `without_finger_roll` 部分模式仍保留，其产物不包含四指侧摆曲线，也不代表整手标定通过。 + +### 拇指 roll 姿态歧义诊断 + +需要检查图像证据时，可使用通用诊断入口，只采拇指 roll 一次往返： + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o30_right_product.yaml \ + --diagnostic-capture o30_thumb_roll +``` + +该预设沿用原有 baseline、反馈保护、食指避让和退出恢复顺序。未能冻结三维零位时只保存 +明确标记的原始诊断观测；诊断会话禁止用于正式结果发布或断点恢复。原始图像角点记录还包含 +可见的其他 Tag，可用于离线检查刚性关系，但不会自动把辅助 Tag 作为通过验收的依据。 + +2026-09-19 的 `20260919_150313` 诊断中，准备阶段训练的两个转轴候选在随后两个方向的 +独立图像上可区分，原有 0.03 px 差异要求和 0.01 检验门槛均保持。 +双标签共享转轴的实验尚不足以排除所有候选，未接入正式解算。 +这次诊断不能替代正式三轮训练、第四轮验证及最终 JSON/URDF 验收；标签正对相机并非禁用条件。 + +`20260919_150751` 的 roll、MCP 各四轮采集完成，但 IP 前的 yaw=0 MCP 父标签准备连续两次 +无法区分候选。其相同回零姿态与已验证 roll 模型的旋转差分别约 1.5°、20°；该跨任务对照 +仅作诊断，不直接授权候选。随后 `20260919_152030` 有限诊断尝试用 CMC roll 的独立 +预备运动重新确认 ID1，保持 yaw=MCP=0，并保留食指避让。ID1 成功区分候选，但 IP 的 +ID2 候选仍不能区分；该实验未通过完整验证，已停止，当时正式 Profile 恢复原配置。 +实验配置与原始图像保存在诊断目录，禁止用于正式发布或混入断点。 + +### 父标签参考与相邻平行转轴的复用 + +后续离线核查确认：单独重复 MCP 扫描没有利用已有的刚性安装证据；仅凭某一组候选的 +重投影误差更小，也不能可靠选解。现通过通用任务字段 `parent_reference` 分开声明两种来源： + +- `pose_joint: thumb_cmc_roll`：复用已验证的 ID1 图像模型。IP 前恢复 yaw=MCP=roll=0， + 先静态采集至少十张当前图像，检查掌心与 ID1 姿态、祖先通道反馈及图像来源；不增加 MCP 扫描。 + 食指侧摆由原任务正常恢复到 255，不需要重新做 roll 避让。该检查不修改 roll 或 MCP 的零位。 +- `geometry_joint: thumb_mcp`：使用已通过独立图像检查的 MCP 轴线与 ID1 安装关系， + 把转轴表达在刚性 ID1 坐标系中。它不随上游 yaw 或 MCP 自身转动改变。 +- 从受 SHA256 保护的原始 URDF 提取 MCP/IP 的平行关系和轴间距 + `0.048556745401643224 m`；约束 IP 自己的图像解算，IP 曲线和限位仍须独立实测。 + 传递来源的不确定度计入 IP 的几何验收，不能因减少优化变量就把轴视作无误差。 + +候选仍用分开的准备阶段训练/验证图像判别，保留原 1.5 px 图像门槛、0.03 px 候选差异 +及 0.01 检验门槛。无法区分时保存诊断并暂停，不自动重复相同扫描；短暂采图不足的有界恢复保留。 +正式三轮训练、第四轮独立验证、稳态指令映射、几何零偏与 JSON/URDF 文件回读验收均保留。 + +记录同时绑定来源模型、安装不确定度、原零位、当前复核图像及 CAD 哈希。内部采图重试可以沿用 +原安装证据，但必须记录版本衔接；停机重启通过原恢复流程重新确认来源。 +新流程使用 `unified_schedule_v10_verified_parent_geometry_feedback_interleaved`， +旧流程的断点不能直接混入，需要新会话采集。旧格式记录仍可按其原有含义读取。 + +数值回放中，原始 IP 准备图像仍无法独立排除两组候选；加入已验证父参考与上述约束后， +两组候选的验证 RMS 约为 0.605 px 和 0.898 px，比较概率约 `5.96e-8`，通过原门槛。 +该离线回放用了两个已停止会话的诊断图像，只证明算法在这些数据上的可辨识性， +不授权跨会话实机复用，也不代表完整标定、绝对转轴或最终产物已经验收通过。 +对应图像已加入离线回归夹具 `test/fixtures/o30_transferred_ip_geometry.json.gz`。 + +本次新增 35 项算法、参考复核和证据回读测试通过，O30 产物及 G20/L6/O6/O12 相关回归已运行。 +全量回归发现的旧测试夹具和断言已按现有合同更新：明确读取产品绑定的 URDF、保留全机位 +原始图像日志、为合成 Profile 声明空保留范围，以及检查方向起点的运动采样。 +仍保留一项可在修改前备份上同样复现的遗留失败： +`test_urdf_zero.py::test_end_on_phase_rejects_oblique_monocular_depth_bias`， +使用 G20 `legacy_11` 兼容求解器，预期 4° 的偏置算得约 3.896578°，超出该测试的 ±0.05°。 +未降低该断言，也未将它作为 O30 验收通过；O30 正式流程仍需重新采集并完成独立产物验收。 + +## 构建和加载环境 + +在当前工作区构建外部 SDK，构建、安装、日志目录均位于当前工作区。SDK 源码仍使用原目录。 + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.bash +colcon --log-base log/o30_calibration build \ + --base-paths src/linkerhand_calibration \ + /home/lxp/projects/linkerhand-o30-ros2/linker_hand_o30_ros2_sdk \ + --packages-select linkerhand_calibration linker_hand_o30_ros2_sdk \ + --build-base build/o30_calibration --install-base install/o30_calibration \ + --symlink-install +source install/o30_calibration/setup.bash +``` + +后续新终端按顺序加载 `/opt/ros/jazzy/setup.bash`、`install/setup.bash`、 +`install/o30_calibration/setup.bash`。启动器自动启动 SDK；运行正式标定前退出其他 SDK 实例、GUI 控制器及占用相机的 MVS。 + +适配器 `o30_ros` 使用 SDK 原有话题： + +| 功能 | 话题 | +|---|---| +| 位置指令 | `/cb_right_hand_control_cmd` | +| 位置反馈 | `/cb_right_hand_state` | +| 速度、力矩设置 | `/cb_right_hand_setting_cmd` | +| 身份及在线状态 | `/cb_right_hand_info` | + +本配置使用 `libcanbus`、设备编号 0、30 Hz 反馈，关闭自动摆位、触觉和不使用的速度/电流查询。 +设备速度及力矩设为 200；发送完整 20 通道位置向量。 +准备、避让、零位预备和任务退出使用约 13.352 秒的全量程余弦轨迹，峰值为 30 指令单位/秒。 +该过渡速度由 `command_trajectory_full_range_seconds: 13.35176877775662` 统一配置， +包括恢复 baseline；动作次序保持不变，到位与采样稳定性由实时反馈确认。 +正式扫描使用 `scan_trajectory_full_range_seconds: 13.35176877775662`，峰值为 30 指令单位/秒。 +扫描参数仅作用于运动扫描和稳态点之间的行程；整数指令存在一单位量化步进。 +提速后的实机准确度仍须通过本次现场独立验证，软件测试不能替代现场验收。 + +## 当前反馈与指令职责（2026-09-19 恢复反馈模式） + +O30 使用 `motion_observation: feedback`、`feedback_travel_matches_command: false`， +发布依据仍为 `release_basis: steady_command`。反馈参与运动与采集判断，反馈角度映射只作诊断。 + +| 环节 | 当前规则 | +|---|---| +| 最终 JSON | 下发指令→视觉角度，独立验证指令映射及修正 URDF；反馈映射失败不单独阻止发布。 | +| 启动与采样 | 要求 20 通道有效、新鲜的反馈；程序先按配置顺序恢复完整 baseline，再锁定基准。稳态点和零位在反馈稳定后使用新图像。 | +| 恢复与避让 | 已测零位使用其原始反馈参考确认归位，拇指 roll 归位后才恢复食指侧摆。 | +| 采集质量 | 检查带同步图像的指令覆盖、各运动关节独立的反馈行程及分布、保持关节是否变化和图像时间关联。 | + +目标值与反馈相差约 10 不构成单独的停止条件；不假设二者相等，也不事先强行用 +指令跨度推算等比例反馈行程。例如指令 0 时实测反馈为 10，回零时核对的是该实测参考， +而不是等待反馈变成 0。同一保持姿态内的反馈漂移仍会影响稳定性判断，不能把目标偏差 +容许量当作稳态抖动容许量。 + +反馈延迟跨过换向时，未建立指令/反馈比例关系的运动段按本段实际观测的转向点检查 +所需方向的位移。`20260919_165005` 中，回程开始的反馈为 221，随后补报到 247， +再正常回落至 221；旧逻辑仍要求低于起始反馈，错误地等待 218.5。现在这段回程 +可以作为运动证据，保持后的图像仍须通过原有稳定窗口和独立指令映射验收。 +只有反向变化、完全静止、其他通道运动或重复时间戳均不能代替本关节所需方向的运动； +已测零位仍按绝对反馈参考确认。稳态点仍穿插在四轮往返中，没有增加独立稳态往返。 + +- 反馈运动、方向和稳定性使用真实 SDK 样本，按消息时间戳去重；缺失、越界或超过 1 秒不更新仍会暂停。 +- O30 原 SDK 只提供消息级反馈时间，保留该时间与图像的关联，不生成逐通道测量时间。 +- 零位及稳态采样仍需至少 0.2 秒的独立反馈稳定窗口；采样期间反馈变化会清空当前稳态候选。 +- 指令覆盖按各自通道的指令区间检查，端点要求同步图像。反馈覆盖在各自实测区间检查样本数、分箱和空缺， + 不把中指 0–80 的指令边界当作反馈边界；小于运动分辨阈值的噪声不能归一化成完整运动。 +- 固定关节按保持姿态的实测反馈比较，恒定偏差不等于关节移动；零位准备仍核对完整保持指令和反馈。 + 准备运动的几何保持检查按原始 URDF 和 Tag 安装连杆确定相关反馈通道；其他指及 Tag 下游关节的反馈 + 不会改变当前 Tag 的几何模型。完整 20 通道保持指令仍需一致，全部反馈仍原样记录。 + 例如 ID1 位于 `thumb_proximal`，检查 roll/yaw/MCP 三轴，IP 通道的变化不应使其几何证据失效。 +- SDK 身份、通信、异常/离线/过流/过温、竞争控制端、固定基准漂移及人工中止保护保持。 + 固件端点堵转标志继续保留诊断，实际无运动由反馈运动检查处理。 +- O30 的 `acquisition.stall_timeout_seconds` 为 4 秒，从明显运动需求开始且没有真实反馈推进时计时； + 持续发送新指令不会重置计时。此调整给端点回程更多响应时间，不保证已确认的堵转能自行恢复。 + 反馈新鲜度、稳态采样、已测零位归位、运动覆盖和独立验收门限不变。其他型号仍为 2 秒。 + 本次参数调整更新了受保护配置哈希；旧断点保持原样保存，新配置不能直接复用旧配置下的断点。 + +指令与反馈的拟合输入保持分离。稳态节点的训练形状也使用指令→视觉观测， +不能因恢复反馈门控而重新依赖反馈角度拟合。JSON/URDF 的角度精度、几何可观测性、 +原始 CAD 保护、端点证据和第四轮独立验收门槛不变。 + +当前配置哈希已更新，调度恢复为 `unified_schedule_v8_approach_path_interleaved`。 +此前视觉门控会话保留只读，不能直接作为当前策略的完成断点；重新采集后仍可按当前策略断点恢复。 +四指分组任务被中断后整组重做。本次仅修改软件并离线验证，实机保持停止，尚无新的现场验收产物。 + +## 相机移动后重标外参 + +仅改变相机位置和朝向，镜头、焦距、对焦及成像分辨率/裁剪均未改变时，可以沿用内参。 +这次已沿用 `~/.ros/camera_info/` 内三份文件，并重新求得三相机之间的外参： +重投影 RMS 为 0.933275 px，最大旋转重复性为 0.052949°，最大平移重复性为 0.496086 mm； +FRONT+SIDE、FRONT+TOP 各 15 组有效观测，质量检查通过。以下命令保留供下次移动相机后重新标定。 + +以下命令使用当前工具默认的 **8×5 内角点、27 mm 方格**；棋盘不一致时必须修改对应三个参数。 + +```bash +ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \ + front_camera_serial:=DB2163742 \ + side_camera_serial:=DB2163749 \ + top_camera_serial:=DB2163739 \ + front_camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml \ + side_camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163749.yaml \ + top_camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163739.yaml \ + checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \ + output_file:=$PWD/config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml +``` + +在界面分别采集 FRONT+SIDE、FRONT+TOP 的不同棋盘姿态，每对至少 15 组有效观测，质量通过后点击 SAVE。 +通用外参工具沿用 `/g20_extrinsics/...` 图像命名空间，此名称不会改变输出的相机身份或 O30 绑定。 +退出外参工具，再计算新文件的 SHA256： + +```bash +sha256sum config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml +``` + +将输出路径和 SHA256 分别填写到 `src/linkerhand_calibration/config/o30_right_product.yaml` 的 +`artifacts.camera_extrinsics` 与 `artifacts.camera_extrinsics_sha256`；同时确认 `serial_number` 对应本台右手。 +相机移动前的外参和采集记录仅保留作历史诊断，新外参绑定后应从头采集整手数据。 +配置检查会核对三份内参指纹、相机序列号、外参质量和各输入文件 SHA256。 + +```bash +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o30_right_product.yaml --validate-only + +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o30_right_product.yaml +``` + +## 通道、观测与任务 + +全部 Tag 使用 36h11，黑色外框边长 16.5 mm。2026-09-19 用户更正当前实机 +`LHO30-03.1-034-R-J-3-A` 的 baseline：四指侧摆零位为竖直状态，小指 52、无名指 113、 +中指 196、食指 255，其余关节为 0。此约定替代此前提供的 baseline；对应输出角度为 0。 +配置指纹同步更新，旧 baseline 下的零位观测和断点不能复用，须重新采集。按 SDK 通道顺序，向量为： + +```text +[0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +``` + +| SDK 索引 | SDK 名称 | URDF 关节 | baseline | 指令增大时角度方向 | +|---|---|---|---|---| +| 0 | thumb_roll | thumb_cmc_roll | 0 | 正 | +| 1 | thumb_yaw | thumb_cmc_yaw | 0 | 正 | +| 2 | index_yaw | index_mcp_roll | 255 | 负 | +| 3 | middle_yaw | middle_mcp_roll | 196 | 负 | +| 4 | ring_yaw | ring_mcp_roll | 113 | 负 | +| 5 | little_yaw | pinky_mcp_roll | 52 | 负 | +| 6 | thumb_root1 | thumb_mcp | 0 | 正 | +| 7–10 | index/middle/ring/little_root1 | 对应四指 mcp_pitch | 0 | 正 | +| 11–14 | index/middle/ring/little_root2 | 对应四指 pip | 0 | 正 | +| 15 | thumb_tip | thumb_ip | 0 | 正 | +| 16–19 | index/middle/ring/little_tip | 对应四指 dip | 0 | 正 | + +| 任务顺序 | 关节与观测 | 固定姿态 / 收尾 | +|---|---|---| +| 1 | thumb_cmc_roll，正面 0→1 | 食指侧摆 0；结束先拇指归零，再食指侧摆 255 | +| 2 | thumb_mcp，正面 0→1 | yaw=80；结束先 MCP 归零，再 yaw=0 | +| 3 | thumb_ip，正面 1→2 | 其他拇指关节 0 | +| 4 | thumb_cmc_yaw，顶部 16→17 | 其余关节保持 baseline，完成后 yaw 归零再进入下一任务 | +| 5–7 | 小指 pitch：3→4，pip：3→4，dip：4→5 | 每次仅一个关节运动，其他两个 0；完成后三关节置 255 | +| 8–10 | 无名指 pitch：3→6，pip：3→6，dip：6→7 | 保持小指避让；完成后再累加无名指避让 | +| 11–13 | 中指 pitch：3→8，pip：3→8，dip：8→9 | 保持小指、无名指避让；完成后再累加中指避让 | +| 14–16 | 食指 pitch：3→10，pip：3→10，dip：10→11 | 保持前三指避让 | +| 17 | 小/无名/中/食 mcp_roll，正面 0→12/13/14/15 | 先恢复屈伸 baseline，再执行下述四段联动 | + +ID1 始终安装在 `thumb_proximal`，同时用于 roll 与 MCP。每个关节在自己的固定姿态下采集零位，不要求所有 Tag 同时可见。 +四指零位准备使用 `evidence_scope: path`：先到联动低端点（中指 80，其余 0）， +再到四指均为 255 的高端点，返回联动低端点，最后到各自 baseline。 +初次摆到低端点的图像不参与建模；之后完整路径的图像确定转轴，baseline 图像单独冻结零位。 +小指的短距离归零段不足以可靠确定转轴，因此不能只保留最后一段图像。 +完整路径仍只保留最多 129 个指令分箱，并记录每张来源图像的真实运动段、会话身份和内容哈希； +重试会清空前次准备证据。其他型号默认 `evidence_scope: arrival`，保留原来的最后一段准备逻辑。 +更改路径不改变零位或降低可观测性门槛；旧调度数据仍可读取，不能混入本调度的断点恢复。 +共享 Tag 的图像运动模型按任务管理，不能把 roll 模型当作 MCP 曲线使用。 +进入 IP 时在声明的 yaw=MCP=roll=0 姿态下静态复核 ID1，再使用 MCP 在 ID1 坐标系中的 +实测安装几何约束 IP 解算；不再额外运动 MCP，不替换任何已冻结的关节零位。 + +四指侧摆每轮执行完整闭环： + +1. 小指、无名指、食指 0→255，中指 80→255，同一归一化进度联动。 +2. 同步反向，中指回到 80，其余回到 0。 +3. 其余三指保持 0，中指 80→0。 +4. 其余三指保持 0，中指 0→80。 + +80 只是分段边界，中指零位为 baseline 196。四指分别保留自己的指令、反馈和 Tag；中指独立段只要求 ID0、ID14。 +独立稳态采样网格覆盖各 baseline 和分段边界，额外插入同步路径上其他通道所需的停留点；保持不动的通道不计入样本。 +使用 `command_capture_mode: interleaved`，在同四轮路径中分别记录运动图像和停止后的稳态图像： +前三轮训练反馈映射、几何和指令映射，第四轮独立验证；验证停点使用训练网格中点及端点。 +运动图像不充当稳态图像，指令曲线始终使用实测指令和独立图像角度。 +每一小段扫描到达中间节点后交给紧接的稳态采集统一等待稳定，避免重复停留； +每个方向的起点、终点保留反馈到位停留，确保运动记录本身具有端点图像与完整反馈范围。 +实际运动证据、反馈稳定窗口、独立图像数量和独立验证阈值均保留。 +原六轮 `separate` 模式仍可用于其他配置，但其记录不能与当前四轮配置混合恢复。 + +对于普通单关节，六轮模式的纯运动时间约为 240.3 秒；当前四轮模式约为 106.8 秒。 +此计算不包含零位、避让、反馈稳定和图像采样时间，不作为实机完成时限。 +四指联动任务仍在每轮完成全部四段，中指低段不会因缩短采集而丢失。 +单段采样不足允许一次同速重扫，段身份参与数据筛选。暂停或中断后，四指分组任务整体重做，并重新采零位；其余任务沿用逐关节零位复核后恢复的机制。 + +### 提速实测与当前限制 + +按用户后续要求，过渡轨迹也从峰值 20 调到 30；后续 184003 会话已执行前三个拇指任务的准备与恢复。 +下面的现场记录发生在此变更之前，当时扫描峰值为 30，准备、避让与恢复峰值仍为 20。 + +2026-09-18 最终端点流程的会话 `20260918_175403` 中，拇指 roll 前五个方向均一次通过， +每程实际采集约 19.3–20.0 秒，包含中间稳态停点和起终点观测；按此速度估算普通关节四轮 +约 2 分 40 秒，另加准备和归零。此估算不适用于有额外路径的四指联动任务，也不计重扫。 + +第三轮反向时，设备目标已经降至 223，反馈仍为 253；SDK 直接回读报告 +`thumb_roll: 执行器层判定堵转`。设备速度、力矩的直接回读均为 200。 +用户确认无外部卡住后，只做了一次从 253 开始的受控回零，仍触发无运动保护， +没有恢复避让食指。该会话不完整，没有生成可发布的标定 JSON 或修正 URDF。 +记录与诊断位于该会话的 `raw_samples.jsonl`、`paused_device_readback.json`、 +`efficiency_trial_summary.json` 和 `supervised_return.jsonl`。 + +当前保留三轮训练和第四轮独立验证，用于检查重复性、双向回差和未参与拟合的数据； +没有将两轮采集视为等效替代。提速仅减少额外往返、中间重复等待及扫描行程时间， +没有放宽角度误差、反馈覆盖、端点观测或无运动保护。 +最终端点调度相关的执行、效率及完整 20 关节合成产物测试共 22 项通过； +此处记录的是 175403 会话的堵转；后续 184003 会话已完整采集前三个拇指任务。 +合成测试不能证明当前实机的整手标定精度。 + +## 输出和验收边界 + +现场通过后，统一发布链路输出 JSON v3、修正 URDF、标定报告及独立验证报告。 +每个主动关节都有独立验收的指令曲线，包括增大/减小分支；反馈曲线单独报告诊断状态。 +没有跨指复制、被动关节或新增 mimic。 + +15 个非末端关节使用 CAD 串联轴几何求解零偏，执行数值可观测性及第四轮验证。 +`thumb_ip` 和四指 `dip` 的 baseline 使用用户接受的原始 CAD 零位，报告标记 `assumed_source_cad_zero`，保留原 `origin`。 +20 个关节都独立测量运动曲线，并用实测角度范围更新限位。 +保持连杆尺寸、`origin.xyz`、轴、mesh、惯量和原 effort/velocity；包括原始无名指侧摆的零 effort/velocity，不自动替换为其他值。 +修正结果由 JSON 重建、文件回读后验收；原始 `linkerhand_O30i_right-V2_0819.urdf` 始终作为只读输入。 + +## 软件验证记录（2026-09-18） + +以下均为离线软件验证，没有启动硬件或发送运动指令： + +| 验证 | 结果 | +|---|---| +| 标定包与外部 O30 SDK 按本文命令构建 | 2 个包成功；SDK 源文件内容保持不变 | +| O30 配置、SDK、任务路径、观测与启动描述 | 9 项通过 | +| 20 主动关节独立合成数据、产物回读、分段恢复 | 2 项通过；15 个求解零偏误差小于 0.004 rad；JSON 重建 URDF 字节一致 | +| 标准 `check_urdf` 解析合成修正产物 | 通过 | +| 受影响模块回归 | 78 项通过；末次分段质量与恢复补测 32 项通过 | +| 全包回归，覆盖 G20/L6/O6/O12 | 首次 1639 通过、21 失败、1 跳过；后续处理见下文 | +| 正式产品配置 `--validate-only` | 初次验证拒绝缺失的新外参；本次现场外参绑定后检查通过，未启动机械手标定 | + +全包回归的 2 项失败来自新增运动接口后旧测试夹具缺少依赖，已改用正式 `MotionCommand` +并补齐采集接口，相关 8 项复跑全部通过。其余 19 项在修改前提交 `a8eaa4c` 的隔离代码中同样失败： + +| 原有失败所在测试文件 | 数量 | +|---|---| +| `test_all_view_image_acceptance.py` | 5 | +| `test_axis_line_gauge.py` | 2 | +| `test_capture_journal.py` | 1 | +| `test_final_image_sources.py` | 1 | +| `test_image_motion_capture_evidence.py` | 3 | +| `test_runtime_pose_branch_guard.py` | 6 | +| `test_urdf_zero.py` | 1 | + +这些既有失败涉及 O6 夹具的 Tag 尺寸、原始 URDF 文件选择、旧日志断言和几何误差断言。 +本次没有放宽标定验收门槛；更新测试夹具后仅复跑受影响测试，未再次执行全包回归。 +合成数据误差仅证明软件链路，现场精度仍须以实机第四轮独立验证和发布报告为准。 +本次更新外参哈希后，产品文件绑定及 O30 启动描述两项针对性测试均通过。 + +## 指令与反馈的验收职责（2026-09-18 更新） + +按现场确认，O30 使用 `measurement.release_basis: steady_command`。指令映射和修正 URDF +必须独立验收通过；反馈映射仅供诊断。其他 Profile 默认保留 `feedback_and_command`。 + +前三轮连续运动图像只确定冻结的视觉转轴和参考，不用运动中的指令值拟合实际角度。 +前三轮稳态图像拟合独立的指令曲线，并用于 CAD 串联轴几何、Tag 安装位姿的训练； +第四轮稳态图像独立验证指令曲线、几何零偏及最终 JSON＋URDF 的原始角点重投影。 +全部 20 关节的限位取训练指令曲线的实测范围，五个末端关节保留已确认的 CAD 零位。 + +报告 `quality.release_basis` 和限位证据明确标记来源；反馈诊断放在各关节 +`feedback_diagnostic` 和 `feedback_mapping_diagnostics.json`。反馈训练范围外的验证样本 +记录数量和图像身份，不夹紧、不外推、不混入训练。2026-09-19 起位置反馈数值只作诊断； +设备健康报告失联、活动故障和视觉运动/回零验证失败仍会暂停，详见本页前述视觉运动策略。 + +181201 会话的拇指 roll 按新流程离线复核：60 张独立稳态验证图像，指令映射 MAE +0.347683°、P95 0.730440°、最大 0.872949°。反馈训练范围 3–250，验证 859 张中 +71 张超出范围。这是单关节诊断，尚不是整手 URDF 或正式发布的验收。 + +184003 会话的前三个拇指任务各有 60 张独立稳态验证图像：roll 的 MAE/P95/最大误差为 +0.185306°/0.464223°/0.684213°，MCP 为 0.152367°/0.424799°/0.547374°， +IP 为 0.440852°/1.273472°/1.426604°。该会话在四指零位准备时停止，不是完整产物。 + +图像证据在内存中共享不可修改的冻结模型，逐帧图像诊断仍保持独立；JSON 内容、模型哈希 +和完整回读校验保持一致。对上述实测日志中 1000 条含模型记录抽样，保留内存从 44.09 MiB +降至 15.13 MiB。运行日志另记录超过 50 ms 的控制循环和垃圾回收停顿，用于定位反馈处理延迟， +不改变反馈新鲜度门槛。独立 SDK 监测在上次暂停期间仍有连续反馈，具体停顿来源需要后续日志确认。 + +190152 会话记录到随采集增长的垃圾回收停顿，最大约 0.55 秒。正式 ROS 运行改用轻量采集索引： +内存只保留覆盖率、分段重试和训练停点选择需要的字段；原始图像、位姿、冻结模型和哈希仍完整落盘。 +最终验收进程只读取已同步的日志边界，不能用轻量索引替代证据。相同 1000 条实测记录的索引 +保留内存为 1.025 MiB，完整共享模型记录为 15.135 MiB。进程内验收与诊断模式保留完整记录接口。 + +该会话 roll 指令验证通过（MAE 0.278128°，P95 0.706998°，最大 0.709360°）， +MCP 则因初始零位与首轮回程后的视觉位置相差约 2.1°而未通过。最后一轮 MCP 的 856 张图像 +全部保持 yaw 指令 80、反馈 76；随后 yaw=0 的 MCP 动作属于 IP 父标签准备。 +用户随后检查并固定了 ID1。旧 roll/MCP 的安装证据不能拼接到新安装状态，本次会话不发布产物。 + +### ID1 固定后的重采与当前断点 + +191601 会话重新采集后，roll 的 60 张独立稳态验证图像通过，MAE/P95/最大误差为 +0.274847°/0.668562°/0.854174°。MCP 首次回零仍相对冻结零位变化约 1.39°， +yaw 全程保持指令 80、反馈 76；因此停止该会话，没有以新的回零位置替换训练零位。 +轻量采集索引在这次实机运行中将节点内存稳定在约 210–214 MiB，记录到的垃圾回收停顿 +约 0.09–0.17 秒；此前 190152 会话最大约 0.55 秒。 + +随后分别检查连续/分段 MCP 路径、yaw 避让重新进入、roll 恢复后进入 MCP。 +这些独立诊断保存于 `mcp_path_diagnostic_20260918`、`mcp_entry_diagnostic_20260918` +和 `mcp_roll_entry_diagnostic_20260918`，均位于 `calibration_output/O30_RIGHT_001/`。 +连续/分段路径的五次视觉回零中位数跨度约 0.046°；roll 恢复后的四次运动回零跨度约 0.091°。 +这些结果未复现正式任务初次回零的 1.39°变化，尚不能确定其原因；未据此添加额外预热往返， +也未把诊断记录纳入正式训练或验证。 + +193712 会话再次开始正式采集，roll 第一轮两个方向采集完成，第二轮正向在指令 223、 +反馈 189 时触发 `MOTION-STALL-303`。关闭标定与 SDK 进程后，原 SDK 只读查询仍确认 +目标 223、位置 189，并报告 `thumb_roll: 执行器层判定堵转`；通信错误为 0，速度/力矩均为 200。 +保护日志中的 `goal=191.5` 是要求观察到的最小反馈推进量，不是下发指令。 +该会话的 `paused_device_readback.json` 保存直接设备读数，`restart_status.json` 保存断点与继续条件。 +必须先恢复正常硬件运动，再验证设备身份、固定基准及零位的一致性，才能判断哪些已完成单元可恢复。 +本会话尚无可发布 JSON 或修正 URDF；SDK 的 61 个受保护文件及原始 URDF 哈希均保持不变。 + +195023 会话重新从头采集，roll、MCP、IP 指令独立验证均通过（各 60 张稳态验证图像）， +最大误差分别为 0.841727°、0.513759°、1.443653°。小指侧摆零位准备两次均因转轴不确定度 +超出 3°而停止：255→72 只提供约 16.5°的转动，转轴不确定度约 3.7°。 +离线加密至逐整数指令抽样后仍约 3.07°，未通过原门槛，详见该会话的 +`pinky_sampling_diagnostic.json`。据此新增上述完整路径证据能力，保留原始记录，不将其改写为通过。 +新的 Profile 与调度尚需重新完成现场采集及整手独立验收。 + +200926 会话采用完整准备路径后，小指与无名指侧摆转轴建模均通过，转轴不确定度分别为 +2.307°、1.200°；774 张来源图像以及 6 份冻结模型完成来源哈希和图像模型回读校验， +见 `preparation_path_readback.json`。同会话 roll/MCP/IP 指令验证最大误差分别为 +0.663211°、0.559848°、1.473215°。 +该会话在中指侧摆准备阶段停止：两次转轴不确定度均约 7.6°，中指 ID14 的单张图像 +正方形拟合误差中位数约 0.80 px，小指/无名指约 0.17/0.20 px；不能仅以电机跟踪偏差解释。 +已要求检查 ID14 的平整度、边框形状和刚性固定，未放宽验收；此时无整手正式产物。 +保持固定基准、相机与拇指标签不变时，可在逐关节零位复核通过后恢复前三个拇指任务； +四指分组任务整体重新采集。保存的状态见该会话 `restart_status.json`。 + +2026-09-19 的 `100934` 恢复会话通过整场基准和拇指 roll 零位复核，但一次性导入旧零位 +来源记录使控制回调阻塞 3.530 秒，触发通信保护。独立只读监测同期持续收到反馈，SDK +健康报告保持在线、通信错误为 0。零位来源记录现与扫描记录共用每批最多 64 条的导入机制： +批次之间继续处理反馈、安全检查及中止，原零位只有在全部来源与引用记录落盘后才能授权运动。 +未放宽通信门槛，未更改配置或旧记录，因此仍可从 `200926` 断点恢复。 +对 `100934` 的 32,291 条实测记录分批重放，最长单批 0.1863 秒，顺序与内容回读一致， +见该会话 `resume_transfer_replay.json`；这项验证不是整手精度验收。 + +随后 `20260919_101609` 会话成功复用 roll 的 8 个采集单元，导入期间未再触发通信暂停。 +进入 MCP 零位准备时，MCP 指令升至 51，反馈连续停在 0，触发无运动保护。停机后直接 +读取设备仍为目标 51、位置 0；yaw 目标 80、反馈 76，速度与力矩均为 200，通信错误为 0。 +用户确认暂不处理该硬件问题，本次保留姿态并关闭标定、SDK、相机与只读监测；没有待执行动作。 +最新断点说明见 `20260919_101609/restart_status.json`，设备读数见 `paused_device_readback.json`。 + +断点发现按同一次从头采集及其恢复会话中的完整单元数选择候选，同数优先较新记录； +重复完成记录不增加计数，显式从头采集的边界阻止回溯到更旧的被替代记录。 +因此这次只导入 roll 的会话不会遮住 `200926` 中另外两个拇指任务。 +对实际会话目录运行选择器，仍选择 `20260918_200926`，见 `next_resume_selection.json`。 +这只选择候选数据：硬件恢复后仍须重新通过基准与各关节零位检查，MCP/IP 当前尚未授权复用， +四指侧摆整体重做，ID14 的几何可观测性仍待实机重验。启动状态明确显示“已请求断点恢复”， +不再在等待基准复核时误显示为从头采集。 + +## 位置反馈可选策略的软件验证（2026-09-19） + +- 20 个独立主动关节的非线性合成数据，分别使用变化的反馈、全部缺失的反馈、全部固定为 0 的反馈，均通过指令映射、15 个可观测零偏、五个 CAD 零位、限位及 JSON/URDF 回读检查；JSON 重建的 URDF 字节一致，无 mimic，原始 URDF 未改动。 +- 视觉监测单独覆盖缺图、实际不动、稳态抖动、回零未完成、每指独立观测、断点版本隔离，以及几何工作线程未运行时角点监测仍更新的情况。计算中的图像按自身时间戳取稳态证据，不能使用后续图像补证。 +- 本轮运行了 SDK、运动执行、采集、恢复、几何和产物相关回归；扩大回归中的 366 项通过，六项旧日志数量断言因新增全机位原始图像而失效。修正这组测试对“原始图像”和“有效关节样本”的区分后，相关视觉、分支保护及日志 46 项复跑通过。另有 58 项 SDK、统一引擎和产物检查通过。不将这些有重叠的测试数量相加；既有全包夹具问题仍按上文单独记录。 +- 对 `20260919_101609` 会话 MCP 的 112 帧原始图像进行只读重放：位置反馈为 0，实际相对角点行程为 28.736 px，视觉监测没有误报无运动。受控静止反例在原片段后额外延长 0.2 秒的合成帧后触发暂停。结果保存在该会话的 `feedback_optional_visual_replay.json`，明确不属于标定验收或端点证明。 +- 同会话拇指 roll 的 137 个历史稳态窗口，角点噪声中位数为 0.835 px、P95 为 1.528 px，98.54% 位于配置的 2 px 监测阈值内。此统计只用于运动监测参数检查，不替代独立角度精度验收。 +- 产品配置检查通过；外部 SDK 的 61 份源码文件及原始 URDF 哈希保持不变。本次没有启动机械手，也没有发布实机标定产物。 + +2026-09-19 `110202` 实机切换会话在基准恢复时发现，顶部两 Tag 相距约十个 Tag 边长， +原透视归一化将约 0.2 px 的检测波动放大为数像素，导致视觉稳定判断误暂停。 +运动监测改为四角点最小二乘相似变换（`relative_tag_corners_v2_similarity`），仅消除共同 +平移、旋转及均匀缩放；不把非共面的运动 Tag 投影到基准 Tag 平面，也不用于角度拟合。 +对该会话 297 条原始角点记录离线重放，roll/yaw 稳态窗口最大波动分别为 0.126/0.387 px， +保持 2 px 稳定阈值即可通过基准恢复，见 `110202/similarity_motion_replay.json`。 +运动、缺图、回零保护和独立角度验收门槛均保留；调度版本升至 v10，旧视觉策略记录不混用。 +此重放不构成实机标定产物验收。 +相似变换版本的运动、任务执行、就绪与 20 关节合成产物回归共 43 项通过。 +再次重放 MCP 反馈恒为 0 的 112 帧记录，实际运动见证为 27.370 px，未误暂停； +静止反例仍在 3.901 秒触发无运动暂停，记录单独保存为 +`101609/feedback_optional_similarity_replay.json`，保留此前算法诊断原件。 +随后 `110506` 会话发现量程工位遗留非零目标（食指 MCP、无名指 PIP/DIP 等）， +侧面仅可见 ID3/4/5，无法确认被遮挡关节回到基准，因此未执行该恢复路径。 +本次没有完整采集单元和验收产物，已关闭 SDK/相机，等待操作者恢复实际基准姿态后继续。 +SDK 的 61 份源码与原始 URDF 哈希再次确认未变。 + +随后用户明确要求程序启动时自行恢复基准。初始 `BASELINE` 现使用独立的 +`command_schedule` 完成条件：从设备目标寄存器读取起点，按配置通道组顺序执行限速 +轨迹及端点保持；通信、故障、重复控制端及指令范围保护持续有效，不要求被遮挡的 +运动 Tag 同时可见。日志明确写入 `physical_arrival_verified: false`，不生成稳态或零位 +证据。仅这一初始摆位阶段允许该完成条件，正式采集、任务间恢复、避让释放仍要求 +各自的观测证据；初始摆位之后仍先锁定固定基准，再逐任务冻结实测零位。 +对真实遗留目标姿态的虚拟时钟回归，完整发送基准目标、保持限速，且未生成有效样本、 +关节零位或完成采集单元。视觉相关 28 项检查通过，另有 29 项引擎/执行/就绪回归通过。 +`110927` 会话已自动恢复基准并完成拇指 roll 前两轮；第 3 轮正向 159→191 时视觉检测 +停动，75 帧、2.534 秒内相对角点最大位移仅 0.115 px，SDK 同时报告执行器堵转。 +本次按视觉停止，位置反馈数值仍不参与门控。19,692 条断点记录的视觉来源校验通过， +四个完整方向可作为恢复候选，见 `110927/checkpoint_stop_audit.json`。 +用户随后确认关节运动已恢复，继续启动正式标定并重新复核实际基准/零位。 +`111353` 恢复会话通过固定基准及拇指原零位检查，成功复用前两轮四个完整方向； +第 3 轮正向 191→223 再次发生视觉确认停动。用户要求停止标定以检查设备，已关闭 +标定、SDK、三台相机和只读监测，未发出恢复动作，没有待执行的自动运动。 +断点及回读结果分别保存在该会话的 `restart_status.json`、`checkpoint_stop_audit.json`。 +尚未生成通过整手验收的 JSON/URDF。后续须由用户确认继续，并重新复核实际基准与零位。 + +## 恢复反馈模式的软件验证 + +2026-09-19 根据用户确认恢复反馈模式，产品 Profile SHA256 更新为 +`4f30ece86fef9857dea1cd2d5201541ed16c6c0c81eab8b41a65285303088d01`。 +运行、恢复、同步、SDK 启动器及 G20/L6/O6/O12 相关回归 154 项通过；采集稳定性、 +时间关联、准备路径和 20 主动关节合成产物回归 158 项通过，两组有重叠，不合计数量。 +新增检查覆盖目标/反馈相差 ±10、中指分段范围、测得零位反馈为 10 的归位、 +反馈缺失/不新鲜、单指不动、只有小幅反馈噪声、指令映射不依赖反馈拟合。 +20 关节合成验证保留独立方向曲线、15 个零偏、五个 CAD 零位、原始 URDF 不变及 +JSON 重建修正 URDF 的一致性检查。缺失/恒定反馈的旧测试显式选择视觉策略， +不代表当前生产配置允许在缺失反馈时采集。 +产品配置只读校验通过，外部 SDK 的 61 份源码和原始 URDF 哈希保持不变。 +本次未启动实机；新的现场精度及正式产物仍需实际完整采集与独立验收。 diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index 5ff69ec..c20220a 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -1,6 +1,6 @@ # LinkerHand 多型号统一标定 -G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF +G20、L6、O6、O12、O30 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF 验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。 本包按观测关节冻结实机 baseline 零位，输出实测曲线及明确声明的复制曲线组成的 JSON v3， @@ -9,6 +9,64 @@ G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器 软件仿真通过不等于实机精度通过；当前原始 G20/L6/O12 存在需要确认的 mimic/限位冲突。 本轮实现与验证见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。 +O30 右手的构建、相机外参重标和完整操作说明见 [O30 右手标定](O30_RIGHT_CALIBRATION.md)。 +O30 当前使用反馈判断运动、稳定及零位恢复；允许指令与反馈存在固定偏差。最终 JSON 仍为指令→视觉角度，反馈映射只作诊断。 +测试的分层、选择范围与耗时记录见 [测试说明](TESTING.md)。 + +## O30 自适应训练试验 + +默认仍为 `fixed`：三轮训练加一轮独立验证。新会话可显式选择 `adaptive_2_to_3`：两轮训练后 +检查覆盖、稳态网格、双向重复性和可计算的几何零位质量；证据不足补第三轮，第三轮仍不合格 +就失败。每个方向的缺样恢复仍最多同速重扫一次，准备缺样仍最多局部恢复一次。 +速度、双向运动顺序、节点、准备路径及最终精度门限均保留。 + +```bash +# 只检查配置，不连接硬件 +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o30_right_product.yaml \ + --training-policy adaptive_2_to_3 --validate-only + +# 完成现场准备后，显式开始全新试验会话 +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o30_right_product.yaml \ + --training-policy adaptive_2_to_3 --no-resume +``` + +当前 O30 掌部共享几何依赖最后采集的四指侧摆。前面的任务无法可靠提前计算零位置信区间， +因此保守保留第三轮；最后一个任务满足全部门限时才减轮。本版完整计划的稳态停点最多从 +1390 降到 1340（约 3.6%），不承诺总耗时同比下降。所有任务都省一轮时的 1052 个停点只是 +理论下界，当前依赖和运动顺序下不能实现。实机连续通过及实际耗时验收完成前，不启用新默认。 + +采集计划 `capture_plan_v1` 绑定任务、运动分段、准备路径、网格与实际训练/验证身份。 +轮次 ID 保持不变：减轮后依次采集 0、1、3，界面显示第 1、2、3 轮；ID 3 始终为独立验证。 +`training_decision` 保存增补/冻结/失败决定、输入与计划哈希、冻结局部模型哈希及统计。 +几何依赖未齐时明确记录 `deferred`，不能将局部曲线一致性冒充零位认证。 +零位仍按真实独立轮次计算 Student t 置信区间，1° 半宽不变；曲线重复性使用既有角度误差门限。 +重复图像不会增加独立轮数，验证数据不参与训练、增补决策或模型选择。 + +最终产物仍依次经过训练冻结、独立验证、落盘 JSON、从 JSON 重建 URDF、图像与运动学回读、 +成对发布。Tag 安装拟合及运动范围证据也使用实际训练轮集合。五个末节 CAD 零位假设及已验证 +运动范围的精度声明保留；失败候选不会更新通过指针。 + +旧断点沿用固定策略，新旧策略不能拼接。自适应断点只复用连续完整任务；需要重做某个任务时， +其后依赖旧训练输入的任务一并重新采集。离线复算自适应会话也须传入同一 `--training-policy`。 +原始日志与已发布产物不被改写。训练预检在有总时限的独立进程执行，超时或取消会清理子进程。 +新会话的 `stage_timing.json` 按控制周期边界汇总准备、几何计算、等待、采样、恢复、断点加载及最终验收耗时。 + +已有固定三轮训练日志可作只读对照，保留同一独立验证集： + +```bash +PYTHONPATH=src/linkerhand_calibration python3 -m linkerhand_calibration.runtime.training_replay \ + --raw-samples calibration_output/O30_RIGHT_001/20260919_214048/raw_samples.jsonl \ + --source-urdf src/linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf \ + --output /tmp/o30-training-comparison.json +``` + +此工具复用采集索引，仅比较已完成任务的训练与角度验证，不生成发布产物，也不认证全手零位。 +2026-09-19 21:40 会话的 12 个完整任务在两种训练轮数下均通过原独立角度门限；其共享几何尚未 +齐全，不能据此批准两轮零位标定。约 2.88 GB 日志索引加载实测约 25 秒，作为后续性能工作的基线； +本次没有改动日志存储格式。 + ## 启动 ```bash @@ -26,10 +84,21 @@ ros2 run linkerhand_calibration calibrate_hand --config \ src/linkerhand_calibration/config/o12_right_product.yaml ``` -其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。 +其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`、`o30_right_product.yaml`。 AprilTag 检测参数直接读取产品指定的受保护 Tag YAML，统一启动文件不再覆盖其中的采样分辨率。 `calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。 `--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。 +产品配置的 `resume_mode: passed` 表示操作者确认安装位置未变：只读取已通过的完整任务， +不回放历史图像、不重新判定旧采样质量，也不复核已完成关节的物理零位；未完成任务整组重新采集。 +O30 当前使用此模式。配置/设备身份核对、实时保护和最终产物验收仍执行，恢复策略写入原始日志。 +默认 `resume_mode: verify` 会逐帧回读校验历史图像；若大断点超过默认 120 秒，可用 +`--startup-timeout-seconds 600` 显式延长节点初始化及设备就绪等待。此参数不改变运动、反馈新鲜度、 +堵转保护或精度验收门槛，也不会跳过断点基准和关节零位复核。 +断点预校验会共享不可变模型的解析结果，并在至少 256 帧时使用最多 4 个独立进程回放图像； +工作进程只做离线计算，在实时采集开始前退出。每帧角点、姿态、样本绑定及原始记录的内容哈希仍完整检查， +不跨会话缓存校验结论；小断点及收尾进程沿用串行校验。 +标定节点在启动校验后将长期驻留的对象移出循环垃圾回收扫描，避免大断点触发数秒停顿； +实时新增对象仍正常回收，节点退出后恢复原对象的回收管理。反馈超时保护不变。 重新开始时先安全回基准，仍必须确保现场没有障碍物。 ### JSON 到修正 URDF @@ -332,7 +401,8 @@ POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK ## 哪些情况暂停 实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、 -物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。 +物理越限、明显运动要求下超过 Profile 的无推进等待时间（O30 为 4 秒，其他型号为 2 秒）、 +已锁固定基准连续 10 帧漂移超过 5 px 而暂停。 开始后相机投影参数变化也会拒绝继续使用混合坐标数据。 当前关节持续缺失零位观测会暂停并列出关节、机位和所需 Tag；尚未轮到的手指遮挡不阻塞当前任务。 零位和稳态点的反馈稳定窗口以最新独立反馈为终点，并保留跨越窗口起点的一条反馈。 diff --git a/src/linkerhand_calibration/TESTING.md b/src/linkerhand_calibration/TESTING.md new file mode 100644 index 0000000..bfd2aa4 --- /dev/null +++ b/src/linkerhand_calibration/TESTING.md @@ -0,0 +1,61 @@ +# 标定测试的执行范围 + +日常修改运行受影响用例和必要契约用例，目标为 60 秒内完成。不要因为修改了一行代码就运行 +整手拟合或全量 `colcon test`。公共产物契约变更需要覆盖相关型号，发布前再执行全量回归。 + +测试保留原文件位置，避免破坏现有共享夹具。`test/conftest.py` 集中维护分类；函数上的显式标记优先。 + +| 标记 | 内容 | 何时执行 | +| --- | --- | --- | +| `quick` | 局部求解、调度、恢复、隔离和拒绝边界 | 日常选择受影响文件 | +| `replay` | 已记录的真实图像、准备失败和姿态歧义 | 修改对应观测或恢复逻辑 | +| `integration` | 完整拟合、产物回读、进程或 ROS 链路 | 修改公共契约或发布前 | +| `legacy` | 历史格式、旧型号算法及兼容入口 | 修改兼容契约或发布前 | + +从工作区根目录准备环境： + +```bash +source /opt/ros/jazzy/setup.bash +source install/setup.bash +export PYTHONPATH="$PWD/src/linkerhand_calibration:${PYTHONPATH:-}" +export PYTHONDONTWRITEBYTECODE=1 +``` + +采集计划、训练接口日常验证： + +```bash +python3 -m pytest -q -m quick \ + src/linkerhand_calibration/test/test_capture_plan.py \ + src/linkerhand_calibration/test/test_adaptive_training.py \ + src/linkerhand_calibration/test/test_command_motion.py \ + --durations=8 --timings-json=/tmp/calibration-quick.json +``` + +涉及零位恢复时加 `test_zero_recovery.py`；涉及断点时加 `test_prepared_resume.py`、 +`test_passed_resume.py`；涉及发布时加 `test_artifact_publication.py`。真实中指 DIP 歧义回归位于 +`test_measured_transfer.py::test_real_middle_dip_fits_but_remains_ambiguous`，其正确结果仍为拒绝不充分证据。 + +完整 O30 产物链路按需单独执行： + +```bash +python3 -m pytest -q \ + src/linkerhand_calibration/test/test_o30_artifacts.py::test_o30_nonlinear_twenty_channel_finalization \ + --durations=8 --timings-json=/tmp/calibration-o30-integration.json +``` + +该用例从正式采集证据入口进入，检查采样完成、训练决策回放、运动授权、受保护相机文件、 +原始角点、JSON→URDF 重建和回读。输入来自独立 FK 合成场景，使用兼容的几何运动授权； +不模拟在线图像模型选择，不替代实机连续采集及精度验收。末节 CAD 零位假设仍须保留原语义。 + +整手输入由会话级夹具共享，只读使用；需要篡改的用例先复制。反馈缺失、反馈卡住已改为局部 +映射契约测试，不再各生成一套整手 JSON/URDF。篡改、取消和失败指针检查优先复用验收边界。 + +`--timings-json` 记录各用例 setup/call/teardown 耗时、结果、层级和 Python/YAML 源码指纹。 +同一版本通过的测试不无理由复跑；修复失败后只重跑失败及受新改动影响的用例。裸 `pytest` +仍会运行全量，标记不会悄悄关闭测试。分层收集可用 `--collect-only -m replay` 等命令检查。 + +本次软件验证记录保存在工作区 +`calibration_output/O30_RIGHT_001/software_review_adaptive_training/software_validation.json`， +同目录保留各组耗时记录和历史数据对照。O30 正式产物链路与混合轮数统计约 44 秒， +恢复、断点及真实 DIP 歧义回归约 19 秒，在线训练评估与取消约 6 秒。记录保留开发期间 +的失败及修复后复查关系，不将不同代码版本的增量检查称为一次最终全量回归。 diff --git a/src/linkerhand_calibration/config/o30_right_18_tags.yaml b/src/linkerhand_calibration/config/o30_right_18_tags.yaml new file mode 100644 index 0000000..5c9eb84 --- /dev/null +++ b/src/linkerhand_calibration/config/o30_right_18_tags.yaml @@ -0,0 +1,57 @@ +/o30_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.0165 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [0, 1, 2, 12, 13, 14, 15] + frames: [front_base, thumb_mcp, thumb_ip, pinky_mcp_roll, ring_mcp_roll, middle_mcp_roll, index_mcp_roll] + sizes: [0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165] +/o30_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.0165 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [3, 4, 5, 6, 7, 8, 9, 10, 11] + frames: [side_base, pinky_pip, pinky_dip, ring_pip, ring_dip, middle_pip, middle_dip, index_pip, index_dip] + sizes: [0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165] +/o30_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.0165 + max_hamming: 0 + detector: + threads: 4 + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [16, 17] + frames: [top_base, thumb_cmc_yaw] + sizes: [0.0165, 0.0165] diff --git a/src/linkerhand_calibration/config/o30_right_product.yaml b/src/linkerhand_calibration/config/o30_right_product.yaml new file mode 100644 index 0000000..3df1fe2 --- /dev/null +++ b/src/linkerhand_calibration/config/o30_right_product.yaml @@ -0,0 +1,40 @@ +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: 1d2241cafc6acabca4d21711682ea012facd3ebce283fb0d8a7eea116d9696d4 +model: O30 +side: right +tag_layout: o30_right_18 +namespace: /o30_calibration +serial_number: O30_RIGHT_001 +can_interface: can0 +output_root: calibration_output +resume_mode: passed +cameras: + front: + serial_number: DB2163742 + camera_name: hikrobot_front_DB2163742 + camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml + side: + serial_number: DB2163749 + camera_name: hikrobot_side_DB2163749 + camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml + top: + serial_number: DB2163739 + camera_name: hikrobot_top_DB2163739 + camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml +artifacts: + source_urdf: package://linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf + source_urdf_sha256: 2be8428498ed8c17d7c39dee50e76ece6d362310cf26cd43834b8f4c79021b5b + camera_extrinsics: config/o30_three_camera_extrinsics_20260919_front_recalibrated.yaml + camera_extrinsics_sha256: 59f587e8e371f09456043c17cd0268fe1f170f1b7efdf9d2cb744c73c194953c + calibration_config: package://linkerhand_calibration/config/o30_three_camera_calibration.yaml + calibration_config_sha256: 95a8fd3299151755ea3b5209b74b9ec9de1c2d07a05d557e9018e742fb66ce30 + tag_config: package://linkerhand_calibration/config/o30_right_18_tags.yaml + tag_config_sha256: cba23cb42cbdc37455b631fc7dd063e73f48920e86fb06759ab01e26f4fcbf90 +release: + required_independent_passes: 1 + static_repeatability_deg: 1.0 +sdk: + driver: linker_hand_o30_ros2_sdk + transport: libcanbus diff --git a/src/linkerhand_calibration/config/o30_three_camera_calibration.yaml b/src/linkerhand_calibration/config/o30_three_camera_calibration.yaml new file mode 100644 index 0000000..17df655 --- /dev/null +++ b/src/linkerhand_calibration/config/o30_three_camera_calibration.yaml @@ -0,0 +1,58 @@ +o30_calibration: + ros__parameters: + command_topic: /cb_right_hand_control_cmd + state_topic: /cb_right_hand_state + setting_topic: /cb_right_hand_setting_cmd + front_camera_info_topic: /o30_calibration/front/camera/camera_info + front_detections_topic: /o30_calibration/front/apriltag/detections + side_camera_info_topic: /o30_calibration/side/camera/camera_info + side_detections_topic: /o30_calibration/side/apriltag/detections + top_camera_info_topic: /o30_calibration/top/camera/camera_info + top_detections_topic: /o30_calibration/top/apriltag/detections + baseline_command_u8: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + baseline_speed_u8: 200 + preflight_speed_u8: 200 + formal_speed_u8: 200 + speed_settle_seconds: 0.2 + command_trajectory_full_range_seconds: 20.02765316663493 + torque_u8: 200 + repetitions: 4 + preflight_checkpoints_u8: [0, 127, 255] + tag_size_m: 0.0165 + tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17] + tag_size_overrides_m: [0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, + 0.0165, 0.0165, 0.0165, 0.0165, 0.0165, 0.0165] + minimum_detection_rate: 0.95 + minimum_joint_frame_rate: 0.85 + minimum_feedback_hz: 25.0 + maximum_state_image_skew_ms: 50.0 + maximum_hamming: 0 + minimum_decision_margin: 30.0 + minimum_edge_pixels: 30.0 + pnp_maximum_reprojection_error_px: 1.5 + pnp_maximum_pose_jump_deg: 35.0 + pnp_maximum_translation_jump_m: 0.04 + pnp_maximum_tag_tilt_deg: 75.0 + pnp_tracker_reset_seconds: 5.0 + minimum_sweep_frames: 40 + minimum_state_span_u8: 240.0 + minimum_sweep_bins: 32 + maximum_bin_gap: 16 + maximum_monotonic_correction_deg: 2.0 + passive_maximum_monotonic_correction_deg: 3.0 + maximum_validation_mae_deg: 1.0 + maximum_validation_p95_deg: 2.0 + maximum_validation_error_deg: 3.0 + mimic_minimum_multiplier: 0.5 + mimic_maximum_multiplier: 2.2 + mimic_maximum_cycle_range: 0.03 + mimic_maximum_residual_p95_deg: 2.0 + endpoint_tolerance_u8: 2.0 + endpoint_hold_seconds: 1.0 + motor_stall_timeout_seconds: 4.0 + position_timeout_seconds: 60.0 + sweep_timeout_seconds: 90.0 + automatic_sweep_retry_limit: 1 + non_target_motion_tolerance_u8: 3.0 + fixed_base_maximum_corner_drift_px: 5.0 + fixed_base_movement_confirmation_frames: 10 diff --git a/src/linkerhand_calibration/config/profiles/o30_right_18.yaml b/src/linkerhand_calibration/config/profiles/o30_right_18.yaml new file mode 100644 index 0000000..9f6e84e --- /dev/null +++ b/src/linkerhand_calibration/config/profiles/o30_right_18.yaml @@ -0,0 +1,1044 @@ +schema_version: 1 +profile_id: O30/right/o30_right_18/v1 +namespace: /o30_calibration +sdk_adapter: o30_ros +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] + baseline_u8: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + command_index_by_joint: + thumb_cmc_roll: 0 + thumb_cmc_yaw: 1 + index_mcp_roll: 2 + middle_mcp_roll: 3 + ring_mcp_roll: 4 + pinky_mcp_roll: 5 + thumb_mcp: 6 + index_mcp_pitch: 7 + middle_mcp_pitch: 8 + ring_mcp_pitch: 9 + pinky_mcp_pitch: 10 + index_pip: 11 + middle_pip: 12 + ring_pip: 13 + pinky_pip: 14 + thumb_ip: 15 + index_dip: 16 + middle_dip: 17 + ring_dip: 18 + pinky_dip: 19 + urdf_joint_by_joint: + thumb_cmc_roll: thumb_cmc_roll + thumb_cmc_yaw: thumb_cmc_yaw + index_mcp_roll: index_mcp_roll + middle_mcp_roll: middle_mcp_roll + ring_mcp_roll: ring_mcp_roll + pinky_mcp_roll: pinky_mcp_roll + thumb_mcp: thumb_mcp + index_mcp_pitch: index_mcp_pitch + middle_mcp_pitch: middle_mcp_pitch + ring_mcp_pitch: ring_mcp_pitch + pinky_mcp_pitch: pinky_mcp_pitch + index_pip: index_pip + middle_pip: middle_pip + ring_pip: ring_pip + pinky_pip: pinky_pip + thumb_ip: thumb_ip + index_dip: index_dip + middle_dip: middle_dip + ring_dip: ring_dip + pinky_dip: pinky_dip + speed_slot_by_command_index: + '0': 0 + '1': 1 + '2': 2 + '3': 3 + '4': 4 + '5': 5 + '6': 6 + '7': 7 + '8': 8 + '9': 9 + '10': 10 + '11': 11 + '12': 12 + '13': 13 + '14': 14 + '15': 15 + '16': 16 + '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] +vision: + views: + - name: front + tags: + - role: front_base + id: 0 + fixed_reference: true + size_m: 0.0165 + link: hand_base_link + - role: thumb_mcp + id: 1 + fixed_reference: false + size_m: 0.0165 + link: thumb_proximal + - role: thumb_ip + id: 2 + fixed_reference: false + size_m: 0.0165 + link: thumb_distal + - role: pinky_mcp_roll + id: 12 + fixed_reference: false + size_m: 0.0165 + link: pinky_metacarpals + - role: ring_mcp_roll + id: 13 + fixed_reference: false + size_m: 0.0165 + link: ring_metacarpals + - role: middle_mcp_roll + id: 14 + fixed_reference: false + size_m: 0.0165 + link: middle_metacarpals + - role: index_mcp_roll + id: 15 + fixed_reference: false + size_m: 0.0165 + link: index_metacarpals + - name: side + tags: + - role: side_base + id: 3 + fixed_reference: true + size_m: 0.0165 + link: hand_base_link + - role: pinky_pip + id: 4 + fixed_reference: false + size_m: 0.0165 + link: pinky_middle + - role: pinky_dip + id: 5 + fixed_reference: false + size_m: 0.0165 + link: pinky_distal + - role: ring_pip + id: 6 + fixed_reference: false + size_m: 0.0165 + link: ring_middle + - role: ring_dip + id: 7 + fixed_reference: false + size_m: 0.0165 + link: ring_distal + - role: middle_pip + id: 8 + fixed_reference: false + size_m: 0.0165 + link: middle_middle + - role: middle_dip + id: 9 + fixed_reference: false + size_m: 0.0165 + link: middle_distal + - role: index_pip + id: 10 + fixed_reference: false + size_m: 0.0165 + link: index_middle + - role: index_dip + id: 11 + fixed_reference: false + size_m: 0.0165 + link: index_distal + - name: top + tags: + - role: top_base + id: 16 + fixed_reference: true + size_m: 0.0165 + link: hand_base_link + - role: thumb_cmc_yaw + id: 17 + fixed_reference: false + size_m: 0.0165 + link: thumb_metacarpals_base2 + common_frame: calibration_common + extrinsic_reference_view: front + extrinsics_quality_limits: + reprojection_rms_px: 1.5 + maximum_rotation_repeatability_deg: 0.3 + maximum_translation_repeatability_m: 0.0015 + minimum_capture_counts: + front_side_captures: 15 + front_top_captures: 15 +motion: + tasks: + - key: thumb_cmc_roll_front + view: front + command_index: 0 + joints: [thumb_cmc_roll] + auxiliary_commands: + - [2, 0] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: + - - [0, 0] + - - [2, 255] + preparation_groups: + - [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [0] + - key: thumb_mcp_front + view: front + command_index: 6 + joints: [thumb_mcp] + auxiliary_commands: + - [1, 80] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: + - - [6, 0] + - - [1, 0] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [6] + - key: thumb_ip_front + view: front + command_index: 15 + joints: [thumb_ip] + auxiliary_commands: [] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 16, 17, 18, 19] + - [15] + parent_reference: + pose_joint: thumb_cmc_roll + geometry_joint: thumb_mcp + - key: thumb_cmc_yaw_top + view: top + command_index: 1 + joints: [thumb_cmc_yaw] + auxiliary_commands: [] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [1] + - key: pinky_mcp_pitch_side + view: side + command_index: 10 + joints: [pinky_mcp_pitch] + auxiliary_commands: [] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [10] + - key: pinky_pip_side + view: side + command_index: 14 + joints: [pinky_pip] + auxiliary_commands: [] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 15, 16, 17, 18, 19] + - [14] + - key: pinky_dip_side + view: side + command_index: 19 + joints: [pinky_dip] + auxiliary_commands: [] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: + - - [10, 255] + - [14, 255] + - [19, 255] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18] + - [19] + parent_reference: + pose_joint: pinky_mcp_pitch + geometry_joint: pinky_pip + - key: ring_mcp_pitch_side + view: side + command_index: 9 + joints: [ring_mcp_pitch] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [9] + - key: ring_pip_side + view: side + command_index: 13 + joints: [ring_pip] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 14, 15, 16, 17, 18, 19] + - [13] + - key: ring_dip_side + view: side + command_index: 18 + joints: [ring_dip] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: + - - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 19] + - [18] + parent_reference: + pose_joint: ring_mcp_pitch + geometry_joint: ring_pip + - key: middle_mcp_pitch_side + view: side + command_index: 8 + joints: [middle_mcp_pitch] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [8] + - key: middle_pip_side + view: side + command_index: 12 + joints: [middle_pip] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 13, 14, 15, 16, 17, 18, 19] + - [12] + - key: middle_dip_side + view: side + command_index: 17 + joints: [middle_dip] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: + - - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + - [8, 255] + - [12, 255] + - [17, 255] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 18, 19] + - [17] + parent_reference: + pose_joint: middle_mcp_pitch + geometry_joint: middle_pip + - key: index_mcp_pitch_side + view: side + command_index: 7 + joints: [index_mcp_pitch] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + - [8, 255] + - [12, 255] + - [17, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [7] + - key: index_pip_side + view: side + command_index: 11 + joints: [index_pip] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + - [8, 255] + - [12, 255] + - [17, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 13, 14, 15, 16, 17, 18, 19] + - [11] + - key: index_dip_side + view: side + command_index: 16 + joints: [index_dip] + auxiliary_commands: + - [10, 255] + - [14, 255] + - [19, 255] + - [9, 255] + - [13, 255] + - [18, 255] + - [8, 255] + - [12, 255] + - [17, 255] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + exit_waypoints: [] + preparation_groups: + - [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 17, 18, 19] + - [16] + parent_reference: + pose_joint: index_mcp_pitch + geometry_joint: index_pip + - key: fingers_mcp_roll_front + view: front + command_index: 5 + joints: [pinky_mcp_roll, ring_mcp_roll, middle_mcp_roll, index_mcp_roll] + start_u8: 0 + end_u8: 255 + preflight_speed_u8: 200 + formal_speed_u8: 200 + preparation_groups: + - [0, 1, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + - [5, 4, 3, 2] + segments: + - key: linked_increasing + start_commands: + - [5, 0] + - [4, 0] + - [3, 80] + - [2, 0] + end_commands: + - [5, 255] + - [4, 255] + - [3, 255] + - [2, 255] + joints: [pinky_mcp_roll, ring_mcp_roll, middle_mcp_roll, index_mcp_roll] + - key: linked_decreasing + start_commands: + - [5, 255] + - [4, 255] + - [3, 255] + - [2, 255] + end_commands: + - [5, 0] + - [4, 0] + - [3, 80] + - [2, 0] + joints: [pinky_mcp_roll, ring_mcp_roll, middle_mcp_roll, index_mcp_roll] + - key: middle_lower_decreasing + start_commands: + - [5, 0] + - [4, 0] + - [3, 80] + - [2, 0] + end_commands: + - [5, 0] + - [4, 0] + - [3, 0] + - [2, 0] + joints: [middle_mcp_roll] + - key: middle_lower_increasing + start_commands: + - [5, 0] + - [4, 0] + - [3, 0] + - [2, 0] + end_commands: + - [5, 0] + - [4, 0] + - [3, 80] + - [2, 0] + joints: [middle_mcp_roll] + joint_zero_references: + thumb_cmc_roll: + task_key: thumb_cmc_roll_front + command: [0, 0, 0, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [255, 0, 0, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + thumb_mcp: + task_key: thumb_mcp_front + command: [0, 80, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [0, 80, 255, 196, 113, 52, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + thumb_ip: + task_key: thumb_ip_front + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 0, 0, 0, 0] + pinky_mcp_roll: + task_key: fingers_mcp_roll_front + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + evidence_scope: path + approach_commands: + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + ring_mcp_roll: + task_key: fingers_mcp_roll_front + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + evidence_scope: path + approach_commands: + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + middle_mcp_roll: + task_key: fingers_mcp_roll_front + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + evidence_scope: path + approach_commands: + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + index_mcp_roll: + task_key: fingers_mcp_roll_front + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + evidence_scope: path + approach_commands: + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + pinky_mcp_pitch: + task_key: pinky_mcp_pitch_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0] + pinky_pip: + task_key: pinky_pip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 255, 0, 0, 0, 0, 0] + pinky_dip: + task_key: pinky_dip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255] + ring_mcp_pitch: + task_key: ring_mcp_pitch_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 255, 0, 0, 0, 255, 0, 0, 0, 0, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 255, 255, 0, 0, 0, 255, 0, 0, 0, 0, 255] + ring_pip: + task_key: ring_pip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 255, 0, 0, 0, 255, 0, 0, 0, 0, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 255, 0, 0, 255, 255, 0, 0, 0, 0, 255] + ring_dip: + task_key: ring_dip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 255, 0, 0, 0, 255, 0, 0, 0, 0, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 255, 0, 0, 0, 255, 0, 0, 0, 255, 255] + middle_mcp_pitch: + task_key: middle_mcp_pitch_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 0, 255, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 255, 255, 255, 0, 0, 255, 255, 0, 0, 0, 255, 255] + middle_pip: + task_key: middle_pip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 0, 255, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 255, 255, 0, 255, 255, 255, 0, 0, 0, 255, 255] + middle_dip: + task_key: middle_dip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 0, 255, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 0, 255, 255, 0, 0, 255, 255, 0, 0, 255, 255, 255] + index_mcp_pitch: + task_key: index_mcp_pitch_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 255, 255, 255, 0, 255, 255, 255, 0, 0, 255, 255, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 255, 255, 255, 255, 0, 255, 255, 255, 0, 0, 255, 255, 255] + index_pip: + task_key: index_pip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 255, 255, 255, 0, 255, 255, 255, 0, 0, 255, 255, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 255, 255, 255, 255, 255, 255, 255, 0, 0, 255, 255, 255] + index_dip: + task_key: index_dip_side + command: [0, 0, 255, 196, 113, 52, 0, 0, 255, 255, 255, 0, 255, 255, 255, 0, 0, 255, 255, 255] + approach_commands: + - [0, 0, 255, 196, 113, 52, 0, 0, 255, 255, 255, 0, 255, 255, 255, 0, 255, 255, 255, 255] + thumb_cmc_yaw: + task_key: thumb_cmc_yaw_top + command: [0, 0, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + approach_commands: + - [0, 255, 255, 196, 113, 52, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + speed_parameters: + baseline_u8: 200 + preflight_u8: 200 + formal_u8: 200 + torque_u8: 200 + speed_settle_seconds: 0.2 + command_trajectory_full_range_seconds: 13.35176877775662 + scan_trajectory_full_range_seconds: 13.35176877775662 + endpoint_hold_seconds: 1.0 + stall_timeout_seconds: 4.0 + return_groups: + - [0, 1, 6, 15] + - [7, 8, 9, 10, 11, 12, 13, 14, 16, 17, 18, 19] + - [2, 3, 4, 5] +measurement: + release_basis: steady_command + measurements: + thumb_cmc_roll: + joint: thumb_cmc_roll + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_mcp + pose_axis_line_required: true + thumb_mcp: + joint: thumb_mcp + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_mcp + pose_axis_line_required: true + thumb_ip: + joint: thumb_ip + kind: relative_rotation + view: front + parent_role: thumb_mcp + child_role: thumb_ip + pose_axis_line_required: true + thumb_cmc_yaw: + joint: thumb_cmc_yaw + kind: relative_rotation + view: top + parent_role: top_base + child_role: thumb_cmc_yaw + pose_axis_line_required: true + pinky_mcp_roll: + joint: pinky_mcp_roll + kind: relative_rotation + view: front + parent_role: front_base + child_role: pinky_mcp_roll + pose_axis_line_required: true + pinky_mcp_pitch: + joint: pinky_mcp_pitch + kind: relative_rotation + view: side + parent_role: side_base + child_role: pinky_pip + pose_axis_line_required: true + pinky_pip: + joint: pinky_pip + kind: relative_rotation + view: side + parent_role: side_base + child_role: pinky_pip + pose_axis_line_required: true + pinky_dip: + joint: pinky_dip + kind: relative_rotation + view: side + parent_role: pinky_pip + child_role: pinky_dip + pose_axis_line_required: true + ring_mcp_roll: + joint: ring_mcp_roll + kind: relative_rotation + view: front + parent_role: front_base + child_role: ring_mcp_roll + pose_axis_line_required: true + ring_mcp_pitch: + joint: ring_mcp_pitch + kind: relative_rotation + view: side + parent_role: side_base + child_role: ring_pip + pose_axis_line_required: true + ring_pip: + joint: ring_pip + kind: relative_rotation + view: side + parent_role: side_base + child_role: ring_pip + pose_axis_line_required: true + ring_dip: + joint: ring_dip + kind: relative_rotation + view: side + parent_role: ring_pip + child_role: ring_dip + pose_axis_line_required: true + middle_mcp_roll: + joint: middle_mcp_roll + kind: relative_rotation + view: front + parent_role: front_base + child_role: middle_mcp_roll + pose_axis_line_required: true + middle_mcp_pitch: + joint: middle_mcp_pitch + kind: relative_rotation + view: side + parent_role: side_base + child_role: middle_pip + pose_axis_line_required: true + middle_pip: + joint: middle_pip + kind: relative_rotation + view: side + parent_role: side_base + child_role: middle_pip + pose_axis_line_required: true + middle_dip: + joint: middle_dip + kind: relative_rotation + view: side + parent_role: middle_pip + child_role: middle_dip + pose_axis_line_required: true + index_mcp_roll: + joint: index_mcp_roll + kind: relative_rotation + view: front + parent_role: front_base + child_role: index_mcp_roll + pose_axis_line_required: true + index_mcp_pitch: + joint: index_mcp_pitch + kind: relative_rotation + view: side + parent_role: side_base + child_role: index_pip + pose_axis_line_required: true + index_pip: + joint: index_pip + kind: relative_rotation + view: side + parent_role: side_base + child_role: index_pip + pose_axis_line_required: true + index_dip: + joint: index_dip + kind: relative_rotation + view: side + parent_role: index_pip + child_role: index_dip + pose_axis_line_required: true + directional_zero: true +zero: + active_joints: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, thumb_ip, index_mcp_roll, index_mcp_pitch, index_pip, index_dip, + middle_mcp_roll, middle_mcp_pitch, middle_pip, middle_dip, ring_mcp_roll, ring_mcp_pitch, ring_pip, ring_dip, pinky_mcp_roll, + pinky_mcp_pitch, pinky_pip, pinky_dip] + passive_joints: [] + direct_zero_joints: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, index_mcp_roll, index_mcp_pitch, index_pip, middle_mcp_roll, + middle_mcp_pitch, middle_pip, ring_mcp_roll, ring_mcp_pitch, ring_pip, pinky_mcp_roll, pinky_mcp_pitch, pinky_pip] + axis_joints: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, thumb_ip, index_mcp_roll, index_mcp_pitch, index_pip, index_dip, + middle_mcp_roll, middle_mcp_pitch, middle_pip, middle_dip, ring_mcp_roll, ring_mcp_pitch, ring_pip, ring_dip, pinky_mcp_roll, + pinky_mcp_pitch, pinky_pip, pinky_dip] + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: {} + cad_frozen_joints: [thumb_ip, pinky_dip, ring_dip, middle_dip, index_dip] + cad_zero_assumptions: + thumb_ip: User accepts original CAD joint zero at baseline; not independently measured. + pinky_dip: User accepts original CAD joint zero at baseline; not independently measured. + ring_dip: User accepts original CAD joint zero at baseline; not independently measured. + middle_dip: User accepts original CAD joint zero at baseline; not independently measured. + index_dip: User accepts original CAD joint zero at baseline; not independently measured. + spatial: + base_pose_strategy: full_hand + parallel_root_pattern: true + root_anchor_joints: [thumb_cmc_roll, index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll] + depth_free_axis_projection: true + axis_order: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, thumb_ip, index_mcp_roll, index_mcp_pitch, index_pip, index_dip, + middle_mcp_roll, middle_mcp_pitch, middle_pip, middle_dip, ring_mcp_roll, ring_mcp_pitch, ring_pip, ring_dip, pinky_mcp_roll, + pinky_mcp_pitch, pinky_pip, pinky_dip] + axis_parent_joint: + thumb_cmc_yaw: thumb_cmc_roll + thumb_mcp: thumb_cmc_yaw + pinky_mcp_pitch: pinky_mcp_roll + ring_mcp_pitch: ring_mcp_roll + middle_mcp_pitch: middle_mcp_roll + index_mcp_pitch: index_mcp_roll + phase_parent_joint: + thumb_ip: thumb_mcp + pinky_pip: pinky_mcp_pitch + pinky_dip: pinky_pip + ring_pip: ring_mcp_pitch + ring_dip: ring_pip + middle_pip: middle_mcp_pitch + middle_dip: middle_pip + index_pip: index_mcp_pitch + index_dip: index_pip + offset_observer_joint: + thumb_cmc_roll: thumb_cmc_yaw + thumb_cmc_yaw: thumb_mcp + thumb_mcp: thumb_ip + pinky_mcp_roll: pinky_mcp_pitch + pinky_mcp_pitch: pinky_pip + pinky_pip: pinky_dip + ring_mcp_roll: ring_mcp_pitch + ring_mcp_pitch: ring_pip + ring_pip: ring_dip + middle_mcp_roll: middle_mcp_pitch + middle_mcp_pitch: middle_pip + middle_pip: middle_dip + index_mcp_roll: index_mcp_pitch + index_mcp_pitch: index_pip + index_pip: index_dip +quality: + training_cycles: [0, 1, 2] + holdout_cycle: 3 + hard_threshold_keys: [maximum_mimic_residual_rad, maximum_state_image_skew_ms, maximum_validation_error_rad, minimum_detection_rate] + retry_metric_scope: {} + isolated_holdout: true +scope: + default_scope: full + calibrate_joints: + without_finger_roll: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, thumb_ip, + index_mcp_pitch, index_pip, index_dip, middle_mcp_pitch, middle_pip, middle_dip, + ring_mcp_pitch, ring_pip, ring_dip, pinky_mcp_pitch, pinky_pip, pinky_dip] + full: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, thumb_ip, index_mcp_roll, index_mcp_pitch, index_pip, index_dip, + middle_mcp_roll, middle_mcp_pitch, middle_pip, middle_dip, ring_mcp_roll, ring_mcp_pitch, ring_pip, ring_dip, pinky_mcp_roll, + pinky_mcp_pitch, pinky_pip, pinky_dip] + frozen_joints: + full: [] + without_finger_roll: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll] + retained_cad_scopes: + without_finger_roll: + reason: User excludes four finger MCP roll joints from motion calibration and URDF correction. Their source CAD zero at the held baseline is an unmeasured prerequisite of partial geometry validation. + root_anchor_joints: [thumb_cmc_roll] + orientation_anchor_joint: index_mcp_pitch + directed_base_axis_joints: [thumb_cmc_roll, index_mcp_pitch] +artifacts: + output_schema_version: 3 + directional_command_joints: [thumb_cmc_roll, thumb_cmc_yaw, thumb_mcp, thumb_ip, index_mcp_roll, index_mcp_pitch, index_pip, + index_dip, middle_mcp_roll, middle_mcp_pitch, middle_pip, middle_dip, ring_mcp_roll, ring_mcp_pitch, ring_pip, ring_dip, + pinky_mcp_roll, pinky_mcp_pitch, pinky_pip, pinky_dip] + calibration_filename: o30_right_{serial_number}_calibration.json + corrected_urdf_filename: linkerhand_o30_right_{serial_number}_zero_calibrated.urdf + protected_input_fields: [calibration_config_sha256, camera_extrinsics_sha256, profile_config_sha256, source_urdf_sha256, + tag_config_sha256] + publication_pointer: latest_passed + session_compatibility_tokens: [o30_20_active_v1, segmented_motion_v1, task_scoped_image_models_v1] + publish_corrected_urdf: true +acquisition: + # Fixed Tags witness image stationarity; their physical normals are not required. + fixed_reference_mode: stationary_image + joint_zero_timeout_seconds: 5.0 + command_capture_mode: interleaved + steady_training_nodes: 9 + steady_extra_training_nodes: {} + policy_version: unified_engine_v8_all_view_images + mapping_probe_maximum_rad: 0.0 + automatic_rescan_limit: 1 + minimum_valid_samples: 40 + minimum_bins: 32 + maximum_unobserved_fraction: 0.0625 + legacy_minimum_span_01: 0.9411764705882353 + physical_first_cycle_minimum_span_01: 0.85 + physical_repeat_minimum_fraction: 0.9 + stall_timeout_seconds: 4.0 + feedback_stale_seconds: 1.0 + fixed_reference_minimum_frames: 10 + fixed_reference_maximum_drift_px: 5.0 + fixed_reference_confirmation_frames: 10 + motion_model_scope: task + feedback_travel_matches_command: false + motion_observation: feedback + visual_stale_seconds: 1.0 + visual_minimum_motion_px: 1.0 + visual_stability_px: 0.35 + visual_zero_tolerance_px: 1.0 + require_endpoint_observations: true +urdf: + limit_policies: + thumb_cmc_roll: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + thumb_cmc_yaw: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + thumb_mcp: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + thumb_ip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + index_mcp_roll: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + index_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + index_pip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + index_dip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + middle_mcp_roll: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + middle_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + middle_pip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + middle_dip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + ring_mcp_roll: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + ring_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + ring_pip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + ring_dip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + pinky_mcp_roll: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + pinky_mcp_pitch: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + pinky_pip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + pinky_dip: + source_limits: calibrated_motion_envelope + evidence_source: Measured independent 0–255 motion; original CAD limits are nominal. + authorized_fields: + thumb_cmc_roll: [limit.lower, limit.upper, origin.rpy] + thumb_cmc_yaw: [limit.lower, limit.upper, origin.rpy] + thumb_mcp: [limit.lower, limit.upper, origin.rpy] + thumb_ip: [limit.lower, limit.upper] + index_mcp_roll: [limit.lower, limit.upper, origin.rpy] + index_mcp_pitch: [limit.lower, limit.upper, origin.rpy] + index_pip: [limit.lower, limit.upper, origin.rpy] + index_dip: [limit.lower, limit.upper] + middle_mcp_roll: [limit.lower, limit.upper, origin.rpy] + middle_mcp_pitch: [limit.lower, limit.upper, origin.rpy] + middle_pip: [limit.lower, limit.upper, origin.rpy] + middle_dip: [limit.lower, limit.upper] + ring_mcp_roll: [limit.lower, limit.upper, origin.rpy] + ring_mcp_pitch: [limit.lower, limit.upper, origin.rpy] + ring_pip: [limit.lower, limit.upper, origin.rpy] + ring_dip: [limit.lower, limit.upper] + pinky_mcp_roll: [limit.lower, limit.upper, origin.rpy] + pinky_mcp_pitch: [limit.lower, limit.upper, origin.rpy] + pinky_pip: [limit.lower, limit.upper, origin.rpy] + pinky_dip: [limit.lower, limit.upper] +joint_coverage: + thumb_cmc_roll: measured_static_dynamic + thumb_cmc_yaw: measured_static_dynamic + thumb_mcp: measured_static_dynamic + thumb_ip: measured_dynamic_cad_static + index_mcp_roll: measured_static_dynamic + index_mcp_pitch: measured_static_dynamic + index_pip: measured_static_dynamic + index_dip: measured_dynamic_cad_static + middle_mcp_roll: measured_static_dynamic + middle_mcp_pitch: measured_static_dynamic + middle_pip: measured_static_dynamic + middle_dip: measured_dynamic_cad_static + ring_mcp_roll: measured_static_dynamic + ring_mcp_pitch: measured_static_dynamic + ring_pip: measured_static_dynamic + ring_dip: measured_dynamic_cad_static + pinky_mcp_roll: measured_static_dynamic + pinky_mcp_pitch: measured_static_dynamic + pinky_pip: measured_static_dynamic + pinky_dip: measured_dynamic_cad_static diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 039ebe7..f074a24 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -63,7 +63,7 @@ def _launch_stack(context): raise RuntimeError(f"tag config does not exist: {tag_config}") topic_prefix = f"/{model.lower()}" uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk" - if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk"}: + if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk", "o30_ros"}: raise RuntimeError("SDK adapter has no ROS launch binding") topics = sdk_topics(contract.typed_profile) command_topic, state_topic = topics.command, topics.feedback @@ -274,6 +274,12 @@ def _launch_stack(context): }], ) ) + if contract.typed_profile.sdk_adapter == "o30_ros": + from linkerhand_calibration.runtime.adapters.o30_ros import launch_parameters + sdk = Node(package="linker_hand_o30_ros2_sdk", executable="linker_hand_o30_ros2_sdk", + name="linker_hand_o30_sdk", output="screen", + condition=IfCondition(LaunchConfiguration("start_sdk")), + parameters=[launch_parameters(side=hand_type)]) calibration = Node( package="linkerhand_calibration", executable="three_camera_calibration_node", @@ -293,8 +299,12 @@ def _launch_stack(context): "resume_raw_samples_path": LaunchConfiguration( "resume_raw_samples_path" ), + "resume_mode": LaunchConfiguration("resume_mode"), + "training_policy": LaunchConfiguration("training_policy"), + "initial_command_file": LaunchConfiguration("initial_command_file"), "command_topic": command_topic, "state_topic": state_topic, + **({"setting_topic": topics.setting} if topics.setting else {}), "camera_extrinsics_file": LaunchConfiguration( "camera_extrinsics_file" ), @@ -449,6 +459,9 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument("session_dir", default_value=""), DeclareLaunchArgument("resume_raw_samples_path", default_value=""), + DeclareLaunchArgument("resume_mode", default_value="verify"), + DeclareLaunchArgument("training_policy", default_value="fixed"), + DeclareLaunchArgument("initial_command_file", default_value=""), DeclareLaunchArgument( "calibration_config", default_value="", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py new file mode 100644 index 0000000..33df39c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/capture_plan.py @@ -0,0 +1,127 @@ +"""One immutable partition contract for acquisition, fitting and release. + +Cycle IDs are evidence identities, not display ordinals. Optional training +never renumbers the independent validation cycle or changes its command grid. +""" + +from dataclasses import asdict, dataclass, replace +import hashlib +import json + + +PLAN_VERSION = "capture_plan_v1" +FIXED = "fixed" +ADAPTIVE = "adaptive_2_to_3" +LEGACY_TRAINING = (0, 1, 2) +LEGACY_HOLDOUT = 3 + + +def evidence_digest(value): + return hashlib.sha256(json.dumps(value, sort_keys=True, separators=(",", ":"), + allow_nan=False).encode()).hexdigest() + + +@dataclass(frozen=True) +class TaskPartition: + task_key: str + training: tuple[int, ...] + holdout: int + command_training: tuple[int, ...] + command_holdout: int + + @property + def motion_cycles(self): + return (*self.training, self.holdout) + + def cycles(self, phase): + return ((*self.command_training, self.command_holdout) + if phase == "steady" else self.motion_cycles) + + +@dataclass(frozen=True) +class CapturePlan: + policy: str + command_capture_mode: str + tasks: tuple[TaskPartition, ...] + motion_contract_json: str + + @classmethod + def from_profile(cls, profile): + tasks = tuple(task_partition(profile, task) for task in profile.motion.tasks) + # JSON normalization keeps equality stable across checkpoint round trips. + from .sampling import command_nodes + motion = {"unit": profile.command.unit, "motion": asdict(profile.motion), + "nodes": {task.key: {"training": command_nodes(profile, task), + "holdout": command_nodes(profile, task, holdout=True)} for task in profile.motion.tasks}} + return cls(profile.quality.training_policy, profile.acquisition.command_capture_mode, tasks, + json.dumps(motion, sort_keys=True, separators=(",", ":"), allow_nan=False)) + + def task(self, key): + return next(task for task in self.tasks if task.task_key == key) + + def as_dict(self): + return {"version": PLAN_VERSION, "policy": self.policy, + "command_capture_mode": self.command_capture_mode, + "motion_contract": json.loads(self.motion_contract_json), + "tasks": {t.task_key: {"training_cycles": list(t.training), + "holdout_cycle": t.holdout, + "command_training_cycles": list(t.command_training), + "command_holdout_cycle": t.command_holdout} for t in self.tasks}} + + @property + def sha256(self): + return evidence_digest(self.as_dict()) + + +def task_for_joint(profile, joint): + donor = profile.measurement.transferred_motion_sources.get(joint, joint) + secondary = {source: primary for primary, source in profile.measurement.cross_view_sources.items()} + donor = secondary.get(donor, donor) + for task in profile.motion.tasks: + if donor in task.joints: + return task + raise ValueError(f"capture_plan_joint_has_no_task:{joint}") + + +def task_partition(profile, task=None, *, joint=None): + if joint is not None: + task = task_for_joint(profile, joint) + key = getattr(task, "key", task) + training = tuple(profile.quality.task_training_cycles.get(key, profile.quality.training_cycles)) + holdout = profile.quality.holdout_cycle + separate = profile.acquisition.command_capture_mode == "separate" + return TaskPartition(key, training, holdout, (holdout + 1,) if separate else training, + holdout + 2 if separate else holdout) + + +def training_cycles(profile, task=None, *, joint=None): + return task_partition(profile, task, joint=joint).training + + +def select_task_training(profile, task_key, cycles): + cycles = tuple(cycles) + if task_key not in {t.key for t in profile.motion.tasks}: + raise ValueError("capture_plan_unknown_task") + if cycles not in (LEGACY_TRAINING, LEGACY_TRAINING[:2]): + raise ValueError("capture_plan_invalid_training_partition") + if cycles != tuple(profile.quality.training_cycles) and profile.quality.training_policy != ADAPTIVE: + raise ValueError("capture_plan_fixed_training_cannot_change") + selections = {**profile.quality.task_training_cycles, task_key: cycles} + return replace(profile, quality=replace(profile.quality, task_training_cycles=selections)) + + +def configure_training(profile, policy): + if policy not in {FIXED, ADAPTIVE}: + raise ValueError("unknown_training_policy") + if profile.artifacts.output_schema_version in {2, 3} and ( + tuple(profile.quality.training_cycles) != LEGACY_TRAINING + or profile.quality.holdout_cycle != LEGACY_HOLDOUT): + raise ValueError("production_partition_requires_versioned_training_policy") + if policy == ADAPTIVE and (profile.artifacts.output_schema_version < 3 + or not profile.command_based_release + or profile.acquisition.command_capture_mode != "interleaved" + or tuple(profile.quality.training_cycles) != LEGACY_TRAINING + or profile.quality.holdout_cycle != LEGACY_HOLDOUT): + raise ValueError("adaptive_training_requires_measured_interleaved_command_release") + return replace(profile, quality=replace(profile.quality, + training_policy=policy, task_training_cycles={})) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/motion_path.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/motion_path.py new file mode 100644 index 0000000..39aa349 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/motion_path.py @@ -0,0 +1,78 @@ +"""Declared multi-channel paths and durable scan identities. + +Only paths own interpolation. Every observation retains its own motor channel; +the scalar path coordinate is never a joint measurement or a new zero. +""" + +from dataclasses import dataclass +import math + + +@dataclass(frozen=True) +class MotionSegment: + key: str + start_commands: tuple[tuple[int, float], ...] + end_commands: tuple[tuple[int, float], ...] + joints: tuple[str, ...] + + @property + def moving_channels(self): + end = dict(self.end_commands) + return tuple(i for i, value in self.start_commands if end[i] != value) + + @property + def command_index(self): + return self.moving_channels[0] + + @property + def start(self): + return dict(self.start_commands)[self.command_index] + + @property + def end(self): + return dict(self.end_commands)[self.command_index] + + @property + def direction(self): + return "increasing" if self.end > self.start else "decreasing" + + def commands_at(self, value, *, quantize=False): + fraction = (value-self.start)/(self.end-self.start) + if not math.isfinite(fraction) or not -1e-9 <= fraction <= 1+1e-9: + raise ValueError("motion segment coordinate outside declared path") + end = dict(self.end_commands) + values = {i: start+(end[i]-start)*fraction for i, start in self.start_commands} + return {i: float(round(v)) for i, v in values.items()} if quantize else values + + +def task_segments(task): + if task.segments: + return task.segments + return tuple(MotionSegment("", ((task.command_index, start),), + ((task.command_index, end),), task.joints) + for start, end in ((task.start_value, task.end_value), (task.end_value, task.start_value))) + + +def task_segment(task, key): + for segment in task.segments: + if segment.key == key: + return segment + raise ValueError(f"unknown motion segment:{task.key}:{key}") + + +def task_channels(task): + return tuple(sorted({i for segment in task_segments(task) for i in segment.moving_channels})) + + +def scan_identity(task, cycle, direction, segment_key=""): + base = (task, cycle, direction) + return (*base, segment_key) if segment_key else base + + +def record_scan_identity(row): + return scan_identity(row.get("task_name"), row.get("cycle"), row.get("direction"), + row.get("segment_key", "")) + + +def segment_metadata(key): + return {"segment_key": key} if key else {} diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index 119d9a3..d9b374e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -6,6 +6,7 @@ from dataclasses import dataclass, field import math from pathlib import PurePath from typing import Mapping +from .motion_path import MotionSegment ACQUISITION_POLICY_VERSION = "unified_engine_v8_all_view_images" @@ -180,6 +181,23 @@ class VisionRigSpec: return frozenset(tag.tag_id for view in self.views for tag in view.tags) +@dataclass(frozen=True) +class ModelPreparation: + """Independent parent-Tag image authorization at this task's holding pose.""" + + joint: str + command: tuple[float, ...] + approach_commands: tuple[tuple[float, ...], ...] + + +@dataclass(frozen=True) +class ParentReferenceSpec: + """Independent sources for a held Tag pose and its local hinge geometry.""" + + pose_joint: str + geometry_joint: str + + @dataclass(frozen=True) class TaskSpec: key: str @@ -198,6 +216,10 @@ class TaskSpec: formal_speed: float | None = None preparation_groups: tuple[tuple[int, ...], ...] = () entry_waypoints: tuple[tuple[tuple[int, float], ...], ...] = () + segments: tuple["MotionSegment", ...] = () + exit_waypoints: tuple[tuple[tuple[int, float], ...], ...] = () + model_preparations: tuple[ModelPreparation, ...] = () + parent_reference: ParentReferenceSpec | None = None @property def start_value(self) -> float: @@ -222,6 +244,7 @@ class JointZeroSpec: task_key: str command: tuple[float, ...] approach_commands: tuple[tuple[float, ...], ...] = () + evidence_scope: str = "arrival" @dataclass(frozen=True) @@ -277,6 +300,7 @@ class MeasurementPolicy: # Explicit motion reuse, independent of legacy zero/mimic transfer policy. # Recipients keep their own SDK channel and cannot become measured donors. transferred_motion_sources: Mapping[str, str] = field(default_factory=dict) + release_basis: str = "feedback_and_command" @dataclass(frozen=True) @@ -314,6 +338,9 @@ class QualityPolicy: hard_threshold_keys: frozenset[str] retry_metric_scope: Mapping[str, str] = field(default_factory=dict) isolated_holdout: bool = False + training_policy: str = "fixed" + # Runtime selections are journal-derived, never guessed from missing rows. + task_training_cycles: Mapping[str, tuple[int, ...]] = field(default_factory=dict) @dataclass(frozen=True) @@ -334,6 +361,7 @@ class AcquisitionPolicy: fixed_reference_minimum_frames: int = 10 fixed_reference_maximum_drift_px: float = 5.0 fixed_reference_confirmation_frames: int = 10 + fixed_reference_mode: str = "tag_pose" require_endpoint_observations: bool = False endpoint_tolerance_01: float = 2.0 / 255.0 steady_training_nodes: int = 9 @@ -343,6 +371,16 @@ class AcquisitionPolicy: steady_window_seconds: float = 0.2 steady_timeout_seconds: float = 2.0 joint_zero_timeout_seconds: float = 2.0 + motion_model_scope: str = "session" + # Native position bytes may have endpoint dead zones or a nonlinear servo + # response. Only a declared common scale can turn command travel into an + # expected encoder displacement before calibration has measured the map. + feedback_travel_matches_command: bool = True + motion_observation: str = "feedback" + visual_stale_seconds: float = 1.0 + visual_minimum_motion_px: float = 1.0 + visual_stability_px: float = 0.35 + visual_zero_tolerance_px: float = 1.0 def observation_streams(profile, task): @@ -354,11 +392,22 @@ def observation_streams(profile, task): yield "observation_joint", name, profile.measurement.measurements[secondary] +@dataclass(frozen=True) +class RetainedCadScope: + """Explicit partial capture with unmeasured, unchanged CAD ancestors.""" + + reason: str + root_anchor_joints: tuple[str, ...] + orientation_anchor_joint: str + directed_base_axis_joints: tuple[str, ...] = () + + @dataclass(frozen=True) class ScopePolicy: calibrate_joints: Mapping[str, frozenset[str]] frozen_joints: Mapping[str, frozenset[str]] default_scope: str = "full" + retained_cad_scopes: Mapping[str, RetainedCadScope] = field(default_factory=dict) def selected_joints(self, scope: str) -> frozenset[str]: try: @@ -418,6 +467,20 @@ class CalibrationProfile: joint_coverage: Mapping[str, str] = field(default_factory=dict) urdf_limit_policies: Mapping[str, JointLimitPolicy] = field(default_factory=dict) + @property + def retained_joints(self) -> frozenset[str]: + if self.scope.default_scope in self.scope.retained_cad_scopes: + return self.scope.frozen_joints[self.scope.default_scope] + return frozenset() + + @property + def command_based_release(self) -> bool: + return self.measurement.release_basis == "steady_command" + + @property + def vision_motion(self) -> bool: + return self.acquisition.motion_observation == "vision" + @property def curve_input_domain(self) -> str: return self.measurement.input_domain or f"feedback_{self.command.unit}" @@ -487,8 +550,23 @@ def _validate_observation_strategy(profile: CalibrationProfile, errors: list[str def validate_profile(profile: CalibrationProfile) -> None: """Hard-check all cross-policy references before hardware is enabled.""" errors: list[str] = [] + from .capture_plan import configure_training, select_task_training + try: + configure_training(profile, profile.quality.training_policy) + for key, cycles in profile.quality.task_training_cycles.items(): + select_task_training(profile, key, cycles) + except ValueError as error: + errors.append(str(error)) command = profile.command baseline = command.baseline_values + if profile.measurement.release_basis not in {"feedback_and_command", "steady_command"}: + errors.append("unsupported measurement release basis") + if profile.command_based_release and (profile.artifacts.output_schema_version != 3 + or profile.acquisition.command_capture_mode != "interleaved" + or profile.measurement.cross_view_sources + or profile.measurement.transferred_motion_sources + or profile.zero.passive_joints): + errors.append("steady command release requires independent active v3 joints and interleaved acquisition") if not command.names or len(command.names) != len(baseline): errors.append("command names and baseline must be non-empty and aligned") if len(set(command.names)) != len(command.names): @@ -630,7 +708,68 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"task {task.key} preparation groups must contain unique valid channels") if any(task.command_index in group for group in task.preparation_groups[:-1]): errors.append(f"task {task.key} measured channel must move after avoidance channels") - for waypoint in task.entry_waypoints: + segment_keys = set() + for segment in task.segments: + starts, ends = dict(segment.start_commands), dict(segment.end_commands) + if (not segment.key or segment.key in segment_keys or set(starts) != set(ends) + or len(starts) != len(segment.start_commands) or len(ends) != len(segment.end_commands) + or not starts or not set(starts) <= indices-command.disabled_indices): + errors.append(f"task {task.key} has an invalid segment identity or channels") + continue + segment_keys.add(segment.key) + if any(not math.isfinite(v) or not command.minimum_values[i] <= v <= command.maximum_values[i] + for i, v in (*segment.start_commands, *segment.end_commands)): + errors.append(f"task {task.key} segment commands are outside domain") + moving = {i for i in starts if starts[i] != ends[i]} + signs = {1 if ends[i] > starts[i] else -1 for i in moving} + if not moving or len(signs) != 1: + errors.append(f"task {task.key} segment must move channels in one native direction") + if (not segment.joints or not set(segment.joints) <= set(task.joints) + or any(_transfer_channel(profile, j) not in moving for j in segment.joints)): + errors.append(f"task {task.key} segment observations must follow moving channels") + if task.segments and segment_keys == {s.key for s in task.segments}: + if set().union(*(set(s.joints) for s in task.segments)) != set(task.joints): + errors.append(f"task {task.key} segments must cover every observed joint") + for previous, following in zip(task.segments, task.segments[1:]): + if dict(previous.end_commands) != dict(following.start_commands): + errors.append(f"task {task.key} segment path must be continuous") + if dict(task.segments[-1].end_commands) != dict(task.segments[0].start_commands): + errors.append(f"task {task.key} segment cycle must be closed") + if task.parent_reference is not None: + reference = task.parent_reference + earlier = {name for previous in profile.motion.tasks[:profile.motion.tasks.index(task)] + for name in previous.joints} + if (task.model_preparations or len(task.joints) != 1 + or profile.acquisition.motion_model_scope != "task" + or not {reference.pose_joint, reference.geometry_joint} <= earlier): + errors.append(f"task {task.key} parent reference requires earlier measured sources and task scope") + elif all(name in measurement_names for name in (*task.joints, reference.pose_joint, reference.geometry_joint)): + child = profile.measurement.measurements[task.joints[0]] + if any(profile.measurement.measurements[name].child_role != child.parent_role + or profile.measurement.measurements[name].view != child.view + for name in (reference.pose_joint, reference.geometry_joint)): + errors.append(f"task {task.key} parent reference must identify the same physical parent Tag") + for preparation in task.model_preparations: + if profile.acquisition.motion_model_scope != "task" or preparation.joint not in measurement_names: + errors.append(f"task {task.key} image preparation needs a measured joint and task model scope") + continue + spec = profile.measurement.measurements[preparation.joint] + roots = {profile.measurement.measurements[j].parent_role for j in task.joints if j in measurement_names} + if spec.child_role not in roots or preparation.joint in task.joints or not preparation.approach_commands: + errors.append(f"task {task.key} image preparation must authorize a parent Tag") + for pose in (*preparation.approach_commands, preparation.command): + if len(pose) != command.command_count or any(not math.isfinite(v) or not lo <= v <= hi + for v, lo, hi in zip(pose, command.minimum_values, command.maximum_values)): + errors.append(f"task {task.key} image preparation command outside domain") + if preparation.command not in {spec.command for spec in profile.motion.joint_zero_references.values() + if spec.task_key == task.key}: + errors.append(f"task {task.key} image preparation must finish at its declared zero pose") + channel = _transfer_channel(profile, preparation.joint) + if any(i != channel and v != preparation.command[i] + for pose in preparation.approach_commands for i, v in enumerate(pose) + if i < len(preparation.command)): + errors.append(f"task {task.key} image preparation must move only its observed parent joint") + for waypoint in (*task.entry_waypoints, *task.exit_waypoints): if not waypoint or len({i for i, _ in waypoint}) != len(waypoint) or any( i not in indices or i in command.disabled_indices or not math.isfinite(v) or not command.minimum_values[i] <= v <= command.maximum_values[i] for i, v in waypoint): @@ -660,6 +799,18 @@ def validate_profile(profile: CalibrationProfile) -> None: zero = profile.zero for name, reference in profile.motion.joint_zero_references.items(): + if reference.evidence_scope not in {"arrival", "path"}: + errors.append(f"invalid zero approach evidence scope:{name}") + if reference.evidence_scope == "path": + if len(reference.approach_commands) < 2: + errors.append(f"path evidence needs at least two declared approach poses:{name}") + elif all(len(pose) == command.command_count + for pose in (reference.command, *reference.approach_commands)): + moving = {i for i, value in enumerate(reference.approach_commands[-1]) + if value != reference.command[i]} + if any(value != reference.command[i] for pose in reference.approach_commands + for i, value in enumerate(pose) if i not in moving): + errors.append(f"path evidence must preserve the final arrival holding pose:{name}") task = next((t for t in profile.motion.tasks if t.key == reference.task_key), None) if task is None or name not in task.joints: errors.append(f"zero reference must precede an observing task:{name}") @@ -671,11 +822,15 @@ def validate_profile(profile: CalibrationProfile) -> None: elif any(pose[i] != command.baseline_values[i] for i in command.disabled_indices): errors.append(f"zero reference moves a disabled channel:{name}") if len(reference.command) == command.command_count: - if reference.command[task.command_index] != command.baseline_values[task.command_index]: + channel = _transfer_channel(profile, name) + if channel is None: + errors.append(f"zero reference has no motor channel:{name}") + continue + if reference.command[channel] != command.baseline_values[channel]: errors.append(f"zero reference must use joint baseline, not task start:{name}") if profile.artifacts.output_schema_version >= 3 and (not reference.approach_commands or len(reference.approach_commands[-1]) != command.command_count - or reference.approach_commands[-1][task.command_index] == reference.command[task.command_index]): + or reference.approach_commands[-1][channel] == reference.command[channel]): errors.append(f"zero reference requires an explicit nonzero final arrival:{name}") first_task = next((t.key for t in profile.motion.tasks if name in t.joints), None) if first_task != reference.task_key: @@ -748,6 +903,22 @@ def validate_profile(profile: CalibrationProfile) -> None: frozen = profile.scope.frozen_joints[name] if selected & frozen or selected | frozen != zero.active_joints: errors.append(f"scope {name} must partition all active joints") + for name, policy in profile.scope.retained_cad_scopes.items(): + if (name not in scopes or name == "full" or not policy.reason.strip() + or not profile.scope.frozen_joints.get(name)): + errors.append(f"retained CAD scope must explicitly name excluded active joints:{name}") + anchors = set(policy.root_anchor_joints) | {policy.orientation_anchor_joint} + if (not policy.root_anchor_joints or not anchors <= profile.scope.calibrate_joints.get(name, ()) + or not set(policy.directed_base_axis_joints) <= anchors): + errors.append(f"partial base pose must use declared measured anchors:{name}") + retained = profile.retained_joints + if retained: + measured = {joint for task in profile.motion.tasks for joint in task.joints} + if (not profile.command_based_release or measured != zero.active_joints - retained + or retained & (set(profile.measurement.measurements) | set(profile.urdf_authorized_fields) + | set(profile.urdf_limit_policies) | set(zero.direct_zero_joints) + | set(zero.cad_zero_assumptions) | set(zero.known_baseline_geometry))): + errors.append("partial capture must preserve excluded CAD joints without claiming measurements or zeros") artifacts = profile.artifacts if artifacts.output_schema_version < 1: @@ -758,10 +929,6 @@ def validate_profile(profile: CalibrationProfile) -> None: for target, source in profile.measurement.transferred_motion_sources.items(): if (target in artifacts.directional_command_joints) != (source in artifacts.directional_command_joints): errors.append("transferred command curves must retain their source direction policy") - for task in profile.motion.tasks: - if artifacts.directional_command_joints.intersection(task.joints): - if profile.command.baseline_values[task.command_index] not in (task.start_value, task.end_value): - errors.append("directional command mapping requires an endpoint baseline") for label, filename in ( ("calibration", artifacts.calibration_filename), ("corrected URDF", artifacts.corrected_urdf_filename), @@ -797,6 +964,23 @@ def validate_profile(profile: CalibrationProfile) -> None: or bounds[0] >= bounds[1] or not policy.safety_evidence_source.strip()): errors.append(f"URDF independent safety bounds require finite limits and evidence:{name}") acquisition = profile.acquisition + if acquisition.motion_observation not in {"feedback", "vision"}: + errors.append("motion observation must be feedback or vision") + if profile.vision_motion and not profile.command_based_release: + errors.append("visual motion requires independent steady-command release") + for key in ("visual_stale_seconds", "visual_minimum_motion_px", "visual_stability_px", "visual_zero_tolerance_px"): + if not math.isfinite(getattr(acquisition, key)) or getattr(acquisition, key) <= 0: + errors.append(f"visual motion threshold must be finite and positive:{key}") + if acquisition.visual_minimum_motion_px <= 2*acquisition.visual_stability_px: + errors.append("visual motion witness must exceed twice the stability noise") + if not isinstance(acquisition.feedback_travel_matches_command, bool): + errors.append("feedback travel correspondence must be a boolean") + if acquisition.motion_model_scope not in {"session", "task"}: + errors.append("motion model scope must be session or task") + if acquisition.fixed_reference_mode not in {"tag_pose", "stationary_image"}: + errors.append("fixed reference mode must be tag_pose or stationary_image") + if acquisition.fixed_reference_mode == "stationary_image" and profile.artifacts.output_schema_version < 3: + errors.append("stationary image references require version 3 measured geometry") if acquisition.command_capture_mode not in {"interleaved", "separate"}: errors.append("unsupported command capture mode") task_by_key = {task.key: task for task in profile.motion.tasks} diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py index 0e36fb4..b514c8a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py @@ -43,7 +43,7 @@ class JointZeroSample: return cls(f"{row['view']}:{int(row['image_stamp_ns'])}", row["view"], int(row["image_stamp_ns"]), tuple(row["relative_quaternion_xyzw"]), tuple(row["relative_translation_xyz_m"]), tuple(row[f"command_vector_{unit}"]), - tuple(row[f"state_{unit}"]), tuple(row.get("command_direction_by_index", ())), + tuple(row.get(f"state_{unit}") or ()), tuple(row.get("command_direction_by_index", ())), JointZeroPose.from_mapping(row["parent_pose_common"]), JointZeroPose.from_mapping(row["child_pose_common"])) @@ -155,6 +155,8 @@ def _validate_branch_bindings(profile, reference): def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, session_epoch, motion_version): spec = profile.motion.joint_zero_references[joint] task = next(t for t in profile.motion.tasks if t.key == spec.task_key) + from ..fitting.motion_fit import channel_for_joint + channel = channel_for_joint(profile, joint) views = {profile.measurement.measurements[joint].view} secondary = profile.measurement.cross_view_sources.get(joint) if secondary: @@ -194,17 +196,17 @@ def build_joint_zero_reference(profile, joint, rows: Sequence[Mapping], *, sessi for sample in samples: if sample.view not in views or sample.command != spec.command: raise ValueError(f"joint_zero_pose_differs:{joint}") - if len(sample.feedback) != profile.command.command_count or not np.all(np.isfinite(sample.feedback)): + if not profile.vision_motion and (len(sample.feedback) != profile.command.command_count or not np.all(np.isfinite(sample.feedback))): raise ValueError(f"joint_zero_feedback_invalid:{joint}") if len(sample.arrival_directions) != profile.command.command_count: raise ValueError(f"joint_zero_arrival_missing:{joint}") if spec.approach_commands: - before = spec.approach_commands[-1][task.command_index] - expected = "increasing" if spec.command[task.command_index] > before else "decreasing" - if sample.arrival_directions[task.command_index] != expected: + before = spec.approach_commands[-1][channel] + expected = "increasing" if spec.command[channel] > before else "decreasing" + if sample.arrival_directions[channel] != expected: raise ValueError(f"joint_zero_arrival_changed:{joint}") - return JointZeroReference(joint, spec.task_key, profile.command.baseline_values[task.command_index], - task.command_index, session_epoch, motion_version, samples, _branch_references(profile, joint, rows)) + return JointZeroReference(joint, spec.task_key, profile.command.baseline_values[channel], + channel, session_epoch, motion_version, samples, _branch_references(profile, joint, rows)) def read_joint_zero_references(profile, records): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py index e478c1a..2689b59 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py @@ -18,6 +18,9 @@ if TYPE_CHECKING: class CalibrationResult: """Training-frozen measurements and parameters consumed by every writer. + output_mappings is the primary geometry/replay domain, explicitly labeled + by each JointMapping. Optional feedback diagnostics never supply geometry. + This is not a publication certificate: the final serialized URDF must still pass independent spatial replay and structural authorization. """ @@ -36,6 +39,7 @@ class CalibrationResult: command_applicability: Mapping = field(default_factory=dict) joint_zero_references: Mapping = field(default_factory=dict) mimic_approximations: Mapping = field(default_factory=dict) + feedback_diagnostics: Mapping = field(default_factory=dict) @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py index a4be453..a2ddcbf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py @@ -3,12 +3,14 @@ import numpy as np -def command_training_cycles(profile): - return (4,) if profile.acquisition.command_capture_mode == "separate" else (0, 1, 2) +def command_training_cycles(profile, task=None, *, joint=None): + from .capture_plan import task_partition + return task_partition(profile, task, joint=joint).command_training def command_holdout_cycle(profile): - return 5 if profile.acquisition.command_capture_mode == "separate" else 3 + from .capture_plan import task_partition + return task_partition(profile).command_holdout def command_nodes(profile, task, *, holdout=False): @@ -16,10 +18,14 @@ def command_nodes(profile, task, *, holdout=False): train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes) train = np.unique(np.r_[train, profile.acquisition.steady_extra_training_nodes.get(task.key, ())]) if profile.artifacts.output_schema_version >= 3: - baseline = profile.command.baseline_values[task.command_index] - if not lo <= baseline <= hi: + from .motion_path import task_channels + baselines = [profile.command.baseline_values[i] for i in task_channels(task)] + if any(not lo <= baseline <= hi for baseline in baselines): raise ValueError(f"baseline_outside_calibrated_support:{task.key}") - train = np.unique(np.r_[train, baseline]) + train = np.unique(np.r_[train, baselines]) + if task.segments: + boundaries = [v for s in task.segments for i, v in (*s.start_commands, *s.end_commands)] + train = np.unique(np.r_[train, boundaries]) values = np.r_[lo, (train[:-1]+train[1:])/2, hi] if holdout else train if profile.command.unit == "u8": values = np.floor(values+.5) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py index 18e553b..99e80d7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py @@ -4,7 +4,7 @@ Feedback is NOT a command label and is never the target of this regression. The visual reference/axis was frozen by the continuous training acquisition. """ -from dataclasses import replace +from dataclasses import asdict, replace import math import numpy as np @@ -13,38 +13,50 @@ from ..urdf.acceptance import angular_metrics from .curve import isotonic_nonincreasing from .spatial import measure_joint_curve_observation from ..domain.sampling import command_nodes, command_training_cycles, command_holdout_cycle +from ..domain.motion_path import task_channels def fit_command_mappings(profile, fit, records_by_joint): + mappings, metrics, applicability = fit_command_observations(profile, fit.curves, records_by_joint, + channels={name: mapping.motor_index for name, mapping in fit.output_mappings.items()}, + reference_inputs=fit.reference_inputs, zero_references=fit.joint_zero_references) + return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics, + command_applicability=applicability) + + +def fit_command_training(profile, curves, records_by_joint, *, channels, reference_inputs, zero_references): + """Fit stable command labels to angles measured in frozen visual frames.""" mappings, metrics, applicability = {}, {}, {} - training_cycles = command_training_cycles(profile) holdout_cycle = command_holdout_cycle(profile) - for joint in sorted(fit.output_mappings): + for joint in sorted(channels): donor = joint if profile.artifacts.output_schema_version >= 3 else profile.zero.transferred_zero_sources.get(joint, joint) rows = records_by_joint.get(donor, ()) if not rows or any(r.get("sample_phase") != "steady" for r in rows): raise ValueError(f"steady_command_observations_missing:{joint}") - curve = fit.curves[donor] - gauge = float(np.interp(fit.reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad)) + training_cycles = command_training_cycles(profile, joint=donor) + if any(r["cycle"] not in training_cycles for r in rows): + raise ValueError("command_training_received_nontraining_data") + curve = curves[donor] + gauge = 0.0 # Byte curves already carry their shared coordinate reference. Native # curves retain the arbitrary observed Tag reference in fit.curves. - if profile.command.unit == "u8" or profile.artifacts.output_schema_version >= 3: - gauge = 0.0 + if profile.command.unit != "u8" and profile.artifacts.output_schema_version < 3: + gauge = float(np.interp(reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad)) observed = {r["sample_id"]: measure_joint_curve_observation(curve, quaternion_xyzw=r.get("relative_quaternion_xyzw"), image_relative_xy_px=r.get("image_relative_xy_px"))-gauge for r in rows} if len(observed) != len(rows): raise ValueError("duplicate steady visual image") - channel = fit.output_mappings[joint].motor_index - source_channel = fit.output_mappings[donor].motor_index + channel = channels[joint] + source_channel = channels[donor] branches = {} + maximum_correction = 0.0 directional = joint in profile.artifacts.directional_command_joints task = next(t for t in profile.motion.tasks if donor in t.joints) - lo, hi = sorted((task.start_value, task.end_value)) from .command_sampling import declared_training_nodes knots = declared_training_nodes(profile, task, rows) sign = profile.command.joint_directions[source_channel] - if profile.artifacts.output_schema_version >= 3: + if profile.artifacts.output_schema_version >= 3 and not profile.command_based_release: sign = 1 if curve.angle_rad[-1] > curve.angle_rad[0] else -1 for direction in ("increasing", "decreasing"): values = [] @@ -62,57 +74,95 @@ def fit_command_mappings(profile, fit, records_by_joint): values.append(float(np.median(medians))) values = np.asarray(values) projected = -sign*isotonic_nonincreasing(-sign*values) - if np.max(np.abs(projected-values)) > math.radians(1): + maximum_correction = max(maximum_correction, float(np.max(np.abs(projected-values)))) + if maximum_correction > math.radians(1): raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}") branches[direction] = tuple(float(v) for v in projected) if directional: from .anchored_mapping import anchored_mean branches = {direction: anchored_mean(knots, values, values, - fit.joint_zero_references[joint].baseline_command, sign) + zero_references[joint].baseline_command, sign) for direction, values in branches.items()} mean = tuple((a+b)/2 for a, b in zip(branches["increasing"], branches["decreasing"])) if profile.artifacts.output_schema_version >= 3: from .anchored_mapping import anchored_mean mean = anchored_mean(knots, branches["increasing"], branches["decreasing"], - fit.joint_zero_references[joint].baseline_command, sign) + zero_references[joint].baseline_command, sign) mapping = JointMapping(joint, channel, f"command_{profile.command.unit}", knots, mean, branches["increasing"], branches["decreasing"], donor if donor != joint else None) - validation = [r for r in rows if r["cycle"] == holdout_cycle] - expected = command_nodes(profile, task, holdout=True) - for direction in ("increasing", "decreasing"): - for knot in set(expected): - if sum(r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9) - for r in validation) < profile.acquisition.steady_minimum_samples: - raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}") - errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]), - r["direction"] if directional or profile.artifacts.output_schema_version < 3 else "") - - observed[r["sample_id"]] for r in validation] - quality = angular_metrics(errors) - if not quality.passed: - raise ValueError(f"steady_command_holdout_failed:{joint}:{quality}") mappings[joint] = mapping - from dataclasses import asdict - metrics[joint] = {**asdict(quality), "independently_measured": donor == joint, - "source_joint": donor, "training_cycles": list(training_cycles), "holdout_cycle": holdout_cycle, - "training_sample_ids": sorted(r["sample_id"] for r in rows if r["cycle"] in training_cycles)} - if profile.artifacts.output_schema_version >= 3: - branch_metrics = {direction: angular_metrics([error for error, row in zip(errors, validation) - if row["direction"] == direction]) for direction in ("increasing", "decreasing")} - if not all(value.passed for value in branch_metrics.values()): - raise ValueError(f"command_curve_direction_holdout_failed:{joint}") - metrics[joint]["by_direction"] = {key: asdict(value) for key, value in branch_metrics.items()} + metrics[joint] = {"maximum_monotonic_correction_rad": maximum_correction, + "independently_measured": donor == joint, "source_joint": donor, + "training_cycles": list(training_cycles), "holdout_cycle": holdout_cycle, + "training_sample_ids": sorted(r["sample_id"] for r in rows)} training = [r for r in rows if r["cycle"] in training_cycles] vectors = np.asarray([r[f"command_vector_{profile.command.unit}"] for r in training]) held = {str(i): float(np.median(vectors[:, i])) for i in range(profile.command.command_count) - if i != source_channel} + if i not in task_channels(task)} for i, value in held.items(): if np.max(np.abs(vectors[:, int(i)]-value)) > 1e-8: raise ValueError(f"scan_changed_multiple_command_channels:{donor}:{i}") applicability[joint] = {"task": task.key, "held_command_values": held, - "scope": "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False, - "branch_initialization": "full_range_endpoint_reset"} + "scope": "declared_multi_channel_path" if task.segments else "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False, + "branch_initialization": "recorded_baseline_then_endpoint_resets" if task.segments else "full_range_endpoint_reset"} + if task.segments: + applicability[joint]["motion_segments"] = [asdict(s) for s in task.segments] if directional: applicability[joint].update(command_mapping="directional", direction_source="raw_sdk_input", interior_reversals_validated=False) - return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics, - command_applicability=applicability) + return mappings, metrics, applicability + + +def validate_command_training(profile, curves, mappings, metrics, records_by_joint, *, reference_inputs): + """Evaluate independent images against already frozen frames and mappings.""" + holdout_cycle = command_holdout_cycle(profile) + accepted = {} + for joint, mapping in mappings.items(): + donor = metrics[joint]["source_joint"] + curve = curves[donor] + rows = records_by_joint.get(donor, ()) + if not rows or any(r["cycle"] != holdout_cycle or r.get("sample_phase") != "steady" for r in rows): + raise ValueError(f"steady_holdout_incomplete:{joint}") + ids = [r["sample_id"] for r in rows] + trained = set(metrics[joint]["training_sample_ids"]) | set(curve.circle.get("training_sample_ids", ())) + if len(ids) != len(set(ids)) or trained.intersection(ids): + raise ValueError("command_holdout_image_identity_overlaps_training") + task = next(t for t in profile.motion.tasks if donor in t.joints) + for direction in ("increasing", "decreasing"): + for knot in command_nodes(profile, task, holdout=True): + if sum(r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9) + for r in rows) < profile.acquisition.steady_minimum_samples: + raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}") + gauge = (float(np.interp(reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad)) + if profile.command.unit != "u8" and profile.artifacts.output_schema_version < 3 else 0.) + directional = joint in profile.artifacts.directional_command_joints + errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]), + r["direction"] if directional or profile.artifacts.output_schema_version < 3 else "") + - measure_joint_curve_observation(curve, quaternion_xyzw=r.get("relative_quaternion_xyzw"), + image_relative_xy_px=r.get("image_relative_xy_px")) + gauge for r in rows] + quality = angular_metrics(errors) + if not quality.passed: + raise ValueError(f"steady_command_holdout_failed:{joint}:{quality}") + accepted[joint] = {**metrics[joint], **asdict(quality)} + if profile.artifacts.output_schema_version >= 3: + branches = {direction: angular_metrics([error for error, row in zip(errors, rows) + if row["direction"] == direction]) for direction in ("increasing", "decreasing")} + if not all(value.passed for value in branches.values()): + raise ValueError(f"command_curve_direction_holdout_failed:{joint}") + accepted[joint]["by_direction"] = {key: asdict(value) for key, value in branches.items()} + return accepted + + +def fit_command_observations(profile, curves, records_by_joint, *, channels, reference_inputs, zero_references): + training, validation = {}, {} + for name, rows in records_by_joint.items(): + cycles = command_training_cycles(profile, joint=name) + holdout = command_holdout_cycle(profile) + if any(row["cycle"] not in (*cycles, holdout) for row in rows): + raise ValueError("command_observation_outside_capture_plan") + training[name] = [row for row in rows if row["cycle"] in cycles] + validation[name] = [row for row in rows if row["cycle"] == holdout] + mappings, metrics, applicability = fit_command_training(profile, curves, training, + channels=channels, reference_inputs=reference_inputs, zero_references=zero_references) + return mappings, validate_command_training(profile, curves, mappings, metrics, validation, + reference_inputs=reference_inputs), applicability diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py new file mode 100644 index 0000000..5e2a538 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_motion.py @@ -0,0 +1,114 @@ +"""Command-certified motion with an independent, non-blocking feedback diagnostic. + +Moving images determine only a hinge frame. Stable commanded poses train the +control curves. A fourth cycle never contributes to either training stage. +""" + +from dataclasses import asdict, dataclass, replace +import numpy as np + +from ..domain.measurement import JointCurveFit +from ..urdf.acceptance import angular_metrics +from .command_mapping import fit_command_training as train_commands, validate_command_training +from ..domain.capture_plan import training_cycles, LEGACY_TRAINING, LEGACY_HOLDOUT +from ..domain.sampling import command_training_cycles, command_holdout_cycle +from .motion_fit import channel_for_joint, joint_input_direction +from .observed_motion import image_identity +from .rotation_curve import RotationObservation, fit_rotation_frame, fit_rotation_curve + + +def rotation_observations(rows, domain): + return [RotationObservation(image_identity(row), float(row[domain]), + tuple(row['relative_quaternion_xyzw']), int(row['cycle']), str(row['direction'])) for row in rows] + + +def feedback_diagnostic(rows, *, domain, sign, reference, cycles=LEGACY_TRAINING, holdout_cycle=LEGACY_HOLDOUT): + result = {'required_for_release': False, 'input_domain': domain, 'training_cycles': list(cycles), + 'holdout_cycle': holdout_cycle, 'extrapolation': 'reject'} + try: + if any(row.get(domain) is None or not np.isfinite(row[domain]) for row in rows): + raise ValueError('position_feedback_missing_or_invalid') + observations = rotation_observations(rows, domain) + curve = fit_rotation_curve([r for r in observations if r.cycle in cycles], + input_domain=domain, sdk_to_joint_direction=sign, reference_xyzw=reference, + training_cycles=cycles, holdout_cycle=holdout_cycle) + held = [r for r in observations if r.cycle == holdout_cycle] + supported = [r for r in held if curve.knots[0] <= r.input_value <= curve.knots[-1]] + outside = [r for r in held if not curve.knots[0] <= r.input_value <= curve.knots[-1]] + quality = angular_metrics(curve.holdout_errors(supported)) if supported else None + metrics = {**asdict(quality), 'passed': quality.passed} if quality else None + result.update(status='passed' if held and not outside and metrics and metrics['passed'] else 'incomplete_or_inaccurate', + training_support=[curve.knots[0], curve.knots[-1]], holdout_samples=len(held), + outside_training_support=len(outside), outside_image_ids=[r.sample_id for r in outside], + supported_holdout_metrics=metrics, mapping={**asdict(curve), + "training_sample_ids": sorted(curve.training_sample_ids)}) + except (ValueError, TypeError) as error: + result.update(status='unavailable', reason=str(error)) + return result + + +@dataclass(frozen=True) +class CommandMotionFit: + observed_curves: dict + coordinate_curves: dict + mappings: dict + command_metrics: dict + applicability: dict + reference_inputs: dict + feedback_diagnostics: dict + + @property + def holdout_errors_rad(self): + # Command errors are retained with their per-direction metrics. + return {} + + @property + def cross_view_metrics(self): + return {} + + +def fit_command_training(profile, source_model, sweep, steady, references): + """Training-only motion API. Passing a holdout image is a contract error.""" + if set(sweep) != set(steady) or set(sweep) != set(references): + raise ValueError('command_motion_observation_coverage_differs') + frames = {} + channels = {name: channel_for_joint(profile, name) for name in sweep} + for name, rows in sweep.items(): + cycles = training_cycles(profile, joint=name) + if any(row['cycle'] not in cycles for row in rows): + raise ValueError('motion_training_received_nontraining_data') + view = profile.measurement.measurements[name].view + frames[name] = fit_rotation_frame(rotation_observations(rows, f'command_{profile.command.unit}'), + sdk_to_joint_direction=joint_input_direction(profile, source_model, name), + reference_xyzw=references[name].quaternion(view), training_cycles=cycles) + inputs = {name: references[name].baseline_command for name in frames} + mappings, metrics, applicability = train_commands(profile, frames, steady, + channels=channels, reference_inputs=inputs, zero_references=references) + curves = {} + for name, mapping in mappings.items(): + curves[name] = JointCurveFit(mapping.angle_rad, mapping.decreasing_rad, mapping.increasing_rad, + {**frames[name].circle, 'input_domain': mapping.input_domain, 'input_knots': mapping.knots, + 'mapping_training_sample_ids': metrics[name]['training_sample_ids']}, + metrics[name]["maximum_monotonic_correction_rad"], + float(np.max(np.abs(np.array(mapping.increasing_rad)-mapping.decreasing_rad))), {}) + return CommandMotionFit(curves, curves, mappings, metrics, applicability, inputs, {}) + + +def fit_command_motion(profile, source_model, sweep, steady, references): + train_sweep = {name: [r for r in rows if r['cycle'] in training_cycles(profile, joint=name)] + for name, rows in sweep.items()} + train_steady = {name: [r for r in rows if r['cycle'] in command_training_cycles(profile, joint=name)] + for name, rows in steady.items()} + motion = fit_command_training(profile, source_model, train_sweep, train_steady, references) + held = {name: [r for r in rows if r['cycle'] == command_holdout_cycle(profile)] + for name, rows in steady.items()} + metrics = validate_command_training(profile, motion.coordinate_curves, + motion.mappings, motion.command_metrics, held, reference_inputs=motion.reference_inputs) + diagnostics = {} + for name, rows in sweep.items(): + view = profile.measurement.measurements[name].view + diagnostics[name] = feedback_diagnostic(rows, + domain=f'feedback_{profile.command.unit}', sign=joint_input_direction(profile, source_model, name), + reference=references[name].quaternion(view), cycles=training_cycles(profile, joint=name), + holdout_cycle=profile.quality.holdout_cycle) + return replace(motion, command_metrics=metrics, feedback_diagnostics=diagnostics) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py index 3b8fb49..77165c8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_sampling.py @@ -14,6 +14,8 @@ import numpy as np from ..domain.sampling import command_nodes from ..domain.profile import observation_streams from .rotation_curve import RotationObservation, fit_rotation_curve +from ..domain.motion_path import record_scan_identity, segment_metadata +from ..domain.capture_plan import training_cycles SAMPLING_POLICY = "training_shape_static_knots_v1" @@ -25,24 +27,26 @@ def digest(value): def training_sources(profile, task, records): + cycles = training_cycles(profile, task) streams = {(field, name, spec.view) for field, name, spec in observation_streams(profile, task)} latest = {} for row in records: - if row.get("task_name") == task.key and row.get("cycle") in {0, 1, 2} and row.get("direction"): - key = (row["cycle"], row["direction"]) + if row.get("task_name") == task.key and row.get("cycle") in cycles and row.get("direction"): + key = record_scan_identity(row) latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) result = [] for row in records: - if (row.get("task_name") != task.key or row.get("cycle") not in {0, 1, 2} + if (row.get("task_name") != task.key or row.get("cycle") not in cycles or row.get("sample_phase", "sweep") != "sweep" or "relative_quaternion_xyzw" not in row): continue - if int(row.get("attempt", 1)) != latest.get((row["cycle"], row["direction"])): + if int(row.get("attempt", 1)) != latest.get(record_scan_identity(row)): continue for field, name, view in sorted(streams): if row.get(field) == name and row.get("view") == view: result.append({"field": field, "joint": name, "view": view, "cycle": row["cycle"], "direction": row["direction"], "image_stamp_ns": row["image_stamp_ns"], - "input": float(row[f"feedback_{profile.command.unit}"]), + **segment_metadata(row.get("segment_key", "")), + "input": float(row[f"{'command' if profile.command_based_release else 'feedback'}_{profile.command.unit}"]), "quaternion": list(row["relative_quaternion_xyzw"])}) return sorted(result, key=lambda row: (row["field"], row["joint"], row["view"], row["image_stamp_ns"])) @@ -69,28 +73,32 @@ def build_sampling_plan(profile, task, records): grid = command_nodes(profile, task) knots, reason = grid, "full_grid_insufficient_training_shape" curves = [] - try: - for field, name, spec in observation_streams(profile, task): - rows = [row for row in sources if (row["field"], row["joint"], row["view"]) == (field, name, spec.view)] - observations = [RotationObservation(f"{row['view']}:{row['image_stamp_ns']}", row["input"], - tuple(row["quaternion"]), row["cycle"], row["direction"]) for row in rows] - fitted = fit_rotation_curve(observations, input_domain=f"feedback_{profile.command.unit}", - sdk_to_joint_direction=profile.command.joint_directions[task.command_index]) - if fitted.maximum_monotonic_correction_rad > MAXIMUM_REMOVAL_ERROR_RAD: - raise ValueError("training_shape_noisy") - for branch in (fitted.increasing_rad, fitted.decreasing_rad): - curves.append(np.interp(grid, fitted.knots, branch)) - mandatory_values = {grid[0], grid[-1], grid[len(grid)//2], - profile.command.baseline_values[task.command_index], - *profile.acquisition.steady_extra_training_nodes.get(task.key, ())} - mandatory = {i for i, value in enumerate(grid) if value in mandatory_values} - knots = reduced_knots(grid, curves, mandatory) - reason = "training_shape_bounded" if len(knots) < len(grid) else "full_grid_required_by_training_shape" - except (ValueError, KeyError, TypeError, np.linalg.LinAlgError) as error: - reason = "full_grid:" + str(error) + if task.segments: + reason = "segmented_motion_preserves_full_per_joint_grid" + else: + try: + for field, name, spec in observation_streams(profile, task): + rows = [row for row in sources if (row["field"], row["joint"], row["view"]) == (field, name, spec.view)] + observations = [RotationObservation(f"{row['view']}:{row['image_stamp_ns']}", row["input"], + tuple(row["quaternion"]), row["cycle"], row["direction"]) for row in rows] + fitted = fit_rotation_curve(observations, input_domain=f"{'command' if profile.command_based_release else 'feedback'}_{profile.command.unit}", + sdk_to_joint_direction=profile.command.joint_directions[task.command_index], + training_cycles=training_cycles(profile, task), holdout_cycle=profile.quality.holdout_cycle) + if fitted.maximum_monotonic_correction_rad > MAXIMUM_REMOVAL_ERROR_RAD: + raise ValueError("training_shape_noisy") + for branch in (fitted.increasing_rad, fitted.decreasing_rad): + curves.append(np.interp(grid, fitted.knots, branch)) + mandatory_values = {grid[0], grid[-1], grid[len(grid)//2], + profile.command.baseline_values[task.command_index], + *profile.acquisition.steady_extra_training_nodes.get(task.key, ())} + mandatory = {i for i, value in enumerate(grid) if value in mandatory_values} + knots = reduced_knots(grid, curves, mandatory) + reason = "training_shape_bounded" if len(knots) < len(grid) else "full_grid_required_by_training_shape" + except (ValueError, KeyError, TypeError, np.linalg.LinAlgError) as error: + reason = "full_grid:" + str(error) plan = {"kind": "command_sampling_plan", "policy": SAMPLING_POLICY, "task_name": task.key, "training_nodes": list(knots), "validation_nodes": list(command_nodes(profile, task, holdout=True)), - "source_sha256": digest(sources), "source_count": len(sources), "training_cycles": [0, 1, 2], + "source_sha256": digest(sources), "source_count": len(sources), "training_cycles": list(training_cycles(profile, task)), "maximum_removal_error_rad": MAXIMUM_REMOVAL_ERROR_RAD, "reason": reason} return {**plan, "plan_sha256": digest(plan)} diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py index 1f850cf..775cf33 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py @@ -15,6 +15,7 @@ import numpy as np from scipy.spatial.transform import Rotation from .curve import isotonic_nonincreasing +from ..domain.capture_plan import LEGACY_TRAINING, LEGACY_HOLDOUT @dataclass(frozen=True) @@ -37,6 +38,8 @@ class RotationCurve: sdk_to_joint_direction: int training_sample_ids: frozenset[str] maximum_monotonic_correction_rad: float = 0.0 + training_cycles: tuple[int, ...] = LEGACY_TRAINING + holdout_cycle: int = LEGACY_HOLDOUT @property def angle_rad(self) -> tuple[float, ...]: @@ -59,7 +62,7 @@ class RotationCurve: return angle, off_axis_error def holdout_errors(self, observations: Sequence[RotationObservation]) -> tuple[float, ...]: - _validate_observations(observations, cycles={3}) + _validate_observations(observations, cycles={self.holdout_cycle}) if any(row.sample_id in self.training_sample_ids for row in observations): raise ValueError("holdout image identity overlaps training") reference = Rotation.from_quat(self.reference_xyzw) @@ -78,6 +81,25 @@ def _rotation(quaternion: Sequence[float]) -> Rotation: return Rotation.from_quat(values) +def common_directional_input_support(values, directions) -> tuple[float, float]: + """Training-only support shared by fitting and live acquisition checks.""" + inputs = np.asarray(values, dtype=float) + if inputs.ndim != 1 or len(inputs) != len(directions) or not np.all(np.isfinite(inputs)): + raise ValueError("directional SDK inputs must be finite matching vectors") + if any(direction not in {"increasing", "decreasing"} for direction in directions): + raise ValueError("invalid SDK support direction") + supports = [] + for direction in ("increasing", "decreasing"): + selected = inputs[np.asarray([value == direction for value in directions], dtype=bool)] + if not selected.size: + raise ValueError("both directional SDK supports are required") + supports.append((float(np.min(selected)), float(np.max(selected)))) + lower, upper = max(s[0] for s in supports), min(s[1] for s in supports) + if upper <= lower: + raise ValueError("directional SDK supports do not overlap") + return lower, upper + + def _validate_observations(rows: Sequence[RotationObservation], *, cycles: set[int]) -> None: if not rows: raise ValueError("rotation observations are missing") @@ -96,6 +118,8 @@ def fit_rotation_curve( observations: Sequence[RotationObservation], *, input_domain: str, sdk_to_joint_direction: int, knot_count: int = 65, reference_xyzw: Sequence[float] | None = None, + training_cycles: tuple[int, ...] = LEGACY_TRAINING, + holdout_cycle: int = LEGACY_HOLDOUT, ) -> RotationCurve: """Fit three complete training cycles without integerizing physical inputs. @@ -107,17 +131,69 @@ def fit_rotation_curve( raise ValueError("an explicit native SDK input domain is required") if sdk_to_joint_direction not in {-1, 1} or knot_count < 32: raise ValueError("invalid direction or curve knot count") - _validate_observations(observations, cycles={0, 1, 2}) - required = {(cycle, direction) for cycle in range(3) for direction in ("increasing", "decreasing")} + if len(training_cycles) < 2 or holdout_cycle in training_cycles: + raise ValueError("at least two independent training cycles and an isolated holdout are required") + _validate_observations(observations, cycles=set(training_cycles)) + required = {(cycle, direction) for cycle in training_cycles for direction in ("increasing", "decreasing")} groups = {(row.cycle, row.direction) for row in observations} if groups != required: - raise ValueError("three complete bidirectional training cycles are required") + raise ValueError("complete declared bidirectional training cycles are required") for cycle, direction in required: if sum(row.cycle == cycle and row.direction == direction for row in observations) < 40: raise ValueError("each training direction requires 40 unique observations") inputs = np.asarray([row.input_value for row in observations]) if input_domain.endswith("u8") and (np.min(inputs) < 0 or np.max(inputs) > 255): raise ValueError("byte observations are outside the SDK domain") + frame = fit_rotation_frame(observations, sdk_to_joint_direction=sdk_to_joint_direction, + reference_xyzw=reference_xyzw, training_cycles=training_cycles) + reference = Rotation.from_quat(frame.reference_xyzw) + axis = np.asarray(frame.axis_xyz) + angles = (reference.inv() * Rotation.from_quat([r.quaternion_xyzw for r in observations])).as_rotvec() @ axis + lower, upper = common_directional_input_support(inputs, [row.direction for row in observations]) + knots = np.linspace(lower, upper, knot_count) + branches = {} + maximum_correction = 0.0 + for direction in ("increasing", "decreasing"): + mask = np.asarray([row.direction == direction for row in observations]) + x, y = inputs[mask], angles[mask] + order = np.argsort(x, kind="stable") + unique, starts = np.unique(x[order], return_index=True) + # Median repeated encoder observations before projecting monotonically. + medians = np.asarray([np.median(group) for group in np.split(y[order], starts[1:])]) + projected = -sdk_to_joint_direction * isotonic_nonincreasing(-sdk_to_joint_direction * medians) + maximum_correction = max(maximum_correction, float(np.max(np.abs(projected - medians)))) + branches[direction] = tuple(float(v) for v in np.interp(knots, unique, projected)) + return RotationCurve(input_domain, tuple(float(v) for v in knots), + branches["increasing"], branches["decreasing"], tuple(reference.as_quat()), + tuple(axis), sdk_to_joint_direction, frozenset(row.sample_id for row in observations), maximum_correction, + tuple(training_cycles), holdout_cycle) + + +@dataclass(frozen=True) +class VisualRotationFrame: + """Training-only hinge frame, independent of any SDK-to-angle regression.""" + reference_xyzw: tuple[float, ...] + axis_xyz: tuple[float, ...] + training_sample_ids: tuple[str, ...] + training_cycles: tuple[int, ...] = LEGACY_TRAINING + + @property + def circle(self): + return {"space": "relative_rotation_3d", "reference_quaternion_xyzw": self.reference_xyzw, + "axis_xyz": self.axis_xyz, "training_cycles": list(self.training_cycles), + "training_sample_ids": self.training_sample_ids} + + +def fit_rotation_frame(observations, *, sdk_to_joint_direction, reference_xyzw=None, + training_cycles=LEGACY_TRAINING): + _validate_observations(observations, cycles=set(training_cycles)) + for cycle in training_cycles: + for direction in ("increasing", "decreasing"): + if sum(r.cycle == cycle and r.direction == direction for r in observations) < 40: + raise ValueError("visual frame requires 40 training images per cycle/direction") + if sdk_to_joint_direction not in {-1, 1}: + raise ValueError("visual frame requires a signed physical direction") + inputs = np.asarray([row.input_value for row in observations]) rotations = Rotation.from_quat([row.quaternion_xyzw for row in observations]) # Use one observed reference, never assert that its input is a CAD zero. reference = rotations[int(np.argmin(inputs))] if reference_xyzw is None else _rotation(reference_xyzw) @@ -134,27 +210,5 @@ def fit_rotation_curve( axis, angles = -axis, -angles if np.ptp(angles) >= math.pi - math.radians(1): raise ValueError("hinge travel exceeds this rotation primitive's unambiguous interval") - supports = [] - for direction in ("increasing", "decreasing"): - selected = [i for i, row in enumerate(observations) if row.direction == direction] - x = inputs[selected] - supports.append((float(np.min(x)), float(np.max(x)))) - lower, upper = max(s[0] for s in supports), min(s[1] for s in supports) - if upper <= lower: - raise ValueError("directional SDK supports do not overlap") - knots = np.linspace(lower, upper, knot_count) - branches = {} - maximum_correction = 0.0 - for direction in ("increasing", "decreasing"): - mask = np.asarray([row.direction == direction for row in observations]) - x, y = inputs[mask], angles[mask] - order = np.argsort(x, kind="stable") - unique, starts = np.unique(x[order], return_index=True) - # Median repeated encoder observations before projecting monotonically. - medians = np.asarray([np.median(group) for group in np.split(y[order], starts[1:])]) - projected = -sdk_to_joint_direction * isotonic_nonincreasing(-sdk_to_joint_direction * medians) - maximum_correction = max(maximum_correction, float(np.max(np.abs(projected - medians)))) - branches[direction] = tuple(float(v) for v in np.interp(knots, unique, projected)) - return RotationCurve(input_domain, tuple(float(v) for v in knots), - branches["increasing"], branches["decreasing"], tuple(reference.as_quat()), - tuple(axis), sdk_to_joint_direction, frozenset(row.sample_id for row in observations), maximum_correction) + return VisualRotationFrame(tuple(reference.as_quat()), tuple(axis), + tuple(sorted(row.sample_id for row in observations)), tuple(training_cycles)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py index f1d0eeb..7de45f1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py @@ -9,6 +9,7 @@ from typing import Mapping from ..domain.profile import CalibrationProfile, validate_profile from ..domain.result import CalibrationResult, JointMapping +from ..domain.capture_plan import CapturePlan, ADAPTIVE, training_cycles from ..urdf.kinematics import UrdfKinematicModel from .motion_fit import channel_for_joint, fit_profile_motion, joint_input_direction from .spatial import ZeroCalibrationProfile, fit_joint_axis_measurement, solve_urdf_zero_offsets, with_depth_free_axis_projection @@ -49,7 +50,7 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi raise ValueError("spatial zero has no position in the declared axis order") direct = tuple(name for name in axes if name in direct) return ZeroCalibrationProfile( - hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints), + hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints - profile.retained_joints), stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias), direct_zero_joints=direct, axis_joints=axes, inherited_zero_joints={} if profile.artifacts.output_schema_version >= 3 else dict(profile.zero.transferred_zero_sources), @@ -70,8 +71,56 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi independent_observed_motion=profile.artifacts.output_schema_version >= 3) +def build_axis_observations(profile, model, geometry_records, motion, zero_profile, *, include_holdout=True): + """Build only the axis observations present in the declared partition.""" + measured = profile.artifacts.output_schema_version >= 3 + geometry_domain = f"command_{profile.command.unit}" if profile.command_based_release else profile.curve_input_domain + observations = [] + plan = CapturePlan.from_profile(profile) + phase = "steady" if profile.command_based_release else "sweep" + for cycle in sorted({c for part in plan.tasks for c in part.cycles(phase)}): + if not include_holdout and cycle in { + part.command_holdout if phase == "steady" else part.holdout for part in plan.tasks}: + continue + by_joint = {} + for joint in zero_profile.axis_joints: + task = next(task for task in profile.motion.tasks if joint in task.joints) + if cycle not in plan.task(task.key).cycles(phase): + continue + rows = geometry_records[joint] + if profile.command_based_release: + rows = [dict(row, input_value=float(row[geometry_domain]), + state_values=row[f"state_{profile.command.unit}"]) for row in rows] + elif profile.command.unit == "rad": + rows = [dict(row, input_value=float(row[profile.curve_input_domain]), state_values=row["state_rad"]) + for row in rows] + else: + rows = [dict(row, command_u8=int(round(float(row[profile.curve_input_domain])))) for row in rows] + parent = zero_profile.phase_parent_joint.get(joint) + if parent is not None and parent not in by_joint: + if cycle != plan.task(task.key).holdout and profile.quality.training_policy == ADAPTIVE: + continue # No same-cycle parent observation: do not fabricate a geometric sample. + raise ValueError("spatial axis order must place phase parents first") + task = next(task for task in profile.motion.tasks if joint in task.joints) + baseline = (motion.reference_inputs[joint] if measured else task.start_value if profile.command.unit == "rad" + else profile.command.baseline_values[channel_for_joint(profile, joint)]) + axis = fit_joint_axis_measurement(joint, rows, cycle=cycle, zero_command_u8=baseline, + input_to_joint_direction=joint_input_direction(profile, model, joint), + constrained_circle_joints=zero_profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + axis_common_constraint=None if parent is None else by_joint[parent].axis_common_xyz, + separate_axial_residual=True, + condition_command_field=f"command_vector_{profile.command.unit}" if profile.command_based_release else None) + if profile.zero.spatial.get("depth_free_axis_projection", False): + axis = with_depth_free_axis_projection(axis, rows[0]["camera_center_common_xyz_m"]) + observations.append(axis) + by_joint[joint] = axis + return observations + + def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, records_by_joint, - *, cross_view_records=None, zero_references=None, motion_fitted=lambda _motion: None) -> CalibrationResult: + *, cross_view_records=None, zero_references=None, steady_records=None, motion_fitted=lambda _motion: None) -> CalibrationResult: """Shared native-domain motion, spatial zero and standard mimic fit. No mechanical endpoint or range-centre fallback can replace a requested @@ -89,9 +138,30 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco if not rows or any(not required_pose_fields.issubset(row) for row in rows): raise ValueError(f"spatial calibration requires identified common-frame pose observations:{joint}") model = UrdfKinematicModel(source_urdf) + if profile.retained_joints: + from ..urdf.partial_scope import require_profile_held_pose + from ...profiles.observations import compile_tag_links + links = compile_tag_links(profile, model) + for name, rows in records_by_joint.items(): + spec = profile.measurement.measurements[name] + for row in rows: + for role in (spec.parent_role, spec.child_role): + require_profile_held_pose(profile, model, row[f"command_vector_{profile.command.unit}"], links[role]) zero_profile = compile_spatial_profile(profile) - motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records, - zero_references=zero_references) + if profile.command_based_release: + from .command_motion import fit_command_motion + motion = fit_command_motion(profile, model, records_by_joint, steady_records or {}, zero_references or {}) + geometry_records = steady_records + geometry_domain = f"command_{profile.command.unit}" + else: + motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records, + zero_references=zero_references) + geometry_records = records_by_joint + geometry_domain = profile.curve_input_domain + for joint in profile.zero.axis_joints: + if not geometry_records.get(joint) or any(not required_pose_fields.issubset(row) + for row in geometry_records[joint]): + raise ValueError(f"spatial calibration requires identified geometry poses:{joint}") motion_fitted(motion) curves = motion.coordinate_curves measured = profile.artifacts.output_schema_version >= 3 @@ -102,36 +172,15 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco "known_cad_baseline_rad": baseline_geometry[name]}) if measured and name in baseline_geometry and name not in zero_profile.direct_zero_joints else curve for name, curve in curves.items()} - observations = [] - for cycle in range(4): - by_joint = {} - for joint in zero_profile.axis_joints: - rows = records_by_joint[joint] - if profile.command.unit == "rad": - rows = [dict(row, input_value=float(row[profile.curve_input_domain]), state_values=row["state_rad"]) - for row in rows] - else: - rows = [dict(row, command_u8=int(round(float(row[profile.curve_input_domain])))) for row in rows] - parent = zero_profile.phase_parent_joint.get(joint) - if parent is not None and parent not in by_joint: - raise ValueError("spatial axis order must place phase parents first") - task = next(task for task in profile.motion.tasks if joint in task.joints) - baseline = (motion.reference_inputs[joint] if measured else task.start_value if profile.command.unit == "rad" - else profile.command.baseline_values[channel_for_joint(profile, joint)]) - axis = fit_joint_axis_measurement(joint, rows, cycle=cycle, zero_command_u8=baseline, - input_to_joint_direction=joint_input_direction(profile, model, joint), - constrained_circle_joints=zero_profile.constrained_circle_joints, - view_normal_common_xyz=rows[0]["view_normal_common_xyz"], - canonical_zero_direction="decreasing", - axis_common_constraint=None if parent is None else by_joint[parent].axis_common_xyz, - separate_axial_residual=True) - if profile.zero.spatial.get("depth_free_axis_projection", False): - axis = with_depth_free_axis_projection(axis, rows[0]["camera_center_common_xyz_m"]) - observations.append(axis) - by_joint[joint] = axis + observations = build_axis_observations(profile, model, geometry_records, motion, zero_profile) + plan = CapturePlan.from_profile(profile) + geometry_training = tuple(sorted({cycle for part in plan.tasks for cycle in + (part.command_training if profile.command_based_release else part.training)})) + geometry_holdout = (plan.tasks[0].command_holdout if profile.command_based_release else profile.quality.holdout_cycle) zero = solve_urdf_zero_offsets(source_urdf=source_urdf, measurements=observations, curves=spatial_curves, motor_by_joint={joint: channel_for_joint(profile, joint) for joint in curves}, - zero_profile=zero_profile, training_cycles=(0, 1, 2), validation_cycle=3, + zero_profile=zero_profile, training_cycles=geometry_training, validation_cycle=geometry_holdout, + allow_partial_training_cycles=profile.quality.training_policy == ADAPTIVE, maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20), maximum_validation_mae_rad=math.radians(1), maximum_validation_p95_rad=math.radians(2), maximum_validation_error_rad=math.radians(3), maximum_confidence_half_width_rad=math.radians(1), @@ -145,11 +194,15 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco offsets[name], methods[name] = datum.cad_angle_rad, "independent_known_baseline_geometry" for name in profile.zero.cad_zero_assumptions: offsets[name], methods[name] = 0.0, "assumed_source_cad_zero" - mappings = {name: JointMapping(name, channel_for_joint(profile, name), profile.curve_input_domain, + mappings = {name: JointMapping(name, channel_for_joint(profile, name), geometry_domain, tuple(curve.circle["input_knots"]), curve.angle_rad, curve.increasing_rad, curve.decreasing_rad) for name, curve in curves.items()} return CalibrationResult(curves, mappings, offsets, methods, {}, motion.holdout_errors_rad, zero, - motion.cross_view_metrics, motion.reference_inputs, joint_zero_references=dict(zero_references)) + motion.cross_view_metrics, motion.reference_inputs, joint_zero_references=dict(zero_references), + command_mappings=motion.mappings if profile.command_based_release else {}, + command_holdout_metrics=motion.command_metrics if profile.command_based_release else {}, + command_applicability=motion.applicability if profile.command_based_release else {}, + feedback_diagnostics=motion.feedback_diagnostics if profile.command_based_release else {}) for name in profile.zero.active_joints - offsets.keys() - profile.zero.transferred_zero_sources.keys(): if name not in profile.zero.cad_frozen_joints: raise ValueError(f"active static zero has no measured or CAD-retention policy:{name}") @@ -171,7 +224,7 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco import numpy as np training = records_by_joint[donor] support = [[float(r[profile.curve_input_domain]) for r in training - if r["cycle"] in {0, 1, 2} and r["direction"] == direction] + if r["cycle"] in training_cycles(profile, joint=donor) and r["direction"] == direction] for direction in ("increasing", "decreasing")] lo, hi = math.ceil(max(min(s) for s in support)), math.floor(min(max(s) for s in support)) knots = tuple(float(v) for v in range(lo, hi+1)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py index 2fe9046..19afc63 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py @@ -21,187 +21,71 @@ from .stages import ( from .types import ZeroSolveResult -def decide_acceptance( - *, - cycles: CycleEvidence, - fit: TrainingFit, - geometry_evidence: GeometryEvidence, - holdout: HoldoutEvidence, - limits: OffsetLimits, - observability: ObservabilityEvidence, - options: ZeroSolveOptions, - problem: ZeroProblem, -) -> AcceptanceDecision: - applied_training = cycles.applied_training - confidence_half_widths = cycles.confidence_half_widths - cycle_consistent = cycles.cycle_consistent - diagnostic_offset_limits = limits.diagnostic_offset_limits - fixed_offsets = problem.fixed_offsets - improvement_confidence_lower = holdout.improvement_confidence_lower - improvement_passed = holdout.improvement_passed - inconsistent_cycles = cycles.inconsistent_cycles - insignificant_large = cycles.insignificant_large - maximum_confidence_half_width_rad = options.maximum_confidence_half_width_rad - maximum_observability_condition_number = options.maximum_observability_condition_number - maximum_offset_rad = options.maximum_offset_rad - maximum_validation_error_rad = options.maximum_validation_error_rad - maximum_validation_mae_rad = options.maximum_validation_mae_rad - maximum_validation_p95_rad = options.maximum_validation_p95_rad - observability_condition_number = observability.observability_condition_number - observability_parameter_count = observability.observability_parameter_count - observability_rank = observability.observability_rank - observation_failures = geometry_evidence.observation_failures - offset_limits = limits.offset_limits - output_offsets = problem.output_offsets - palm_orientation_validation_passed = holdout.palm_orientation_validation_passed - product_finger_rolls = limits.product_finger_rolls +def decide_training_acceptance(*, cycles, fit, geometry_evidence, limits, observability, options, problem): + """All training gates shared by early stopping and final acceptance.""" profile = problem.profile - training_offsets = fit.training_offsets - validation_errors = holdout.validation_errors - - finger_roll_common_mode = ( - float( - np.median( - [training_offsets[name] for name in product_finger_rolls] - ) - ) - if profile.parallel_root_pattern - and product_finger_rolls - else 0.0 - ) - configured_limit_exceeded: list[str] = [] - for name, limit in zip(profile.direct_zero_joints, offset_limits): - if name in fixed_offsets: + common_mode = (float(np.median([fit.training_offsets[name] for name in limits.product_finger_rolls])) + if profile.parallel_root_pattern and limits.product_finger_rolls else 0.) + failures = {} + for name, limit, diagnostic_limit in zip(profile.direct_zero_joints, + limits.offset_limits, limits.diagnostic_offset_limits): + if name in problem.fixed_offsets: continue - # A post-solve mechanical endpoint datum is the value that will be - # published for this joint. The visual root-axis scalar remains a - # nuisance gauge used for holdout geometry and must not be compared - # with the safety bound of a different, endpoint-anchored output. - checked_offset = output_offsets.get(name, training_offsets[name]) - if name in product_finger_rolls: - # The four roll motors share the same electrical centre and the - # absolute palm axial datum is recovered from the root-line - # pattern. Protect the independently assembled finger-to-finger - # deviations with the strict finger bound; protect their shared - # common mode with the unchanged global zero bound. Treating the - # same common datum as four independent failures is both - # over-counting and sensitive to the palm-frame gauge. - checked_offset -= finger_roll_common_mode - if abs(checked_offset) > limit + math.radians(0.01): - configured_limit_exceeded.append(name) - if ( - product_finger_rolls - and abs(finger_roll_common_mode) - > maximum_offset_rad + math.radians(0.01) - ): - configured_limit_exceeded.append("finger_mcp_roll_common_mode") - diagnostic_bound_hits: list[str] = [] - for name, limit in zip( - profile.direct_zero_joints, diagnostic_offset_limits - ): - if name in fixed_offsets: - continue - checked_offset = output_offsets.get(name, training_offsets[name]) - if name in product_finger_rolls: - # Match the configured-limit and publication convention above. - # The raw common roll is a fitted-palm-frame gauge; only the - # finger-to-finger deviation is a physical zero correction. - checked_offset -= finger_roll_common_mode - if abs(checked_offset) >= limit - math.radians(0.01): - diagnostic_bound_hits.append(name) - failure_reasons: dict[str, str] = {} - if not palm_orientation_validation_passed: - failure_reasons["palm_orientation"] = ( - "palm_orientation_holdout_too_large" - ) - requires_full_observability = bool( - profile.parallel_root_pattern - and profile.base_pose_strategy == "full_hand" - ) - if ( - requires_full_observability - and observability_rank < observability_parameter_count - ): - failure_reasons["palm_and_static_zero"] = ( - "zero_observation_jacobian_rank_deficient" - ) - elif ( - requires_full_observability - and observability_condition_number - > maximum_observability_condition_number - ): - failure_reasons["palm_and_static_zero"] = ( - "zero_observation_jacobian_ill_conditioned" - ) - for name in configured_limit_exceeded: - failure_reasons[name] = "zero_offset_exceeds_configured_limit" - for name in diagnostic_bound_hits: - failure_reasons[name] = "zero_offset_reached_diagnostic_bound" - for name in inconsistent_cycles: - failure_reasons[name] = "zero_offset_cycle_difference_too_large" - for name in insignificant_large: - failure_reasons[name] = "zero_offset_not_statistically_significant" - if maximum_confidence_half_width_rad is not None: - for name, half_width in confidence_half_widths.items(): - if half_width > maximum_confidence_half_width_rad: - failure_reasons[name] = ( - "zero_offset_confidence_interval_too_wide" - ) - # This solver publishes rotational encoder zeros only. A post-fit CAD to - # measured axis-line displacement is invariant to the joint's own zero - # and cannot be repaired by changing that rotational parameter. Keep the - # per-joint and aggregate values in ZeroSolveResult for geometry audit, - # but do not misclassify a fixed link-origin/Tag-depth discrepancy as a - # failed rotational holdout. Axis-point *fit* quality is still guarded - # above for every phase observation that actually uses line position. - if not improvement_passed: - for name, value in applied_training.items(): - if ( - name not in fixed_offsets - and value != 0.0 - and improvement_confidence_lower.get(name, 0.0) <= 0.0 - ): - failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" - # Geometry failures are the root cause and must not be hidden by the - # downstream validation symptom produced by the same bad observation. - failure_reasons.update(observation_failures) - passed = bool( - validation_errors.size - == len(profile.direct_zero_joints) - len(fixed_offsets) - and float(np.mean(validation_errors)) <= maximum_validation_mae_rad - and float(np.percentile(validation_errors, 95.0)) - <= maximum_validation_p95_rad - and ( - maximum_validation_error_rad is None - or float(np.max(validation_errors)) - <= maximum_validation_error_rad - ) - and cycle_consistent - and not insignificant_large - and not configured_limit_exceeded - and not diagnostic_bound_hits - and not observation_failures - and ( - not requires_full_observability - or ( - observability_rank == observability_parameter_count - and observability_condition_number - <= maximum_observability_condition_number - ) - ) - and not any( - reason == "zero_offset_confidence_interval_too_wide" - for reason in failure_reasons.values() - ) - and improvement_passed - and palm_orientation_validation_passed - ) + checked = problem.output_offsets.get(name, fit.training_offsets[name]) + if name in limits.product_finger_rolls: + checked -= common_mode + if abs(checked) > limit + math.radians(.01): + failures[name] = "zero_offset_exceeds_configured_limit" + if abs(checked) >= diagnostic_limit - math.radians(.01): + failures[name] = "zero_offset_reached_diagnostic_bound" + if limits.product_finger_rolls and abs(common_mode) > options.maximum_offset_rad + math.radians(.01): + failures["finger_mcp_roll_common_mode"] = "zero_offset_exceeds_configured_limit" + if profile.parallel_root_pattern and profile.base_pose_strategy == "full_hand": + if observability.observability_rank < observability.observability_parameter_count: + failures["palm_and_static_zero"] = "zero_observation_jacobian_rank_deficient" + elif observability.observability_condition_number > options.maximum_observability_condition_number: + failures["palm_and_static_zero"] = "zero_observation_jacobian_ill_conditioned" + for name in cycles.inconsistent_cycles: + failures[name] = "zero_offset_cycle_difference_too_large" + for name in cycles.insignificant_large: + failures[name] = "zero_offset_not_statistically_significant" + if options.maximum_confidence_half_width_rad is not None: + for name, width in cycles.confidence_half_widths.items(): + if width > options.maximum_confidence_half_width_rad: + failures[name] = "zero_offset_confidence_interval_too_wide" + failures.update(geometry_evidence.observation_failures) + return AcceptanceDecision(finger_roll_common_mode=common_mode, failure_reasons=failures, + passed=cycles.cycle_consistent and not failures) - return AcceptanceDecision( - finger_roll_common_mode=finger_roll_common_mode, - failure_reasons=failure_reasons, - passed=passed, - ) + +def decide_acceptance( + *, cycles: CycleEvidence, fit: TrainingFit, geometry_evidence: GeometryEvidence, + holdout: HoldoutEvidence, limits: OffsetLimits, observability: ObservabilityEvidence, + options: ZeroSolveOptions, problem: ZeroProblem, +) -> AcceptanceDecision: + training = decide_training_acceptance(cycles=cycles, fit=fit, geometry_evidence=geometry_evidence, + limits=limits, observability=observability, options=options, problem=problem) + failures = dict(training.failure_reasons) + if not holdout.palm_orientation_validation_passed: + failures["palm_orientation"] = "palm_orientation_holdout_too_large" + if not holdout.improvement_passed: + for name, value in cycles.applied_training.items(): + if (name not in problem.fixed_offsets and value != 0. + and holdout.improvement_confidence_lower.get(name, 0.) <= 0.): + failures[name] = "zero_offset_did_not_improve_with_95pct_confidence" + # Geometry is the root cause; retain its reason over downstream symptoms. + failures.update(geometry_evidence.observation_failures) + errors = holdout.validation_errors + passed = bool(training.passed and holdout.improvement_passed + and holdout.palm_orientation_validation_passed + and errors.size == len(problem.profile.direct_zero_joints) - len(problem.fixed_offsets) + and errors.size > 0 + and float(np.mean(errors)) <= options.maximum_validation_mae_rad + and float(np.percentile(errors, 95.)) <= options.maximum_validation_p95_rad + and (options.maximum_validation_error_rad is None + or float(np.max(errors)) <= options.maximum_validation_error_rad)) + return AcceptanceDecision(finger_roll_common_mode=training.finger_roll_common_mode, + failure_reasons=failures, passed=passed) def assemble_result( diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py index 3453e17..4d0fc44 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py @@ -399,6 +399,7 @@ def fit_joint_axis_measurement( canonical_zero_direction: str | None = None, separate_axial_residual: bool = False, input_to_joint_direction: int = -1, + condition_command_field: str | None = None, ) -> JointAxisMeasurement: """Fit one physical screw axis from one complete scan cycle.""" samples = [ @@ -527,17 +528,21 @@ def fit_joint_axis_measurement( ) ) point_common = parent_rotation.apply(point_parent) + parent_translation - state = np.median( - np.asarray([record["state_values"] if "state_values" in record else record["state_u8"] - for record in zero_records], dtype=float), - axis=0, - ) + states = [record.get("state_values", record.get("state_u8")) for record in zero_records] + # Command-conditioned geometry never invents an encoder condition. Keep + # unavailable diagnostics explicit while preserving legacy requirements. + if any(value is None for value in states): + if condition_command_field is None: + raise ValueError("axis_condition_feedback_missing") + state = None + else: + state = tuple(float(value) for value in np.median(np.asarray(states, dtype=float), axis=0)) return JointAxisMeasurement( joint=str(joint), cycle=int(cycle), axis_common_xyz=tuple(float(value) for value in axis_common), point_common_xyz_m=tuple(float(value) for value in point_common), - condition_state_u8=tuple(float(value) for value in state), + condition_state_u8=state, plane_rms_m=float(plane_rms), radial_rms_m=float(circle["radial_rms_m"]), rotation_circle_axis_difference_rad=float(disagreement), @@ -560,4 +565,7 @@ def fit_joint_axis_measurement( pose_axis_line_transverse_rms_m=residual_diagnostics["transverse_rms_m"], axis_point_axial_component_separated=separate_axial, pose_axis_line_source_joints=(str(joint),), + condition_command_u8=(tuple(float(v) for v in np.median( + np.asarray([r[condition_command_field] for r in zero_records], dtype=float), axis=0)) + if condition_command_field else None), ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py index c1c76ed..55a18a9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py @@ -25,6 +25,7 @@ def optimise_offsets( limits: OffsetLimits, problem: TrainingProblem, geometry: ObservationGeometry, + fitted_names=None, ) -> dict[str, float]: diagnostic_offset_limits = limits.diagnostic_offset_limits fixed_offsets = problem.fixed_offsets @@ -48,6 +49,8 @@ def optimise_offsets( ): if name in fixed_offsets: continue + if fitted_names is not None and name not in fitted_names: + continue has_axis_pair = name in profile.same_view_axis_pair_by_offset limit = float( configured_limit if has_axis_pair else diagnostic_limit @@ -57,7 +60,7 @@ def optimise_offsets( candidate = dict(result) candidate[name] = float(value[0]) samples = angular_error_samples( - candidate, selected, base_rotation, base_translation + candidate, selected, base_rotation, base_translation, only_offsets={name} )[name] return np.asarray( [ diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py index e7245cd..8561587 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py @@ -51,11 +51,11 @@ def prepare_zero_problem( raise ValueError("zero profile layout does not match tag_layout") model = UrdfKinematicModel(source_urdf) training = [m for m in measurements if m.cycle in set(training_cycles)] - validation = [m for m in measurements if m.cycle == int(validation_cycle)] + validation = [m for m in measurements if validation_cycle is not None and m.cycle == int(validation_cycle)] expected = set(profile.axis_joints) if ( {m.joint for m in training} != expected - or {m.joint for m in validation} != expected + or (validation_cycle is not None and {m.joint for m in validation} != expected) ): raise ValueError("axis measurements do not contain all required joints/cycles") configured_palm_sources = { @@ -86,7 +86,7 @@ def prepare_zero_problem( ) required_orientation_cycles = { *(int(value) for value in training_cycles), - int(validation_cycle), + *((int(validation_cycle),) if validation_cycle is not None else ()), } for cycle in required_orientation_cycles: cycle_sources = { @@ -107,7 +107,7 @@ def prepare_zero_problem( orientation_validation = tuple( item for item in orientation_measurements - if int(item.cycle) == int(validation_cycle) + if validation_cycle is not None and int(item.cycle) == int(validation_cycle) ) orientation_by_model_cycle: dict[ tuple[str, int], PalmOrientationMeasurement diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py index 42f8d64..a8edd05 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py @@ -27,6 +27,24 @@ class ObservationGeometry: curves: Mapping[str, JointCurveFit] motor_by_joint: Mapping[str, int] orientation_by_model_cycle: Mapping[tuple[str, int], PalmOrientationMeasurement] + incomplete_training_cycles: frozenset[int] = frozenset() + + def complete_offsets(self, selected): + """Offsets whose same-cycle observation dependencies actually exist.""" + keys = {(item.joint, item.cycle) for item in selected} + available = set() + for name, observer in self.profile.offset_observer_joint.items(): + for joint, cycle in keys: + if joint != observer: + continue + pair = self.profile.same_view_axis_pair_by_offset.get(name) + if pair and all((axis, cycle) in self.orientation_by_model_cycle for axis in pair): + available.add(name) + elif observer in self.profile.axis_parent_joint: + available.add(name) + elif (self.profile.phase_parent_joint.get(observer), cycle) in keys: + available.add(name) + return available def for_cycles(self, cycles: Sequence[int]) -> ObservationGeometry: selected = set(cycles) @@ -220,6 +238,7 @@ class ObservationGeometry: selected: Sequence[JointAxisMeasurement], base_rotation: Rotation, base_translation: np.ndarray, + *, only_offsets=None, ) -> dict[str, tuple[float, ...]]: curves = self.curves model = self.model @@ -233,6 +252,8 @@ class ObservationGeometry: name: [] for name in profile.direct_zero_joints } for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if only_offsets is not None and offset_joint not in only_offsets: + continue pair_errors = same_view_axis_pair_errors( offset_joint, selected, offsets ) @@ -277,6 +298,8 @@ class ObservationGeometry: parent_joint = profile.phase_parent_joint[observer_joint] observed_parent = by_key.get((parent_joint, item.cycle)) if observed_parent is None: + if item.cycle in self.incomplete_training_cycles: + continue raise ValueError( f"phase parent is missing: {parent_joint} cycle {item.cycle}" ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py index 85974f3..da0d94b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py @@ -2,12 +2,13 @@ from __future__ import annotations +from dataclasses import dataclass from pathlib import Path from typing import Mapping, Sequence import math from ...domain.measurement import JointCurveFit -from .acceptance import assemble_result, decide_acceptance +from .acceptance import assemble_result, decide_acceptance, decide_training_acceptance from .optimization import solve_selected from .preparation import prepare_offset_limits, prepare_zero_problem from .residuals import ObservationGeometry @@ -22,6 +23,25 @@ from .types import ( from .validation import check_observation_geometry, validate_axis_lines, validate_holdout +@dataclass(frozen=True) +class ZeroTrainingResult: + offsets: Mapping[str, float] + confidence_half_widths: Mapping[str, float] + cycle_values: Mapping[str, Sequence[float]] + failures: tuple[str, ...] + + +def fit_urdf_zero_training(*, measurements, training_cycles, **kwargs): + """Training-only entry: independent validation observations cannot enter.""" + if any(item.cycle not in training_cycles for item in ( + *measurements, *kwargs.get("palm_orientation_measurements", ()))): + raise ValueError("zero_training_received_nontraining_data") + if "validation_cycle" in kwargs: + raise ValueError("zero_training_has_no_validation_cycle") + return solve_urdf_zero_offsets(measurements=measurements, training_cycles=training_cycles, + validation_cycle=None, **kwargs) + + def solve_urdf_zero_offsets( *, source_urdf: str | Path, @@ -30,7 +50,7 @@ def solve_urdf_zero_offsets( curves: Mapping[str, JointCurveFit], motor_by_joint: Mapping[str, int], training_cycles: Sequence[int] = (0, 1), - validation_cycle: int = 2, + validation_cycle: int | None = 2, maximum_offset_rad: float = math.radians(20.0), finger_maximum_offset_rad: float = math.radians(3.0), joint_maximum_offset_rad: Mapping[str, float] | None = None, @@ -50,7 +70,8 @@ def solve_urdf_zero_offsets( fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, static_output_zero_offsets_rad: Mapping[str, float] | None = None, zero_profile: ZeroCalibrationProfile | None = None, -) -> ZeroSolveResult: + allow_partial_training_cycles: bool = False, +) -> ZeroSolveResult | ZeroTrainingResult: """Prepare, fit training only, validate the frozen result, then assemble.""" options = ZeroSolveOptions( maximum_offset_rad=maximum_offset_rad, @@ -81,7 +102,8 @@ def solve_urdf_zero_offsets( static_output_zero_offsets_rad=static_output_zero_offsets_rad, zero_profile=zero_profile, ) - geometry = ObservationGeometry(problem.profile, problem.model, curves, motor_by_joint, problem.orientation_by_model_cycle) + geometry = ObservationGeometry(problem.profile, problem.model, curves, motor_by_joint, problem.orientation_by_model_cycle, + frozenset(training_cycles) if allow_partial_training_cycles else frozenset()) training_geometry = geometry.for_cycles(training_cycles) training_problem = TrainingProblem(problem.profile, problem.training, problem.fixed_offsets, problem.zero_offsets) limits = prepare_offset_limits(problem=problem, options=options) @@ -93,6 +115,11 @@ def solve_urdf_zero_offsets( geometry=geometry, selected=list(measurements), offsets=offsets, rotation=rotation) cycles = estimate_cycle_evidence(problem=training_problem, limits=limits, fit=fit, options=options, geometry=training_geometry, measurements=problem.training, training_cycles=training_cycles) + if validation_cycle is None: + decision = decide_training_acceptance(problem=problem, limits=limits, fit=fit, options=options, + observability=observability, geometry_evidence=geometry_evidence, cycles=cycles) + return ZeroTrainingResult(offsets, cycles.confidence_half_widths, cycles.cycle_values, + tuple(f"{reason}:{name}" for name, reason in sorted(decision.failure_reasons.items()))) holdout = validate_holdout(problem=problem, fit=fit, cycles=cycles, options=options, geometry=geometry, measurements=measurements) lines = validate_axis_lines(problem=problem, fit=fit, cycles=cycles, geometry=geometry) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py index 6ad3f2d..8841fe0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py @@ -174,6 +174,9 @@ def estimate_cycle_evidence( training_cycle_ids = tuple(sorted({int(value) for value in training_cycles})) for cycle in training_cycle_ids: selected = [item for item in measurements if item.cycle == cycle] + available = geometry.complete_offsets(selected) + if not available: + continue # Keep one palm pose while comparing cycles. Refitting a base pose from # only two nearly parallel root axes per cycle makes harmless root-line # noise appear as a large encoder-zero change. @@ -183,17 +186,19 @@ def estimate_cycle_evidence( cycle_rotation, cycle_translation, initial=training_offsets, + fitted_names=available, ) cycle_models[cycle] = (cycle_rotation, cycle_translation) for name, value in cycle_offsets.items(): - cycle_values[name].append(value) + if name in available: + cycle_values[name].append(value) uncertainties: dict[str, float] = {} confidence_half_widths: dict[str, float] = {} cycle_consistent = True inconsistent_cycles: list[str] = [] for name, values in cycle_values.items(): array = np.asarray(values, dtype=float) - spread = float(np.max(array) - np.min(array)) + spread = float(np.ptp(array)) if array.size else float("inf") if spread > maximum_cycle_difference_rad: cycle_consistent = False inconsistent_cycles.append(name) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py index b2ae27c..0dfd0b2 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py @@ -74,7 +74,7 @@ class JointAxisMeasurement: cycle: int axis_common_xyz: tuple[float, float, float] point_common_xyz_m: tuple[float, float, float] - condition_state_u8: tuple[float, ...] + condition_state_u8: tuple[float, ...] | None plane_rms_m: float radial_rms_m: float rotation_circle_axis_difference_rad: float diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py index 9fdbfb0..403d07a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py @@ -14,6 +14,7 @@ import numpy as np from scipy.spatial.transform import Rotation from ..geometry.rotation import robust_rotation_summary +from ..domain.capture_plan import LEGACY_TRAINING from ..urdf.kinematics import UrdfKinematicModel @@ -59,6 +60,7 @@ class FrozenTagInstallation: training_sample_ids: tuple[str, ...] training_rotation_p95_deg: float training_translation_p95_m: float + training_cycles: tuple[int, ...] = LEGACY_TRAINING class TagInstallationFailure(ValueError): @@ -76,20 +78,25 @@ def fit_tag_installations( *, source_model: UrdfKinematicModel, common_from_base, link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose], minimum_samples: int = 40, + training_cycles_by_role: Mapping[str, Sequence[int]] | None = None, ) -> dict[str, FrozenTagInstallation]: - """Fit mounting transforms once across all tasks and three cycles. + """Fit mounting transforms once across each role's declared training cycles. Repeated records of one image/Tag are deduplicated only when their pose and joint conditions agree. Conflicting duplicates are invalid evidence. Training residuals check rigid installation, not a per-frame PnP gate. """ base_from_common = np.linalg.inv(rigid_matrix(common_from_base)) + partitions = {role: tuple(training_cycles_by_role[role]) if training_cycles_by_role is not None + else LEGACY_TRAINING for role in link_by_role} + if any(cycles not in (LEGACY_TRAINING, LEGACY_TRAINING[:2]) for cycles in partitions.values()): + raise ValueError("Tag installation requires declared independent training cycles") grouped: dict[str, dict[str, tuple[int, np.ndarray]]] = {role: {} for role in link_by_role} for row in observations: - if row.cycle not in {0, 1, 2} or not row.sample_id: - raise ValueError("only identified training images may fit Tag installations") if row.role not in link_by_role: raise ValueError(f"undeclared Tag installation:{row.role}") + if row.cycle not in partitions[row.role] or not row.sample_id: + raise ValueError("only identified training images may fit Tag installations") candidate = np.linalg.inv(link_pose(source_model, link_by_role[row.role], row.cad_angles)) @ base_from_common @ rigid_matrix(row.common_from_tag) previous = grouped[row.role].get(row.sample_id) if previous is not None and (previous[0] != row.cycle or not np.allclose(previous[1], candidate, atol=1e-8, rtol=0)): @@ -97,8 +104,8 @@ def fit_tag_installations( grouped[row.role][row.sample_id] = (row.cycle, candidate) output, diagnostics = {}, {} for role, unique in sorted(grouped.items()): - if len(unique) < minimum_samples or {value[0] for value in unique.values()} != {0, 1, 2}: - raise ValueError(f"Tag installation lacks three-cycle training evidence:{role}") + if len(unique) < minimum_samples or {value[0] for value in unique.values()} != set(partitions[role]): + raise ValueError(f"Tag installation lacks declared training evidence:{role}") matrices = np.asarray([value[1] for value in unique.values()]) rotation = Rotation.from_quat(robust_rotation_summary( Rotation.from_matrix(matrices[:, :3, :3]).as_quat().tolist())[0]) @@ -113,7 +120,7 @@ def fit_tag_installations( transform = np.eye(4) transform[:3, :3], transform[:3, 3] = rotation.as_matrix(), translation output[role] = FrozenTagInstallation(role, link_by_role[role], matrix_tuple(transform), - tuple(sorted(unique)), rotation_p95, translation_p95) + tuple(sorted(unique)), rotation_p95, translation_p95, partitions[role]) if any(not row["passed"] for row in diagnostics.values()): raise TagInstallationFailure(diagnostics) return output diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/training_quality.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/training_quality.py new file mode 100644 index 0000000..fb697e9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/training_quality.py @@ -0,0 +1,166 @@ +"""Bounded training decisions; no holdout input and no hardware interface.""" + +from dataclasses import asdict, replace +import hashlib +from itertools import combinations +import math +from pathlib import Path + +import numpy as np +from scipy.stats import t as student_t + +from ..domain.capture_plan import CapturePlan, evidence_digest, select_task_training, training_cycles +from ..domain.motion_path import record_scan_identity +from ..domain.sampling import command_nodes +from ..urdf.kinematics import UrdfKinematicModel +from ..urdf.acceptance import angular_metrics +from .command_motion import fit_command_training +from .spatial import measure_joint_curve_observation + + +TRAINING_DECISION_VERSION = "training_decision_v1" +# Zero confidence uses the production spatial solver's unchanged limit. +MAXIMUM_CONFIDENCE_HALF_WIDTH_RAD = math.radians(1.) +TRAINING_FIELDS = ("kind", "joint", "view", "task_name", "cycle", "direction", "segment_key", + "attempt", "sample_phase", "image_stamp_ns", "motor_index", "command_u8", "command_rad", + "feedback_u8", "feedback_rad", "steady_index", "steady_target", "relative_quaternion_xyzw", + "relative_translation_xyz_m", "parent_pose_common", "child_pose_common", "view_normal_common_xyz", + "camera_center_common_xyz_m", "state_u8", "state_rad", "command_vector_u8", "command_vector_rad") + + +def training_snapshot(profile, records, task_key, cycles): + """Latest attempts only. Validation rows are excluded before all statistics.""" + chosen = select_task_training(profile, task_key, cycles) + allowed = {task.key: training_cycles(chosen, task) for task in chosen.motion.tasks} + latest = {} + for row in records: + if row.get("task_name") in allowed and row.get("cycle") in allowed[row["task_name"]]: + key = record_scan_identity(row) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + rows = [] + for row in records: + if (row.get("kind") != "joint_sample" or row.get("task_name") not in allowed + or row.get("cycle") not in allowed[row["task_name"]] + or row.get("sample_phase", "sweep") not in {"sweep", "steady"} + or int(row.get("attempt", 1)) != latest[record_scan_identity(row)]): + continue + rows.append({key: row[key] for key in TRAINING_FIELDS if key in row}) + return tuple(rows) + + +def _by_joint(rows, phase): + result = {} + for row in rows: + if row.get("sample_phase", "sweep") == phase: + result.setdefault(row["joint"], []).append({**row, + "sample_id": f"{row['view']}:{row['image_stamp_ns']}"}) + return result + + +def _curve_repeatability(profile, task, motion, steady, cycles): + failures, metrics = [], {} + for name in task.joints: + curve = motion.coordinate_curves[name] + values, by_direction = [], {} + for direction in ("increasing", "decreasing"): + directional_values = [] + for node in command_nodes(profile, task): + repeated = [] + for cycle in cycles: + rows = [row for row in steady[name] if row["cycle"] == cycle + and row["direction"] == direction + and math.isclose(float(row["steady_target"]), node, abs_tol=1e-9)] + repeated.append(float(np.median([measure_joint_curve_observation(curve, + quaternion_xyzw=row["relative_quaternion_xyzw"]) for row in rows]))) + values.append(repeated) + directional_values.append(repeated) + array = np.asarray(directional_values) + errors = np.concatenate([array[:, a]-array[:, b] for a, b in combinations(range(len(cycles)), 2)]) + by_direction[direction] = angular_metrics(errors) + array = np.asarray(values) + spread = float(np.max(np.ptp(array, axis=1))) + width = float(np.max(student_t.ppf(.975, len(cycles)-1) + * np.std(array, axis=1, ddof=1) / math.sqrt(len(cycles)))) + metrics[name] = {"maximum_cycle_difference_rad": spread, + "maximum_angle_confidence_half_width_rad": width, + "independent_cycle_count": len(cycles), + "repeatability_by_direction": {key: asdict(value) for key, value in by_direction.items()}} + # Curve consistency is an angular error gate, not a zero-offset CI. + # Applying the 1-degree zero CI limit at every command knot would + # introduce a stricter, unrelated rejection of otherwise sound data. + if not all(value.passed for value in by_direction.values()): + failures.append(f"training_repeatability:{name}") + return failures, metrics + + +def assess_training(profile, task_key, cycles, rows, references, source_urdf): + """Assess an immutable training snapshot. Missing future geometry adds a round. + +The local angle confidence is NOT a zero-offset certificate. When the full +spatial dependency graph is available, the production training-only solver +also checks real zero confidence. Otherwise the conservative third round is +retained and the final spatial gate remains mandatory. +""" + cycles = tuple(cycles) + chosen = select_task_training(profile, task_key, cycles) + task = next(t for t in profile.motion.tasks if t.key == task_key) + if any(row["cycle"] not in training_cycles(chosen, row["task_name"]) for row in rows): + raise ValueError("training_assessment_received_nontraining_data") + sweep, steady = _by_joint(rows, "sweep"), _by_joint(rows, "steady") + reference_inputs = {name: {"baseline_command": ref.baseline_command, + "quaternion": ref.quaternion(profile.measurement.measurements[name].view).tolist()} + for name, ref in references.items() if name in sweep} + source_hash = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() if source_urdf is not None else "" + source = {"rows": rows, "references": reference_inputs, "source_urdf": source_hash} + failures, metrics = [], {} + fitted_model = None + geometry = {"status": "deferred", "reason": "spatial_dependencies_not_complete"} + try: + if source_urdf is None: + raise ValueError("training_source_urdf_missing") + model = UrdfKinematicModel(source_urdf) + local = fit_command_training(chosen, model, + {name: sweep[name] for name in task.joints}, {name: steady[name] for name in task.joints}, + {name: references[name] for name in task.joints}) + fitted_model = {"curves": {name: asdict(curve) for name, curve in local.coordinate_curves.items()}, + "mappings": {name: asdict(mapping) for name, mapping in local.mappings.items()}} + failures, metrics = _curve_repeatability(chosen, task, local, steady, cycles) + required = set(profile.zero.axis_joints) + ready = required <= sweep.keys() & steady.keys() & references.keys() + if ready: + from .session import compile_spatial_profile, build_axis_observations + from .spatial_solver.solve import fit_urdf_zero_training + motion = fit_command_training(chosen, model, {name: sweep[name] for name in required}, + {name: steady[name] for name in required}, {name: references[name] for name in required}) + zero_profile = compile_spatial_profile(chosen) + axes = build_axis_observations(chosen, model, steady, motion, zero_profile, include_holdout=False) + baselines = {**{name: value.cad_angle_rad for name, value in profile.zero.known_baseline_geometry.items()}, + **{name: 0. for name in profile.zero.cad_zero_assumptions}} + curves = {name: replace(curve, circle={**curve.circle, "known_cad_baseline_rad": baselines[name]}) + if name in baselines and name not in zero_profile.direct_zero_joints else curve + for name, curve in motion.coordinate_curves.items()} + from .motion_fit import channel_for_joint + zero = fit_urdf_zero_training(source_urdf=source_urdf, measurements=axes, + training_cycles=tuple(sorted({a.cycle for a in axes})), curves=curves, + motor_by_joint={name: channel_for_joint(profile, name) for name in curves}, + zero_profile=zero_profile, maximum_offset_rad=math.radians(20), + finger_maximum_offset_rad=math.radians(20), + maximum_confidence_half_width_rad=MAXIMUM_CONFIDENCE_HALF_WIDTH_RAD, + maximum_pose_axis_line_rms_m=.0015, allow_partial_training_cycles=True) + geometry = {"status": "checked", "zero_offsets_rad": dict(zero.offsets), + "zero_confidence_half_width_rad": {name: width if math.isfinite(width) else None + for name, width in zero.confidence_half_widths.items()}, + "independent_cycles_by_joint": {name: len(values) for name, values in zero.cycle_values.items()}} + failures.extend(zero.failures) + except (ValueError, KeyError, np.linalg.LinAlgError) as error: + failures.append(str(error)) + decision = ("add_training" if len(cycles) == 2 and (failures or geometry["status"] == "deferred") + else "fail" if failures else "freeze") + record = {"kind": "training_decision", "version": TRAINING_DECISION_VERSION, + "task_name": task_key, "assessed_cycles": list(cycles), "decision": decision, + "failures": failures, "metrics": metrics, "geometry": geometry, + "frozen_training_sha256": (evidence_digest({"local_model": fitted_model, "geometry": geometry}) + if decision == "freeze" else None), + "source_sha256": evidence_digest(source), "source_count": len(rows), + "input_plan_sha256": CapturePlan.from_profile(profile).sha256} + return {**record, "decision_sha256": evidence_digest(record)} diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py index 64cd8e9..7eb690f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py @@ -15,6 +15,7 @@ from .tag_pose.ippe import fixed_pose_reprojection_error from .tag_pose.branch_confirmation import BranchConfirmation from .tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS, FROZEN_BRANCH_POLICY_VERSIONS from .tag_pose.types import SquareTagPose +from .tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence from .image_motion_replay import IMAGE_POSE_SOURCE, has_image_model, replay_image_models, validate_image_sample POLICY = 'training_frozen_articulated_candidates_v1' @@ -83,7 +84,7 @@ def _check_frozen_paths(rows, evidence, selected_paths, role_pairs, fixed_roles) if not isinstance(item, Mapping) or item.get('tag_role') != role: raise ValueError(f'frozen_branch_evidence_missing:{identity}:role={role}') source = item.get('pose_source') - fixed_source = source in {'cached_fixed_reference', 'verified_fixed_reference'} + fixed_source = source in {'cached_fixed_reference', 'verified_fixed_reference', IMAGE_REFERENCE_SOURCE} if (fixed_source != bool(frame['roles'][role].get('locked_reference'))): raise ValueError(f'frozen_branch_pose_source_changed:{identity}:role={role}') @@ -93,7 +94,7 @@ def _check_frozen_paths(rows, evidence, selected_paths, role_pairs, fixed_roles) revision = item.get('reference_branch_revision') else: diagnostics = item.get('candidate_diagnostics', {}) - if (source not in {'current_image', 'verified_fixed_reference', IMAGE_POSE_SOURCE} or not isinstance(diagnostics, Mapping) + if (source not in {'current_image', 'verified_fixed_reference', IMAGE_POSE_SOURCE, IMAGE_REFERENCE_SOURCE} or not isinstance(diagnostics, Mapping) or diagnostics.get('observation_stamp_ns') != stamp or diagnostics.get('branch_status') != 'tracking' or diagnostics.get('branch_frozen') is not True): @@ -108,6 +109,12 @@ def _check_frozen_paths(rows, evidence, selected_paths, role_pairs, fixed_roles) original = _recorded_pose(item.get('selected_pose')) if source == 'verified_fixed_reference': _check_fixed_projection(original, item, frame, role, identity) + if source == IMAGE_REFERENCE_SOURCE: + data = frame['roles'][role] + reference = reference_from_evidence(item, stamp_ns=stamp, camera_matrix=frame['camera_matrix']) + recorded_ref = reference_from_evidence(data, stamp_ns=stamp, camera_matrix=frame['camera_matrix']) + if reference != recorded_ref or not np.array_equal(item.get('corners_xy'), data.get('corners_xy')): + raise ValueError(f'frozen_branch_image_reference_changed:{identity}:role={role}') recorded = _recorded_pose(frame['roles'][role].get('selected')) if not equivalence.equivalent(original, recorded): raise ValueError(f'frozen_branch_recorded_pose_changed:{identity}:role={role}') @@ -135,7 +142,7 @@ def _check_fixed_projection(pose, item, frame, role, identity): def resolve_chain_observations(records, *, task_key, role_pairs, projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=(), - require_frozen_branches=False): + require_frozen_branches=False, training_cycles=(0, 1, 2), holdout_cycle=3): """Return copied observations and auditable candidate-selection evidence. Older captures did not save P. They remain on the legacy path; K is never @@ -156,7 +163,7 @@ def resolve_chain_observations(records, *, task_key, role_pairs, evidence = {int(r['image_stamp_ns']): r for r in rows if ('roles' in r or str(r.get('kind', '')).endswith('pnp_candidate_frame')) and r.get('task_name') == task_key} - report = {'policy': POLICY, 'training_cycles': [0, 1, 2], 'holdout_cycle': 3, + report = {'policy': POLICY, 'training_cycles': list(training_cycles), 'holdout_cycle': holdout_cycle, 'projection_override_used': projection_override is not None, 'is_accuracy_certificate': False} if not evidence: @@ -176,8 +183,10 @@ def resolve_chain_observations(records, *, task_key, role_pairs, stamps = sorted({int(r['image_stamp_ns']) for r in chosen_rows}) if not stamps or any(s not in evidence for s in stamps): raise ValueError('corner replay lacks evidence for accepted observations') - training = np.asarray([int(evidence[s]['cycle']) in (0, 1, 2) for s in stamps]) - if sum(training) < 40 or not any(int(evidence[s]['cycle']) == 3 for s in stamps): + if any(int(evidence[s]['cycle']) not in (*training_cycles, holdout_cycle) for s in stamps): + raise ValueError('corner replay observation outside capture plan') + training = np.asarray([int(evidence[s]['cycle']) in training_cycles for s in stamps]) + if sum(training) < 40 or not any(int(evidence[s]['cycle']) == holdout_cycle for s in stamps): raise ValueError('corner replay requires training and independent holdout') if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]: raise ValueError('holdout must follow the complete training trajectory') diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/frozen_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/frozen_evidence.py new file mode 100644 index 0000000..6526b32 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/frozen_evidence.py @@ -0,0 +1,98 @@ +"""Share immutable, content-bound image models without copying them per frame. + +The JSON representation and its hash are unchanged. Per-image diagnostics stay +independent mutable dictionaries; only the already-frozen model is shared. +""" +from functools import cached_property, lru_cache +import hashlib +import json + + +class FrozenEvidence(dict): + @cached_property + def canonical_json(self): + return json.dumps(self, sort_keys=True, separators=(',', ':'), allow_nan=False) + + @cached_property + def sha256(self): + return hashlib.sha256(self.canonical_json.encode()).hexdigest() + + def _immutable(self, *args, **kwargs): + raise TypeError('frozen evidence cannot be changed') + + __setitem__ = __delitem__ = clear = pop = popitem = setdefault = update = __ior__ = _immutable + + def __deepcopy__(self, memo): + return self + + def __reduce__(self): + return (freeze_evidence, (dict(self),)) + + +def freeze_evidence(value): + if isinstance(value, dict): + return FrozenEvidence((key, freeze_evidence(item)) for key, item in value.items()) + if isinstance(value, (list, tuple)): + return tuple(freeze_evidence(item) for item in value) + return value + + +def canonical_evidence(value): + """Cache serialization only for recursively immutable evidence.""" + if isinstance(value, FrozenEvidence): + return value.canonical_json + return json.dumps(value, sort_keys=True, separators=(',', ':'), allow_nan=False) + + +def evidence_sha256(value): + if isinstance(value, FrozenEvidence): + return value.sha256 + return hashlib.sha256(canonical_evidence(value).encode()).hexdigest() + + +@lru_cache(maxsize=64) +def image_model_evidence(model): + """The model dataclasses contain immutable tuples and are value-hashable.""" + from .tag_pose.image_motion_model import image_model_payload + return freeze_evidence(image_model_payload(model)) + + +class EvidenceDecoder: + """Intern verified models while streaming old and new JSONL records.""" + def __init__(self, *, verify_hashes=True): + self.models = {} + self.references = {} + self.verify_hashes = verify_hashes + + def _intern(self, payload, identity, cache, error): + if not self.verify_hashes: + # Passed-task resume defers replay to finalization. Sharing must + # still preserve the exact content, even for conflicting digests. + previous = cache.get(identity) + if previous is not None and payload == previous[0]: + return previous[1] + frozen = freeze_evidence(payload) + if previous is None: + cache[identity] = (payload, frozen) + return frozen + digest = hashlib.sha256(json.dumps(payload, sort_keys=True, + separators=(',', ':'), allow_nan=False).encode()).hexdigest() + if identity != digest: + raise ValueError(error) + if identity not in cache: + cache[identity] = freeze_evidence(payload) + return cache[identity] + + def object_hook(self, row): + payload = row.get('frozen_image_model') + if payload is not None: + row['frozen_image_model'] = self._intern(payload, row.get('image_model_sha256'), + self.models, 'image_motion_provenance_model_hash_invalid') + reference = row.get('reference_frame') + if reference is not None: + row['reference_frame'] = self._intern(reference, row.get('reference_frame_sha256'), + self.references, 'image_reference_identity_changed') + return row + + def loads(self, line): + return json.loads(line, object_hook=self.object_hook) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py index 997a747..a95d10e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/image_motion_replay.py @@ -6,33 +6,39 @@ Raw IPPE candidates remain separate from the selected constrained poses. """ from collections.abc import Mapping -import hashlib +from functools import lru_cache import json import numpy as np from scipy.spatial.transform import Rotation +from .frozen_evidence import canonical_evidence, evidence_sha256 from .tag_pose.motion_evidence import MotionEvidenceFrame, RoleCandidates from .tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation from .tag_pose.types import SquareTagPose +from .tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence IMAGE_POSE_SOURCE = "image_constrained_current_image" def image_model_sha256(payload): - encoded = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False) - return hashlib.sha256(encoded.encode()).hexdigest() + return evidence_sha256(payload) + + +@lru_cache(maxsize=64) +def _read_image_model(encoded): + from .tag_pose.production_image_motion import image_motion_model_from_dict + + return image_motion_model_from_dict(json.loads(encoded)) def frozen_image_model(item): """Read a named, content-bound model; a marker alone grants no permission.""" - from .tag_pose.production_image_motion import image_motion_model_from_dict - payload, identity = item.get("frozen_image_model"), item.get("image_model_sha256") if not isinstance(payload, Mapping) or image_model_sha256(payload) != identity: raise ValueError("image_motion_provenance_model_hash_invalid") - return image_motion_model_from_dict(payload) + return _read_image_model(canonical_evidence(payload)) def has_image_model(item): @@ -82,6 +88,7 @@ def replay_image_models(frame): or not np.array_equal(np.asarray(frame["camera_matrix"]), model.camera_matrix)): raise ValueError("image_motion_provenance_model_not_frozen_before_image") children = {geometry.relation.child_role for geometry in model.geometry} + references = {ref.role: ref for ref in model.reference_frames} roles, observations = [], [] for role, size in model.tag_sizes: item = frame["roles"].get(role) @@ -98,11 +105,16 @@ def replay_image_models(frame): # A root may itself be an earlier frozen moving parent. Its # current selected pose is used, never its previous image. diagnostics = item.get("candidate_diagnostics", {}) - if (item.get("pose_source") not in {"verified_fixed_reference", IMAGE_POSE_SOURCE} + if (item.get("pose_source") not in {"verified_fixed_reference", IMAGE_POSE_SOURCE, IMAGE_REFERENCE_SOURCE} or diagnostics.get("branch_frozen") is not True or diagnostics.get("branch_status") != "tracking" or diagnostics.get("observation_stamp_ns") != frame["image_stamp_ns"]): raise ValueError("image_motion_provenance_root_not_frozen_in_current_image") + if role in references or item.get("pose_source") == IMAGE_REFERENCE_SOURCE: + reference = reference_from_evidence(item, stamp_ns=frame["image_stamp_ns"], + camera_matrix=frame["camera_matrix"]) + if reference != references.get(role): + raise ValueError("image_motion_provenance_reference_binding_changed") candidates = (_pose(item["selected"]),) points = item.get("corners_xy") if points is not None: @@ -115,7 +127,7 @@ def replay_image_models(frame): roles.append(RoleCandidates(role, candidates, role not in children)) result = select_image_motion_frame(model, ImageMotionFrame( MotionEvidenceFrame(frame["image_stamp_ns"], tuple(roles)), - tuple(map(tuple, frame["camera_matrix"])), tuple(observations))) + tuple(map(tuple, frame["camera_matrix"])), tuple(observations), model.reference_frames)) if not result.resolved: raise ValueError(f"image_motion_provenance_frame_unresolved:{result.reason}") by_role = {item.role: item.pose for item in result.selections} @@ -149,6 +161,13 @@ def validate_image_sample(row, frame, replayed): raise ValueError("image_motion_provenance_sample_model_or_corners_changed") elif has_image_model(data): raise ValueError("image_motion_provenance_sample_solver_marker_removed") + if IMAGE_REFERENCE_SOURCE in (item.get("pose_source"), data.get("pose_source")): + sample_ref = reference_from_evidence(item, stamp_ns=row["image_stamp_ns"], + camera_matrix=frame["camera_matrix"]) + frame_ref = reference_from_evidence(data, stamp_ns=frame["image_stamp_ns"], + camera_matrix=frame["camera_matrix"]) + if sample_ref != frame_ref or not np.array_equal(item.get("corners_xy"), data.get("corners_xy")): + raise ValueError("image_motion_provenance_sample_reference_changed") parent = _pose(evidence["parent"]["selected_pose"]) child = _pose(evidence["child"]["selected_pose"]) rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py index cff1135..3d85c4a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_hinge.py @@ -12,6 +12,7 @@ import numpy as np from scipy.spatial.transform import Rotation from .motion_image_solver import ImageHingeBundle +from .source_hinge import SourceHinge @dataclass(frozen=True) @@ -38,17 +39,25 @@ class CadImageHingeBundle(ImageHingeBundle): uncertainty and dependencies on the parent's installation parameters. """ - def __init__(self, frames, relations, poses, *, constraints, source_urdf_sha256): + def __init__(self, frames, relations, poses, *, constraints, source_urdf_sha256, source_hinges=()): super().__init__(frames, relations, poses) self.constraints = tuple(constraints) self.source_urdf_sha256 = source_urdf_sha256 + self.source_hinges = tuple(source_hinges) + sources = {item.geometry.relation.joint: item for item in self.source_hinges} + if len(sources) != len(self.source_hinges): + raise ValueError("source_hinge_duplicate") indices = {relation.joint: i for i, relation in enumerate(relations)} + if set(sources) & set(indices) or not set(sources) <= {c.parent_joint for c in self.constraints}: + raise ValueError("source_hinge_unused_or_overlapping") self._constraints = {} for constraint in self.constraints: - parent = indices.get(constraint.parent_joint) + parent = indices.get(constraint.parent_joint, sources.get(constraint.parent_joint)) child = indices.get(constraint.child_joint) - if (parent is None or child is None or parent >= child - or relations[child].parent_role != relations[parent].child_role + parent_relation = (parent.geometry.relation if isinstance(parent, SourceHinge) + else None if parent is None else relations[parent]) + if (parent is None or child is None or isinstance(parent, int) and parent >= child + or relations[child].parent_role != parent_relation.child_role or child in self._constraints): raise ValueError("parallel_axis_constraint_outside_observed_chain") self._constraints[child] = (parent, constraint) @@ -58,7 +67,7 @@ class CadImageHingeBundle(ImageHingeBundle): self._columns = {original: column for column, original in enumerate(self._kept)} for index, (parent, _) in sorted(self._constraints.items()): parameters = self._full_initial[self._kept] - _, _, _, parent_mount = self.geometry_values(parameters, parent) + _, _, _, parent_mount = self._parent_values(parameters, parent) _, _, old_pivot, _ = super().geometry_values(self._full_initial, index) tangent = self.axis_basis(parameters, index) vector = parent_mount + old_pivot @@ -77,10 +86,28 @@ class CadImageHingeBundle(ImageHingeBundle): def geometry_dependency_indices(self, index): result = self.geometry_parameter_indices(index) - if index in self._constraints: + if index in self._constraints and isinstance(self._constraints[index][0], int): result += self.geometry_dependency_indices(self._constraints[index][0]) return result + def _parent_values(self, parameters, parent): + if isinstance(parent, int): + return self.geometry_values(parameters, parent) + g = parent.geometry + return (np.asarray(g.axis_parent_xyz), Rotation.from_quat(g.reference_quaternion_xyzw), + np.asarray(g.pivot_parent_xyz_m), np.asarray(g.mount_child_xyz_m)) + + def inherited_uncertainty(self, index): + if index not in self._constraints: + return 0., 0. + parent, constraint = self._constraints[index] + if isinstance(parent, int): + axis, point = self.inherited_uncertainty(parent) + return axis, point + constraint.distance_m * axis + axis = parent.child_axis_uncertainty_rad + # Triangle bound includes the lever arm affected by inherited tilt. + return axis, parent.child_point_uncertainty_m + constraint.distance_m * axis + def _expand(self, parameters): full = self._full_initial.copy() full[self._kept] = parameters @@ -91,8 +118,10 @@ class CadImageHingeBundle(ImageHingeBundle): values = self._expand(parameters)[10*index:10*index+2] return Rotation.from_rotvec(self.bases[index] @ values).as_matrix() @ self.bases[index] parent, constraint = self._constraints[index] - _, reference, _, _ = self.geometry_values(parameters, parent) - basis = reference.inv().as_matrix() @ self.axis_basis(parameters, parent) + axis, reference, _, _ = self._parent_values(parameters, parent) + from .motion_image_solver import _basis + parent_basis = self.axis_basis(parameters, parent) if isinstance(parent, int) else _basis(axis) + basis = reference.inv().as_matrix() @ parent_basis basis[:, 1] *= constraint.axis_sign return basis @@ -101,7 +130,7 @@ class CadImageHingeBundle(ImageHingeBundle): if index not in self._constraints: return super().geometry_values(full, index) parent, constraint = self._constraints[index] - parent_axis, parent_reference, _, parent_mount = self.geometry_values(parameters, parent) + parent_axis, parent_reference, _, parent_mount = self._parent_values(parameters, parent) axis = constraint.axis_sign * parent_reference.inv().apply(parent_axis) values = full[10*index:10*(index+1)] reference = Rotation.from_rotvec(values[2:5]) * self.references[index] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py index 746ab90..53d5402 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/cad_image_model.py @@ -8,8 +8,10 @@ from scipy.spatial.transform import Rotation from .cad_hinge import ParallelAxisGeometry from .image_motion_model import ImageMotionModel +from .source_hinge import SourceHinge CAD_IMAGE_MOTION_MODEL_POLICY = "image_hinge_v2_source_parallel_geometry" +TRANSFERRED_IMAGE_MOTION_MODEL_POLICY = "image_hinge_v3_transferred_parallel_geometry" @dataclass(frozen=True) @@ -26,13 +28,20 @@ class CadImageMotionModel(ImageMotionModel): or any(c not in "0123456789abcdef" for c in self.source_urdf_sha256)): raise ValueError("image_model_source_geometry_missing") geometry = {item.relation.joint: item for item in self.geometry} + measured = set(geometry) + sources = getattr(self, "source_hinges", ()) + if len({item.geometry.relation.joint for item in sources}) != len(sources): + raise ValueError("image_model_source_hinge_duplicate") + if any(item.geometry.relation.joint in geometry for item in sources): + raise ValueError("image_model_source_hinge_overlaps_current") + geometry.update((item.geometry.relation.joint, item.geometry) for item in sources) children = set() for constraint in self.constraints: if not isinstance(constraint, ParallelAxisGeometry): raise ValueError("image_model_source_geometry_invalid") parent, child = geometry.get(constraint.parent_joint), geometry.get(constraint.child_joint) if (parent is None or child is None or parent.relation.child_role != child.relation.parent_role - or constraint.child_joint in children): + or constraint.child_joint in children or constraint.child_joint not in measured): raise ValueError("image_model_source_geometry_graph_changed") children.add(constraint.child_joint) axis = Rotation.from_quat(parent.reference_quaternion_xyzw).inv().apply(parent.axis_parent_xyz) @@ -42,3 +51,19 @@ class CadImageMotionModel(ImageMotionModel): distance = np.linalg.norm(vector - axis * (axis @ vector)) if not np.isclose(distance, constraint.distance_m, atol=1e-8, rtol=0): raise ValueError("image_model_source_axis_distance_changed") + + +@dataclass(frozen=True) +class TransferredImageMotionModel(CadImageMotionModel): + POLICY: ClassVar[str] = TRANSFERRED_IMAGE_MOTION_MODEL_POLICY + policy: str = TRANSFERRED_IMAGE_MOTION_MODEL_POLICY + source_hinges: tuple[SourceHinge, ...] = () + + def __post_init__(self): + if not isinstance(self.source_hinges, tuple) or not self.source_hinges or any( + not isinstance(item, SourceHinge) for item in self.source_hinges): + raise ValueError("image_model_source_hinge_missing") + super().__post_init__() + parents = {c.parent_joint for c in self.constraints} + if any(item.geometry.relation.joint not in parents for item in self.source_hinges): + raise ValueError("image_model_source_hinge_unused") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py new file mode 100644 index 0000000..d1452b6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/hinge_uncertainty.py @@ -0,0 +1,63 @@ +"""Local image-fit uncertainty in both parent and physical child-Tag frames.""" + +import numpy as np + + +def unconstrained_hinge_linearization(bundle, fit, angles): + """Restore full identifiable geometry at identical pixels, without refitting.""" + from scipy.optimize import least_squares + from scipy.spatial.transform import Rotation + from .motion_image_solver import ImageHingeBundle, _basis + + free = ImageHingeBundle(bundle.frames, bundle.relations, bundle.poses) + parameters = [] + for index in range(bundle.joints): + axis, reference, pivot, mount = bundle.geometry_values(fit.x, index) + free.axes[index], free.bases[index] = axis, _basis(axis) + free.references[index] = Rotation.from_rotvec(axis*angles[0, index])*reference + parameters.extend((0., 0., 0., 0., 0., *(free.bases[index].T @ pivot), *mount)) + parameters = np.r_[parameters, (angles[1:]-angles[0]).ravel()] + linearization = least_squares(free.residual, parameters, jac="3-point", + x_scale=free.scale, max_nfev=1) + return free, linearization + + +def hinge_uncertainty(bundle, fit): + if hasattr(bundle, "uncertainty_problem"): + bundle, fit = bundle.uncertainty_problem(fit) + jacobian = fit.jac.toarray() if hasattr(fit.jac, "toarray") else np.asarray(fit.jac) + scaled = jacobian * bundle.scale[None, :] + _, singular, vh = np.linalg.svd(scaled, full_matrices=False) + if singular[-1] <= singular[0] * np.finfo(float).eps * max(scaled.shape): + raise ValueError("image_motion_geometry_rank_deficient") + variance = max(.03 ** 2, float(np.sum(fit.fun ** 2) / max(1, len(fit.fun) - len(fit.x)))) + covariance = ((vh.T / singular ** 2) @ vh * variance) * bundle.scale[:, None] * bundle.scale[None, :] + axes, pivots, child_bounds = [], [], [] + + def coordinates(parameters, index): + axis, reference, pivot, mount = bundle.geometry_values(parameters, index) + child_axis = reference.inv().apply(axis) + # Closest point on the axis in the Tag frame; independent of axial + # pivot gauge, joint angle, and the upstream holding posture. + child_point = -mount + child_axis * (child_axis @ mount) + return np.r_[axis, pivot, child_axis, child_point] + + for index, relation in enumerate(bundle.relations): + derivative = np.zeros((12, len(fit.x))) + for column in bundle.geometry_dependency_indices(index): + step = 1e-5 * bundle.scale[column] + upper, lower = fit.x.copy(), fit.x.copy() + upper[column] += step + lower[column] -= step + derivative[:, column] = (coordinates(upper, index) - coordinates(lower, index)) / (2 * step) + propagated = derivative @ covariance @ derivative.T + bounds = [float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[i:i+3, i:i+3])[-1]))) + for i in (0, 3, 6, 9)] + # A transferred parent is measured, not exact CAD. Keep conservative + # inherited bounds explicit instead of reporting zero axis uncertainty + # merely because that axis was removed from the child optimization. + inherited = bundle.inherited_uncertainty(index) if hasattr(bundle, "inherited_uncertainty") else (0., 0.) + axes.append((relation.joint, bounds[0] + inherited[0])) + pivots.append((relation.joint, bounds[1] + inherited[1])) + child_bounds.append((relation.joint, bounds[2] + inherited[0], bounds[3] + inherited[1])) + return tuple(axes), tuple(pivots), tuple(child_bounds) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py index 75498a8..1ff71f8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_commit.py @@ -9,7 +9,9 @@ from __future__ import annotations from contextlib import ExitStack from collections.abc import Mapping, Sequence -from dataclasses import asdict, dataclass +from ..frozen_evidence import image_model_evidence + +from dataclasses import dataclass import hashlib import json from threading import RLock @@ -17,6 +19,7 @@ from weakref import WeakValueDictionary from typing import TYPE_CHECKING from .image_motion_model import ImageMotionModel +from .image_reference import ImageReferenceSnapshot from .image_motion_projection import select_image_motion_frame from .image_projection_worker import ImageProjectionWorker from .motion_image_types import ImageMotionFrame, ImageRoleQuality @@ -30,7 +33,7 @@ class VerifiedImageMotionDecision: accepted: bool reason: str selections: tuple[MotionPoseSelection, ...] = () - root_snapshots: tuple[MotionTrackerSnapshot, ...] = () + root_snapshots: tuple[MotionTrackerSnapshot | ImageReferenceSnapshot, ...] = () model: ImageMotionModel | None = None model_sha256: str = "" quality: tuple[ImageRoleQuality, ...] = () @@ -41,6 +44,8 @@ _registry_lock = RLock() def _root_current(snapshot): + if isinstance(snapshot, ImageReferenceSnapshot): + return snapshot.tracker.snapshot_current(snapshot) tracker, role = snapshot.tracker, snapshot.role state = tracker._branches.states.get(role) diagnostics = tracker.last_candidate_diagnostics_by_role.get(role, {}) @@ -77,7 +82,7 @@ def _inputs_match(snapshots, roots, model, frame): def verify_image_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], model: ImageMotionModel, frame: ImageMotionFrame, evidence_ids: Mapping[str, str], *, - root_snapshots: Sequence[MotionTrackerSnapshot] = (), + root_snapshots: Sequence[MotionTrackerSnapshot | ImageReferenceSnapshot] = (), projection_worker=None) -> VerifiedImageMotionDecision: """Calculate a decision once; never accept caller-proposed output poses.""" from .tracking import MotionPoseSelection, SquareTagPoseTracker @@ -103,6 +108,10 @@ def verify_image_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], model: roots = tuple(collected) if {s.role for s in roots} != root_roles or len(roots) != len(root_roles): return VerifiedImageMotionDecision(False, "pose_image_motion_root_missing") + declared = {ref.role: ref for ref in model.reference_frames} + actual = {s.role: s.reference for s in roots if isinstance(s, ImageReferenceSnapshot)} + if actual != declared: + return VerifiedImageMotionDecision(False, "pose_image_motion_reference_binding_changed") if not _inputs_match(snapshots, roots, model, frame): return VerifiedImageMotionDecision(False, "pose_image_motion_observation_mismatch") with ExitStack() as stack: @@ -117,7 +126,7 @@ def verify_image_motion_frame(snapshots: Sequence[MotionTrackerSnapshot], model: if not result.resolved: return VerifiedImageMotionDecision(False, result.reason, quality=result.quality) poses = {item.role: item.pose for item in result.selections} - identity = hashlib.sha256(json.dumps(asdict(model), sort_keys=True, separators=(",", ":"), + identity = hashlib.sha256(json.dumps(image_model_evidence(model), sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() selections = tuple(MotionPoseSelection(s, poses[s.role], evidence_ids[s.role]) for s in snapshots) decision = VerifiedImageMotionDecision(True, "", selections, roots, model, identity, result.quality) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py index 6a68cf3..65ea462 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_model.py @@ -3,16 +3,21 @@ from __future__ import annotations from dataclasses import dataclass import math -from typing import ClassVar, Mapping +from typing import ClassVar, Mapping, TYPE_CHECKING import numpy as np from .motion_evidence import MotionRelation, MotionRoleSelection from .motion_image_types import ImageHingeGeometry, ImageRoleQuality from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS +from .image_reference import StationaryImageReference, image_reference_from_dict +from .pose_bridge import PoseBridgeCheck IMAGE_MOTION_MODEL_POLICY = "image_hinge_v1_pre_zero_frozen" +if TYPE_CHECKING: + from .measured_transfer import TransferGeometryCheck + @dataclass(frozen=True) class ImageMotionModel: @@ -27,6 +32,7 @@ class ImageMotionModel: maximum_reprojection_error_px: float = 1.5 reprojection_tie_px: float = .03 policy: str = IMAGE_MOTION_MODEL_POLICY + reference_frames: tuple[StationaryImageReference, ...] = () @property def relations(self): @@ -55,6 +61,16 @@ class ImageMotionModel: or not np.array_equal(matrix[2], [0, 0, 1])): raise ValueError("image_motion_model_camera_invalid") roles = {r for g in self.geometry for r in (g.relation.parent_role, g.relation.child_role)} + roots = roles - {g.relation.child_role for g in self.geometry} + if (not isinstance(self.reference_frames, tuple) + or any(not isinstance(ref, StationaryImageReference) for ref in self.reference_frames) + or len({ref.role for ref in self.reference_frames}) != len(self.reference_frames) + or any(ref.role not in roots or ref.camera_matrix != self.camera_matrix + or dict(self.tag_sizes).get(ref.role) != ref.tag_size_m + or not self.source_stamps_ns + or ref.source_images[-1].stamp_ns >= min(self.source_stamps_ns) + for ref in self.reference_frames)): + raise ValueError("image_motion_reference_frames_invalid") if (len(dict(self.tag_sizes)) != len(self.tag_sizes) or set(dict(self.tag_sizes)) != roles or any(not math.isfinite(s) or s <= 0 for _, s in self.tag_sizes)): raise ValueError("image_motion_model_tag_sizes_invalid") @@ -104,6 +120,9 @@ class ImageMotionHypothesis: training_solver_evaluations: int = 0 training_accepted: bool = False comparison_adjusted_p_values: tuple[tuple[int, float], ...] = () + child_frame_uncertainty: tuple[tuple[str, float, float], ...] = () + source_geometry_checks: tuple[TransferGeometryCheck, ...] = () + pose_bridge_checks: tuple[PoseBridgeCheck, ...] = () @dataclass(frozen=True) @@ -121,12 +140,23 @@ def image_motion_model_from_dict(raw: Mapping) -> ImageMotionModel: tuple(item["axis_parent_xyz"]), tuple(item["reference_quaternion_xyzw"]), tuple(item["pivot_parent_xyz_m"]), tuple(item["mount_child_xyz_m"])) for item in raw["geometry"]) model_type, extra = ImageMotionModel, {} - from .cad_image_model import CAD_IMAGE_MOTION_MODEL_POLICY, CadImageMotionModel - if raw["policy"] == CAD_IMAGE_MOTION_MODEL_POLICY: + extra["reference_frames"] = tuple(image_reference_from_dict(item) + for item in raw.get("reference_frames", ())) + from .cad_image_model import (CAD_IMAGE_MOTION_MODEL_POLICY, CadImageMotionModel, + TRANSFERRED_IMAGE_MOTION_MODEL_POLICY, TransferredImageMotionModel) + from .measured_transfer import MEASURED_TRANSFER_POLICY, MeasuredTransferImageMotionModel + if raw["policy"] in {CAD_IMAGE_MOTION_MODEL_POLICY, TRANSFERRED_IMAGE_MOTION_MODEL_POLICY, MEASURED_TRANSFER_POLICY}: from .cad_hinge import ParallelAxisGeometry model_type = CadImageMotionModel - extra = {"constraints": tuple(ParallelAxisGeometry(**value) for value in raw["constraints"]), - "source_urdf_sha256": raw["source_urdf_sha256"]} + extra.update(constraints=tuple(ParallelAxisGeometry(**value) for value in raw["constraints"]), + source_urdf_sha256=raw["source_urdf_sha256"]) + if raw["policy"] in {TRANSFERRED_IMAGE_MOTION_MODEL_POLICY, MEASURED_TRANSFER_POLICY}: + from .source_hinge import source_hinge_from_dict + model_type = TransferredImageMotionModel + extra["source_hinges"] = tuple(source_hinge_from_dict(value) for value in raw["source_hinges"]) + if raw["policy"] == MEASURED_TRANSFER_POLICY: + model_type = MeasuredTransferImageMotionModel + extra["current_geometry_uncertainty"] = tuple(tuple(v) for v in raw["current_geometry_uncertainty"]) return model_type(geometry=geometry, camera_matrix=tuple(tuple(v) for v in raw["camera_matrix"]), tag_sizes=tuple((str(r), float(s)) for r, s in raw["tag_sizes"]), source_stamps_ns=tuple(raw["source_stamps_ns"]), training_stamps_ns=tuple(raw["training_stamps_ns"]), @@ -134,3 +164,12 @@ def image_motion_model_from_dict(raw: Mapping) -> ImageMotionModel: branches=tuple((str(r), i) for r, i in raw["branches"]), maximum_reprojection_error_px=raw["maximum_reprojection_error_px"], reprojection_tie_px=raw["reprojection_tie_px"], policy=raw["policy"], **extra) + + +def image_model_payload(model): + """Preserve historical model hashes when no coordinate reference exists.""" + from dataclasses import asdict + payload = asdict(model) + if payload.get("reference_frames") == (): + payload.pop("reference_frames") + return payload diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py index a75cbcc..93741b2 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py @@ -39,6 +39,9 @@ def _frame_inputs(model, frame): if tuple(tuple(v) for v in frame.camera_matrix) != model.camera_matrix: raise ValueError("image_motion_camera_changed") observations = {o.role: o for o in frame.observations} + if frame.reference_frames != model.reference_frames: + raise ValueError("image_motion_reference_frames_changed") + references = {ref.role: ref for ref in model.reference_frames} candidates = {r.role: r for r in frame.evidence.roles} roles, sizes = tuple(dict(model.tag_sizes)), dict(model.tag_sizes) if (len(observations) != len(frame.observations) or len(candidates) != len(frame.evidence.roles) @@ -60,6 +63,13 @@ def _frame_inputs(model, frame): frozen = {role: poses[role][0] for role in roots} # A frozen root is still verified against this image, never silently moved. for role, pose in frozen.items(): + if role in references: + reference = references[role] + if pose != reference.pose: + raise ValueError("image_motion_reference_pose_changed") + reference.check_image(role, sizes[role], frame.camera_matrix, + observations[role].corners_xy, frame.evidence.stamp_ns) + continue xyz = Rotation.from_quat(pose.quaternion_xyzw).apply(square_object_points(sizes[role])) xyz += pose.translation_xyz_m if np.any(xyz[:, 2] <= 1e-6): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_reference.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_reference.py new file mode 100644 index 0000000..d1cba5b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/image_reference.py @@ -0,0 +1,180 @@ +"""Image stationarity and a coordinate convention, without a Tag normal. + +A fixed planar marker need not have an observable normal to witness that the +camera/fixture is stationary. The coordinate frame below has camera-aligned +axes. Its origin is near the marker solely to condition the hinge fit; it is +not an estimate of the marker's physical orientation. Moving Tags still need +the ordinary independent image/geometry authorization. +""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass +from functools import cached_property, lru_cache +import hashlib +import json + +import numpy as np + +from ..frozen_evidence import canonical_evidence +from .ippe import solve_square_tag_ippe +from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS +from .types import SquareTagPose + + +IMAGE_REFERENCE_SOURCE = "stationary_image_reference" +IMAGE_REFERENCE_POLICY = "camera_aligned_image_reference_v1" + + +def _canonical(value): + return canonical_evidence(value) + + +@dataclass(frozen=True) +class ReferenceImage: + stamp_ns: int + corners_xy: tuple[tuple[float, float], ...] + + def __post_init__(self): + if (type(self.stamp_ns) is not int or self.stamp_ns <= 0 + or not isinstance(self.corners_xy, tuple) + or any(not isinstance(point, tuple) for point in self.corners_xy) + or np.asarray(self.corners_xy).shape != (4, 2) + or not np.all(np.isfinite(self.corners_xy))): + raise ValueError("image_reference_source_image_invalid") + + +@dataclass(frozen=True) +class ImageReferenceSnapshot: + tracker: object + role: str + epoch: int + stamp_ns: int + reference: StationaryImageReference + image_observation: object + + @property + def previous_pose(self): + return self.reference.pose + + +@dataclass(frozen=True) +class StationaryImageReference: + role: str + tag_size_m: float + camera_matrix: tuple[tuple[float, float, float], ...] + source_images: tuple[ReferenceImage, ...] + origin_xyz_m: tuple[float, float, float] + policy: str = IMAGE_REFERENCE_POLICY + + def __post_init__(self): + matrix = np.asarray(self.camera_matrix) + if (self.policy != IMAGE_REFERENCE_POLICY or not isinstance(self.role, str) or not self.role + or not isinstance(self.camera_matrix, tuple) or matrix.shape != (3, 3) + or any(not isinstance(row, tuple) for row in self.camera_matrix) + or not np.all(np.isfinite(matrix)) or matrix[0, 0] <= 0 or matrix[1, 1] <= 0 + or not np.array_equal(matrix[2], [0, 0, 1]) + or not np.isfinite(self.tag_size_m) or self.tag_size_m <= 0): + raise ValueError("image_reference_camera_or_role_invalid") + if (not isinstance(self.source_images, tuple) or len(self.source_images) < 10 + or any(not isinstance(row, ReferenceImage) for row in self.source_images)): + raise ValueError("image_reference_source_images_invalid") + stamps = [row.stamp_ns for row in self.source_images] + points = np.asarray([row.corners_xy for row in self.source_images]) + if (any(type(s) is not int or s <= 0 for s in stamps) or stamps != sorted(set(stamps)) + or points.shape != (len(stamps), 4, 2) or not np.all(np.isfinite(points))): + raise ValueError("image_reference_source_images_invalid") + origin = np.asarray(self.origin_xyz_m) + if (not isinstance(self.origin_xyz_m, tuple) or origin.shape != (3,) + or not np.all(np.isfinite(origin)) or origin[2] <= 0): + raise ValueError("image_reference_coordinate_origin_invalid") + drift = np.sqrt(np.mean(np.sum((points - np.median(points, axis=0)) ** 2, axis=2), axis=1)) + if np.max(drift) > DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px: + raise ValueError("image_reference_source_not_stationary") + + @cached_property + def _median_corners(self): + return tuple(map(tuple, np.median([row.corners_xy for row in self.source_images], axis=0))) + + @property + def corners_xy(self): + return np.asarray(self._median_corners) + + @property + def pose(self): + # The quaternion is a declared coordinate basis, not a measured Tag + # normal. Zero here is the exact coordinate convention's error. + return SquareTagPose((0., 0., 0., 1.), self.origin_xyz_m, 0.) + + @cached_property + def identity(self): + return hashlib.sha256(_canonical(asdict(self)).encode()).hexdigest() + + def check_image(self, role, size, matrix, corners, stamp_ns): + if (role != self.role or size != self.tag_size_m + or not np.array_equal(matrix, self.camera_matrix) + or stamp_ns < self.source_images[-1].stamp_ns): + raise ValueError("image_reference_binding_changed") + points = np.asarray(corners) + if points.shape != (4, 2) or not np.all(np.isfinite(points)): + raise ValueError("image_reference_current_corners_missing") + error = float(np.sqrt(np.mean(np.sum((points - self.corners_xy) ** 2, axis=1)))) + if error > DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px: + raise ValueError("image_reference_current_image_moved") + return error + + +def coordinate_origin(images, size, matrix): + """Use both candidate centres symmetrically; never select a normal.""" + centres = [] + for row in images: + candidates = [p for p in solve_square_tag_ippe(row.corners_xy, + tag_size_m=size, camera_matrix=matrix) + if p.reprojection_error_px <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px] + if not candidates: + raise ValueError("image_reference_source_projection_invalid") + centres.append(np.mean([p.translation_xyz_m for p in candidates], axis=0)) + return tuple(float(v) for v in np.median(centres, axis=0)) + + +@lru_cache(maxsize=128) +def _read_reference(encoded): + raw = json.loads(encoded) + result = StationaryImageReference(raw["role"], float(raw["tag_size_m"]), + tuple(map(tuple, raw["camera_matrix"])), tuple(ReferenceImage(row["stamp_ns"], + tuple(map(tuple, row["corners_xy"]))) for row in raw["source_images"]), + tuple(raw["origin_xyz_m"]), raw["policy"]) + expected = coordinate_origin(result.source_images, result.tag_size_m, result.camera_matrix) + if not np.allclose(result.origin_xyz_m, expected, atol=1e-10, rtol=0): + raise ValueError("image_reference_coordinate_origin_changed") + return result + + +def image_reference_from_dict(raw): + return _read_reference(_canonical(raw)) + + +def reference_from_evidence(item, *, stamp_ns, camera_matrix=None): + """Read the declared coordinate frame and verify its current image.""" + if item.get("pose_source") != IMAGE_REFERENCE_SOURCE: + raise ValueError("image_reference_pose_source_changed") + ref = image_reference_from_dict(item["reference_frame"]) + if item.get("reference_frame_sha256") != ref.identity: + raise ValueError("image_reference_identity_changed") + diagnostics = item.get("candidate_diagnostics", {}) + if (diagnostics.get("observation_stamp_ns") != stamp_ns + or diagnostics.get("reference_frame_is_tag_pose") is not False + or diagnostics.get("branch_status") != "tracking" + or diagnostics.get("branch_frozen") is not True + or diagnostics.get("branch_revision") != 1 + or item.get("reference_branch_revision") != 1): + raise ValueError("image_reference_observation_unverified") + ref.check_image(item.get("tag_role"), item.get("tag_size_m"), + ref.camera_matrix if camera_matrix is None else camera_matrix, + item.get("corners_xy"), stamp_ns) + pose = item.get("selected_pose", item.get("selected")) + if (not isinstance(pose, dict) or tuple(pose.get("quaternion_xyzw", ())) != ref.pose.quaternion_xyzw + or tuple(pose.get("translation_xyz_m", ())) != ref.origin_xyz_m + or pose.get("reprojection_error_px") != 0.): + raise ValueError("image_reference_coordinate_pose_changed") + return ref diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py new file mode 100644 index 0000000..1781c87 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer.py @@ -0,0 +1,109 @@ +"""Use measured parent geometry as uncertain evidence, never as exact CAD. + +New hinges are fitted to their own training images within the source-bound +geometry uncertainty. The same domain is checked on model readback. These +bounds are not a proof against systematic camera/Tag bias. +""" + +from dataclasses import dataclass, fields +import math +from typing import ClassVar + +import numpy as np +from scipy.spatial.transform import Rotation + +from .image_motion_model import ImageMotionModel +from .motion_evidence import MotionEvidenceParameters +from .source_hinge import SourceHinge +from .cad_hinge import ParallelAxisGeometry + +MEASURED_TRANSFER_POLICY = "image_hinge_v4_uncertain_source_geometry" + + +@dataclass(frozen=True) +class TransferGeometryCheck: + joint: str + axis_difference_rad: float + axis_allowance_rad: float + distance_difference_m: float + distance_allowance_m: float + + @property + def consistent(self): + return (self.axis_difference_rad <= self.axis_allowance_rad+1e-10 + and self.distance_difference_m <= self.distance_allowance_m+1e-10) + + +def transfer_geometry_checks(geometry, constraints, sources, bounds): + """Match independent fixed-source hinges using their measured error bounds. + + The same conservative lever-arm bound used by the former hard-constrained + solver is now used in candidate eligibility as well as reported uncertainty. + Geometry assembled jointly from current images retains its exact CAD solver. + """ + if (not constraints or not sources or any(not isinstance(c, ParallelAxisGeometry) for c in constraints) + or any(not isinstance(s, SourceHinge) for s in sources)): + raise ValueError("measured_transfer_sources_invalid") + parents = {s.geometry.relation.joint: s for s in sources} + children = {g.relation.joint: g for g in geometry} + uncertainty = {name: (axis, point) for name, axis, point in bounds} + if (len(parents) != len(sources) or set(parents) & set(children) + or set(parents) != {c.parent_joint for c in constraints} + or set(children) != {c.child_joint for c in constraints} + or len(children) != len(constraints) or len(uncertainty) != len(bounds) + or set(uncertainty) != set(children)): + raise ValueError("measured_transfer_graph_unsupported") + checks, inherited = [], {} + limits = MotionEvidenceParameters() + for c in constraints: + source, child = parents[c.parent_joint], children[c.child_joint] + g = source.geometry + if g.relation.child_role != child.relation.parent_role: + raise ValueError("measured_transfer_roles_changed") + axis_bound, point_bound = uncertainty[c.child_joint] + inherited_axis = source.child_axis_uncertainty_rad + inherited_point = source.child_point_uncertainty_m + c.distance_m*inherited_axis + if (any(not math.isfinite(v) or v <= 0 for v in (axis_bound, point_bound)) + or axis_bound+inherited_axis > limits.maximum_split_axis_difference_rad + or point_bound+inherited_point > limits.maximum_frame_pivot_error_m): + raise ValueError("image_motion_geometry_uncertain") + axis = c.axis_sign*Rotation.from_quat(g.reference_quaternion_xyzw).inv().apply(g.axis_parent_xyz) + measured_axis = np.asarray(child.axis_parent_xyz) + angle = math.atan2(np.linalg.norm(np.cross(axis, measured_axis)), axis @ measured_axis) + vector = np.asarray(g.mount_child_xyz_m) + child.pivot_parent_xyz_m + # Check the exact domain used by the bounded solver, in the fitted + # axis frame. New image uncertainty cannot enlarge the source prior. + distance = np.linalg.norm(vector-measured_axis*(measured_axis @ vector)) + checks.append(TransferGeometryCheck(c.child_joint, angle, inherited_axis, + abs(float(distance)-c.distance_m), inherited_point)) + inherited[c.child_joint] = (inherited_axis, inherited_point) + return tuple(checks), inherited + + +@dataclass(frozen=True) +class MeasuredTransferImageMotionModel(ImageMotionModel): + POLICY: ClassVar[str] = MEASURED_TRANSFER_POLICY + policy: str = MEASURED_TRANSFER_POLICY + constraints: tuple[ParallelAxisGeometry, ...] = () + source_urdf_sha256: str = "" + source_hinges: tuple[SourceHinge, ...] = () + current_geometry_uncertainty: tuple[tuple[str, float, float], ...] = () + + def __post_init__(self): + super().__post_init__() + if (len(self.source_urdf_sha256) != 64 + or any(c not in "0123456789abcdef" for c in self.source_urdf_sha256) + or any(not isinstance(v, tuple) for v in (self.constraints, self.source_hinges, + self.current_geometry_uncertainty))): + raise ValueError("image_model_source_geometry_missing") + checks, _ = transfer_geometry_checks(self.geometry, self.constraints, + self.source_hinges, self.current_geometry_uncertainty) + if not all(c.consistent for c in checks): + raise ValueError("image_motion_source_geometry_conflict") + + +def bind_measured_transfer(model, constraints, sources, source_hash, bounds): + values = {f.name: getattr(model, f.name) for f in fields(ImageMotionModel) if f.name != "policy"} + return MeasuredTransferImageMotionModel(**values, constraints=tuple(constraints), + source_hinges=tuple(sources), source_urdf_sha256=source_hash, + current_geometry_uncertainty=tuple(bounds)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py new file mode 100644 index 0000000..77f675a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/measured_transfer_solver.py @@ -0,0 +1,81 @@ +"""Image fitting with bounded deviations from an independently measured hinge.""" + +import math +import numpy as np +from scipy.optimize import least_squares +from scipy.sparse import lil_matrix +from scipy.spatial.transform import Rotation + +from .motion_image_solver import ImageHingeBundle, _basis +from .hinge_uncertainty import unconstrained_hinge_linearization +from .measured_transfer import transfer_geometry_checks +from .motion_image_types import ImageHingeGeometry + + +class MeasuredTransferImageHingeBundle(ImageHingeBundle): + def __init__(self, frames, relations, poses, *, constraints, sources): + super().__init__(frames, relations, poses) + # Validate topology and source precision before removing any freedoms. + initial_geometry = tuple(ImageHingeBundle.geometry_values(self, self.initial, i) for i in range(self.joints)) + geometry = tuple(ImageHingeGeometry(r, tuple(a), tuple(q.as_quat()), tuple(p), tuple(m)) + for r, (a, q, p, m) in zip(relations, initial_geometry)) + transfer_geometry_checks(geometry, constraints, sources, + tuple((r.joint, 1e-12, 1e-12) for r in relations)) + parents = {s.geometry.relation.joint: s for s in sources} + by_child = {c.child_joint: c for c in constraints} + self.source_axes, self.source_mounts, self.distances, self.tangents = [], [], [], [] + self.lower = np.full(len(self.initial), -np.inf) + self.upper = np.full(len(self.initial), np.inf) + for i, relation in enumerate(relations): + c = by_child[relation.joint] + source = parents[c.parent_joint] + g = source.geometry + axis = c.axis_sign*Rotation.from_quat(g.reference_quaternion_xyzw).inv().apply(g.axis_parent_xyz) + tangent = _basis(axis) + old_axis, _, pivot, _ = initial_geometry[i] + radial = old_axis-axis*(axis @ old_axis) + tilt = math.atan2(np.linalg.norm(radial), axis @ old_axis) + azimuth = math.atan2(tangent[:, 1] @ radial, tangent[:, 0] @ radial) + vector = pivot+np.asarray(g.mount_child_xyz_m) + phase = math.atan2(tangent[:, 1] @ vector, tangent[:, 0] @ vector) + radius_bound = source.child_point_uncertainty_m+c.distance_m*source.child_axis_uncertainty_rad + self.initial[i*10:i*10+2] = (np.clip(tilt, 1e-9, source.child_axis_uncertainty_rad*.99), azimuth) + self.initial[i*10+5:i*10+7] = phase, 0. + self.scale[i*10+5] = 1. + self.lower[i*10], self.upper[i*10] = 0., source.child_axis_uncertainty_rad + self.lower[i*10+6], self.upper[i*10+6] = -radius_bound, radius_bound + self.source_axes.append(axis) + self.source_mounts.append(np.asarray(g.mount_child_xyz_m)) + self.distances.append(c.distance_m) + self.tangents.append(tangent) + + def geometry_values(self, parameters, index): + v = parameters[index*10:(index+1)*10] + original = self.source_axes[index] + tangent = self.tangents[index] + tilt_direction = tangent @ [math.cos(v[1]), math.sin(v[1])] + change = Rotation.from_rotvec(np.cross(original, tilt_direction)*v[0]) + axis = change.apply(original) + reference = Rotation.from_rotvec(v[2:5])*self.references[index] + basis = change.apply(tangent.T).T + pivot = (self.distances[index]+v[6])*(basis @ [math.cos(v[5]), math.sin(v[5])]) + mount = self.source_mounts[index] + pivot -= mount-axis*(axis @ mount) + return axis, reference, pivot, v[7:10] + + def solve(self, limits): + width = 8*self.joints + sparsity = lil_matrix((self.count*width, len(self.initial)), dtype=int) + for frame in range(self.count): + for joint in range(self.joints): + rows = slice(frame*width+joint*8, frame*width+(joint+1)*8) + sparsity[rows, self.geometry_parameter_indices(joint)] = 1 + if frame: + sparsity[rows, self.static_parameter_count+(frame-1)*self.joints+joint] = 1 + return least_squares(self.residual, self.initial, bounds=(self.lower, self.upper), + jac_sparsity=sparsity.tocsr(), x_scale=self.scale, max_nfev=limits.maximum_solver_evaluations, + tr_options={"atol": 1e-10, "btol": 1e-10}, ftol=1e-9, xtol=1e-9, gtol=1e-7) + + def uncertainty_problem(self, fit): + angles = np.vstack((np.zeros(self.joints), fit.x[self.static_parameter_count:].reshape(-1, self.joints))) + return unconstrained_hinge_linearization(self, fit, angles) 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 65f5540..7c6a044 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 @@ -199,6 +199,24 @@ def _candidate_map(items: Sequence[RoleCandidates], roles: Sequence[str]): return result +def motion_frame_rejection(frame: MotionEvidenceFrame, relations: Sequence[MotionRelation], + *, require_frozen_roots: bool = False, +) -> str: + """Admission uses the solver's existing candidate gates, never fit scores. + + Reject a whole incomplete image, preserving every eligible alternative in + accepted images. The raw rejected image remains part of the capture journal. + """ + roles = _validate_relations(relations) + candidates = _candidate_map(frame.roles, roles) + if candidates is None: + return "motion_candidates_missing_or_invalid" + roots = set(roles) - {relation.child_role for relation in relations} + if require_frozen_roots and any(not candidates[role].frozen for role in roots): + return "root_pose_not_frozen" + return "" + + 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] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py index f1cceb5..228a91e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_diagnostics.py @@ -70,6 +70,17 @@ def _validate_frames(frames, relations, limits): raise ValueError("camera_model_changed") observations = {o.role: o for o in frame.observations} candidates = {r.role: r for r in frame.evidence.roles} + references = {r.role: r for r in frame.reference_frames} + if (frame.reference_frames != frames[0].reference_frames + or len(references) != len(frame.reference_frames) + or not references.keys() <= roots): + raise ValueError("image_motion_reference_frames_changed") + for role, reference in references.items(): + observation = observations.get(role) + if observation is None or role not in candidates or candidates[role].candidates != (reference.pose,): + raise ValueError("image_motion_reference_pose_changed") + reference.check_image(role, observation.tag_size_m, matrix, + observation.corners_xy, frame.evidence.stamp_ns) if len(observations) != len(frame.observations) or any(role not in observations for role in roles): raise ValueError("image_observation_missing_or_duplicate") if any(role not in candidates or not candidates[role].frozen for role in roots): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py index fc5e79f..b4c2dca 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/motion_image_types.py @@ -8,6 +8,7 @@ from __future__ import annotations from dataclasses import dataclass, field from .motion_evidence import MotionEvidenceFrame, MotionRelation +from .image_reference import StationaryImageReference @dataclass(frozen=True) @@ -22,6 +23,7 @@ class ImageMotionFrame: evidence: MotionEvidenceFrame camera_matrix: tuple[tuple[float, float, float], ...] observations: tuple[ImageRoleObservation, ...] + reference_frames: tuple[StationaryImageReference, ...] = () @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py new file mode 100644 index 0000000..721824b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/pose_bridge.py @@ -0,0 +1,132 @@ +"""Independent, same-posture images connect two hinges sharing a rigid Tag. + +The old hinge is never applied while the new hinge moves. Only the shared +stationary posture anchors the new fit and constrains candidate eligibility. +The new hinge axis and pivot come from its preparation arc. No SDK value is +interpreted as radians. +""" + +from dataclasses import dataclass, replace +import math +from typing import TYPE_CHECKING + +import numpy as np +from scipy.spatial.transform import Rotation + +from .motion_evidence import MotionRelation +from .motion_image_types import ImageMotionFrame +from .relative import _relative_pose + +SHARED_TAG_SELECTION_POLICY = "training_squared_loss_shared_pose_holm_v2" +SHARED_TAG_CONDITIONED_POLICY = "training_shared_zero_orientation_holm_v3" +SHARED_ZERO_CONDITIONING = "shared_zero_orientation_v1" +SHARED_POSE_CONDITIONING = "shared_zero_pose_v2" +SHARED_TAG_POSE_CONDITIONED_POLICY = "training_shared_zero_pose_holm_v4" +SHARED_TAG_CONDITIONING_POLICIES = { + SHARED_TAG_SELECTION_POLICY: None, + SHARED_TAG_CONDITIONED_POLICY: SHARED_ZERO_CONDITIONING, + SHARED_TAG_POSE_CONDITIONED_POLICY: SHARED_POSE_CONDITIONING, +} + +if TYPE_CHECKING: + from .image_motion_model import ImageMotionModel + + +@dataclass(frozen=True) +class SharedTagPoseBridge: + relation: MotionRelation + quaternion_xyzw: tuple[float, ...] + translation_xyz_m: tuple[float, ...] + frames: tuple[ImageMotionFrame, ...] + source_model: "ImageMotionModel | None" = None + conditioning: str = SHARED_POSE_CONDITIONING + + +def shared_pose_selection_policy(bridges): + modes = {b.conditioning for b in bridges if b.source_model is not None} + if not modes: + return SHARED_TAG_SELECTION_POLICY + if len(modes) == 1: + mode = next(iter(modes)) + for policy, conditioning in SHARED_TAG_CONDITIONING_POLICIES.items(): + if conditioning is not None and conditioning == mode: + return policy + raise ValueError("shared_pose_conditioning_policy_changed") + + +@dataclass(frozen=True) +class PoseBridgeCheck: + joint: str + status: str + reason: str + maximum_rotation_rad: float = 0. + maximum_translation_m: float = 0. + + +def check_source_pose(bridge): + """Check the old hinge only on new, held-zero images, in its own coordinates. + + This is independent of the new hinge fit. A frozen zero pose may only + condition a new model after the source itself still explains today's pose. + No old scan or old image is solved again. + """ + from .motion_evidence import RoleCandidates + + source = bridge.source_model + if source is None or len(source.geometry) != 1: + return PoseBridgeCheck(bridge.relation.joint, "unresolved", "shared_pose_source_model_invalid") + relation = source.geometry[0].relation + if ((relation.parent_role, relation.child_role) != + (bridge.relation.parent_role, bridge.relation.child_role)): + return PoseBridgeCheck(bridge.relation.joint, "unresolved", "shared_pose_source_roles_changed") + references = {ref.role: ref for ref in source.reference_frames} + frames = tuple(replace(frame, reference_frames=source.reference_frames, + evidence=replace(frame.evidence, roles=tuple( + RoleCandidates(role.role, (references[role.role].pose,), True) + if role.role in references else role for role in frame.evidence.roles))) for frame in bridge.frames) + return check_pose_bridge(source, replace(bridge, relation=relation, frames=frames, source_model=None)) + + +def check_pose_bridge(model, bridge): + """Accept close poses; exclude only disjoint pose-tolerance regions. + + Each estimate gets the existing 2 degree / 5 mm pose-reuse allowance. + Distances between one and two allowances are inconclusive, not rejection. + These are conservative consistency margins, not covariance/confidence claims. + Fresh-image scatter must also pass the existing 1 degree / 1 mm hold gate. + """ + from .image_motion_projection import select_image_motion_frame + + name = bridge.relation.joint + def unknown(reason): + return PoseBridgeCheck(name, "unresolved", reason) + if (bridge.relation not in model.relations or len(bridge.frames) < 10 + or len({f.evidence.stamp_ns for f in bridge.frames}) != len(bridge.frames) + or set(model.source_stamps_ns) & {f.evidence.stamp_ns for f in bridge.frames}): + return unknown("shared_pose_support_invalid") + rotations, points = [], [] + for frame in bridge.frames: + checked = select_image_motion_frame(model, frame) + poses = {s.role: s.pose for s in checked.selections} + if not checked.resolved: + return unknown("shared_pose_current_image_unresolved") + try: + rotation, point = _relative_pose(poses[bridge.relation.parent_role], poses[bridge.relation.child_role]) + except KeyError: + return unknown("shared_pose_roles_missing") + rotations.append(rotation.as_quat()) + points.append(point) + rotations, points = Rotation.from_quat(rotations), np.asarray(points) + if (np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1.) + or np.max(np.linalg.norm(points-np.median(points, axis=0), axis=1)) > .001): + return unknown("shared_pose_current_images_not_stable") + angles = (Rotation.from_quat(bridge.quaternion_xyzw).inv()*rotations).magnitude() + distances = np.linalg.norm(points-np.asarray(bridge.translation_xyz_m), axis=1) + rotation_limit, translation_limit = math.radians(2.), .005 + if np.max(angles) <= rotation_limit and np.max(distances) <= translation_limit: + status, reason = "consistent", "" + elif np.min(angles) > 2*rotation_limit or np.min(distances) > 2*translation_limit: + status, reason = "conflicting", "shared_pose_outside_both_pose_margins" + else: + status, reason = "unresolved", "shared_pose_tolerance_regions_overlap" + return PoseBridgeCheck(name, status, reason, float(np.max(angles)), float(np.max(distances))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py index 8a5ce84..cb83d42 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py @@ -25,6 +25,7 @@ from .motion_image_diagnostics import ( _ordered_relations, _validate_frames, _sample, _subset, _quality, _pnp_diagnostics, ) from .motion_image_solver import ImageHingeBundle +from .hinge_uncertainty import hinge_uncertainty __all__ = ["ImageMotionModel", "ImageMotionResolution", "ImageMotionFrameSelection", "resolve_image_motion", "select_image_motion_frame", "image_motion_model_from_dict", @@ -55,47 +56,17 @@ class ImageMotionParameters: raise ValueError("image_motion_limits_invalid") -def _uncertainty(bundle, fit): - """Gaussian local confidence after estimating all per-image nuisance angles. - - The bundle already fixes angle and axial-pivot gauges. SVD of the full - Jacobian accounts for coupling to per-frame angles. Residual variance has a - 0.03 pixel floor, the existing image branch resolution, to avoid claiming - infinite precision on synthetic or rounded input. Three-sigma conservative - vector bounds cover the axis's two and pivot's three coordinates at >95%. - """ - jacobian = fit.jac.toarray() if hasattr(fit.jac, "toarray") else np.asarray(fit.jac) - scaled = jacobian * bundle.scale[None, :] - _, singular, vh = np.linalg.svd(scaled, full_matrices=False) - if singular[-1] <= singular[0] * np.finfo(float).eps * max(scaled.shape): - raise ValueError("image_motion_geometry_rank_deficient") - sigma_squared = max(.03 ** 2, float(np.sum(fit.fun ** 2) / max(1, len(fit.fun) - len(fit.x)))) - covariance_scaled = (vh.T / singular ** 2) @ vh * sigma_squared - covariance = covariance_scaled * bundle.scale[:, None] * bundle.scale[None, :] - axes, pivots = [], [] - for index, relation in enumerate(bundle.relations): - derivative = np.zeros((6, len(fit.x))) - for column in bundle.geometry_dependency_indices(index): - step = 1e-5 * bundle.scale[column] - upper, lower = fit.x.copy(), fit.x.copy() - upper[column] += step - lower[column] -= step - ua, _, up, _ = bundle.geometry_values(upper, index) - la, _, lp, _ = bundle.geometry_values(lower, index) - derivative[:, column] = np.r_[ua - la, up - lp] / (2 * step) - propagated = derivative @ covariance @ derivative.T - axes.append((relation.joint, float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[:3, :3])[-1]))))) - pivots.append((relation.joint, float(3 * np.sqrt(max(0., np.linalg.eigvalsh(propagated[3:, 3:])[-1]))))) - return tuple(axes), tuple(pivots) - - def _model(bundle, fit, frames, branches, training, validation): from .cad_hinge import CadImageHingeBundle - model_type, extra = ImageMotionModel, {} + model_type, extra = ImageMotionModel, {"reference_frames": frames[0].reference_frames} if isinstance(bundle, CadImageHingeBundle): from .cad_image_model import CadImageMotionModel model_type = CadImageMotionModel - extra = {"constraints": bundle.constraints, "source_urdf_sha256": bundle.source_urdf_sha256} + if bundle.source_hinges: + from .cad_image_model import TransferredImageMotionModel + model_type = TransferredImageMotionModel + extra["source_hinges"] = bundle.source_hinges + extra.update(constraints=bundle.constraints, source_urdf_sha256=bundle.source_urdf_sha256) return model_type(bundle.geometry(fit.x), tuple(tuple(v) for v in frames[0].camera_matrix), tuple((role, next(o.tag_size_m for o in frames[0].observations if o.role == role)) for role, _ in branches), tuple(f.evidence.stamp_ns for f in frames), @@ -104,23 +75,48 @@ def _model(bundle, fit, frames, branches, training, validation): def _fit_hypothesis(frames, relations, poses, branches, training, validation, limits, - *, geometry_constraints=(), source_urdf_sha256=""): + *, geometry_constraints=(), source_urdf_sha256="", source_hinges=(), pose_bridges=()): if any(pose is None for role in poses.values() for pose in role): return ImageMotionHypothesis(branches, False, "incomplete_candidate_family"), None, None try: pnp = _pnp_diagnostics(frames, relations, poses) selected = tuple(frames[i] for i in training) - if geometry_constraints: + if pose_bridges: + from .shared_zero_solver import SharedZeroImageHingeBundle + if geometry_constraints: + raise ValueError("shared_zero_model_graph_unsupported") + bundle = SharedZeroImageHingeBundle(selected, relations, _subset(poses, training), pose_bridges) + elif source_hinges: + from .measured_transfer_solver import MeasuredTransferImageHingeBundle + bundle = MeasuredTransferImageHingeBundle(selected, relations, _subset(poses, training), + constraints=geometry_constraints, sources=source_hinges) + elif geometry_constraints: from .cad_hinge import CadImageHingeBundle bundle = CadImageHingeBundle(selected, relations, _subset(poses, training), - constraints=geometry_constraints, source_urdf_sha256=source_urdf_sha256) + constraints=geometry_constraints, source_urdf_sha256=source_urdf_sha256, source_hinges=source_hinges) else: bundle = ImageHingeBundle(selected, relations, _subset(poses, training)) fit = bundle.solve(limits) model = _model(bundle, fit, frames, branches, training, validation) training_quality = _quality(fit.fun, bundle.child_roles) training_converged = bool(fit.success and np.all(np.isfinite(fit.fun))) - axes, pivots = _uncertainty(bundle, fit) if training_converged else ((), ()) + axes, pivots, child_bounds = hinge_uncertainty(bundle, fit) if training_converged else ((), (), ()) + transfer_checks = () + transfer_reason = "" + if source_hinges and training_converged: + from .measured_transfer import transfer_geometry_checks, bind_measured_transfer + current_bounds = tuple((name, axis, dict(pivots)[name]) for name, axis in axes) + transfer_checks, inherited = transfer_geometry_checks(model.geometry, + geometry_constraints, source_hinges, current_bounds) + axes = tuple((name, value+inherited[name][0]) for name, value in axes) + pivots = tuple((name, value+inherited[name][1]) for name, value in pivots) + child_bounds = tuple((name, axis+inherited[name][0], point+inherited[name][1]) + for name, axis, point in child_bounds) + if all(c.consistent for c in transfer_checks): + model = bind_measured_transfer(model, geometry_constraints, source_hinges, + source_urdf_sha256, current_bounds) + else: + transfer_reason = "image_motion_source_geometry_conflict" p = MotionEvidenceParameters() reason = "" if training_converged else "image_motion_optimizer_unresolved" if not reason and any(q.maximum_frame_rms_px > model.maximum_reprojection_error_px for q in training_quality): @@ -128,6 +124,7 @@ def _fit_hypothesis(frames, relations, poses, branches, training, validation, li if not reason and (any(v > p.maximum_split_axis_difference_rad for _, v in axes) or any(v > p.maximum_frame_pivot_error_m for _, v in pivots)): reason = "image_motion_geometry_uncertain" + reason = reason or transfer_reason training_accepted = not reason try: residuals, valid = bundle.validate(fit.x, tuple(frames[i] for i in validation), @@ -146,14 +143,15 @@ def _fit_hypothesis(frames, relations, poses, branches, training, validation, li hypothesis = ImageMotionHypothesis(branches, converged, reason, training_quality, validation_quality, tuple((r.joint, r.motion_span_rad) for r in pnp), axes, pivots, training_solver_status=int(fit.status), training_solver_evaluations=int(fit.nfev), - training_accepted=training_accepted) + training_accepted=training_accepted, child_frame_uncertainty=child_bounds, + source_geometry_checks=transfer_checks) rms = None if residuals is None else np.sqrt(np.mean(np.sum(residuals ** 2, axis=-1), axis=(1, 2))) return hypothesis, model, rms except (ValueError, FloatingPointError, np.linalg.LinAlgError) as error: return ImageMotionHypothesis(branches, False, str(error)), None, None -def resolve_image_motion(frames, relations, *, parameters=None, geometry_constraints=(), source_urdf_sha256=""): +def resolve_image_motion(frames, relations, *, parameters=None, geometry_constraints=(), source_urdf_sha256="", source_hinges=(), pose_bridges=()): """Only pre-zero preparation frames may enter this expensive training API. Runtime owns session, motion and phase checks. This function knows no SDK @@ -162,7 +160,15 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra """ limits = parameters or ImageMotionParameters() frames, relations = tuple(frames), tuple(relations) + from .pose_bridge import check_source_pose + conditioned = tuple(bridge for bridge in pose_bridges if bridge.source_model is not None) + for bridge in conditioned: + checked = check_source_pose(bridge) + if checked.status != "consistent": + return ImageMotionResolution(False, "image_motion_shared_pose_source_unverified:" + checked.reason) try: + if source_hinges and not geometry_constraints: + raise ValueError("image_motion_source_hinge_requires_geometry") relations = _ordered_relations(relations) if len(relations) > limits.maximum_relations: raise ValueError("image_motion_graph_limit") @@ -179,16 +185,33 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra for indices in product(*(range(len(paths[r])) for r in roles)): branches = tuple(zip(roles, indices)) poses = {role: paths[role][index] for role, index in branches} - geometry = {"geometry_constraints": geometry_constraints, "source_urdf_sha256": source_urdf_sha256} + geometry = {"geometry_constraints": geometry_constraints, "source_urdf_sha256": source_urdf_sha256, + "source_hinges": source_hinges} hypothesis, model, rms = _fit_hypothesis(frames, relations, poses, branches, training, validation, limits, - **(geometry if geometry_constraints else {})) + **(geometry if geometry_constraints else {}), pose_bridges=conditioned) hypotheses.append(hypothesis) models.append(model) residuals.append(rms) + # Independent stationary evidence is an eligibility constraint, applied + # before choosing by training error. Held-out arc scores cannot select a + # different winner. Inconclusive bridge evidence never excludes a family. + from .pose_bridge import check_pose_bridge + for index, model in enumerate(models): + if model is None or not pose_bridges: + continue + checks = tuple(check_pose_bridge(model, bridge) for bridge in pose_bridges) + hypotheses[index] = replace(hypotheses[index], pose_bridge_checks=checks) + if any(check.status == "conflicting" for check in checks): + hypotheses[index] = replace(hypotheses[index], training_accepted=False, + reason=hypotheses[index].reason or "image_motion_shared_pose_conflict") winner = training_candidate_index(hypotheses) + if winner is None and any(h.reason == "image_motion_shared_pose_conflict" for h in hypotheses): + return ImageMotionResolution(False, "image_motion_shared_pose_conflict", hypotheses=tuple(hypotheses)) if winner is None or hypotheses[winner].reason: return ImageMotionResolution(False, "image_motion_no_observable_image_model", hypotheses=tuple(hypotheses)) model = models[winner] + if any(check.status != "consistent" for check in hypotheses[winner].pose_bridge_checks): + return ImageMotionResolution(False, "image_motion_shared_pose_unresolved", hypotheses=tuple(hypotheses)) checks = tuple(select_image_motion_frame(model, frames[i]) for i in validation) if not all(c.resolved for c in checks): return ImageMotionResolution(False, "image_motion_validation_frame_unresolved", hypotheses=tuple(hypotheses)) @@ -201,6 +224,10 @@ def resolve_image_motion(frames, relations, *, parameters=None, geometry_constra if alternative is None or residuals[index] is None or not hypotheses[index].converged: # Missing candidate evidence is not evidence excluding a branch. return ImageMotionResolution(False, "image_motion_alternative_not_evaluable", hypotheses=tuple(hypotheses)) + if any(check.status == "conflicting" for check in hypotheses[index].pose_bridge_checks): + continue + if hypotheses[index].reason == "image_motion_source_geometry_conflict": + continue if hypotheses[index].reason == "image_motion_reprojection_failed": # This converged model violates the unchanged absolute image gate # on training/held-out images. It is already an inadmissible diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py new file mode 100644 index 0000000..336839e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/shared_zero_solver.py @@ -0,0 +1,103 @@ +"""Fit a new hinge using an independently measured shared zero pose. + +Axis, pivot and every image angle are measured anew. The Tag mount follows from +the shared pose and the new pivot, not from the old hinge's mount or axis. +Current stationary witness images are validation-only; the shared pose comes +from the previous passed zero. Both IPPE initializations still +run, and the independent arc holdout retains every ordinary image quality gate. +""" + +import numpy as np +from scipy.optimize import least_squares +from scipy.sparse import lil_matrix +from scipy.spatial.transform import Rotation + +from .motion_image_solver import ImageHingeBundle +from .relative import _relative_pose +from .pose_bridge import SHARED_POSE_CONDITIONING, SHARED_ZERO_CONDITIONING + + +class SharedZeroImageHingeBundle(ImageHingeBundle): + """Remove shared zero variables, keep all image angles free. + + The anchor fixes the angle gauge at the earlier measured zero, not at the + first preparation image. A new angle is therefore needed for that image. + The current implementation covers independent fixed-root hinges, matching + the shared-posture contract; moving-parent transfer has its separate solver. + """ + + def __init__(self, frames, relations, poses, bridges): + super().__init__(frames, relations, poses) + anchors = {bridge.relation: bridge for bridge in bridges} + if (set(anchors) != set(relations) or len(anchors) != len(bridges) + or any(r.parent_role in self.child_roles for r in relations)): + raise ValueError("shared_zero_model_graph_unsupported") + modes = {bridge.conditioning for bridge in bridges} + if len(modes) != 1 or not modes <= {SHARED_POSE_CONDITIONING, SHARED_ZERO_CONDITIONING}: + raise ValueError("shared_pose_conditioning_policy_changed") + # Retain the orientation-only parameterization for explicit legacy + # replay; new acquisitions share the full physical zero posture. + self.share_translation = modes == {SHARED_POSE_CONDITIONING} + self.geometry_width = 4 if self.share_translation else 7 + original = self.initial.copy() + self.anchors = tuple(Rotation.from_quat(anchors[r].quaternion_xyzw) for r in relations) + self.zero_points = tuple(np.asarray(anchors[r].translation_xyz_m) for r in relations) + geometry, angles = [], [] + for index, relation in enumerate(relations): + values = original[index*10:(index+1)*10] + geometry.extend((*values[:2], *values[5:7])) + if not self.share_translation: + geometry.extend(values[7:10]) + angles.append([float((_relative_pose(parent, child)[0] * self.anchors[index].inv() + ).as_rotvec() @ self.axes[index]) for parent, child in + zip(poses[relation.parent_role], poses[relation.child_role])]) + self.initial = np.r_[geometry, np.asarray(angles).T.ravel()] + self.scale = np.r_[np.tile([1., 1.] + [.05]*(self.geometry_width-2), self.joints), + np.ones(self.count*self.joints)] + + @property + def static_parameter_count(self): + return self.geometry_width*self.joints + + def geometry_parameter_indices(self, index): + return tuple(range(self.geometry_width*index, self.geometry_width*(index+1))) + + def geometry_values(self, parameters, index): + values = parameters[index*self.geometry_width:(index+1)*self.geometry_width] + change = Rotation.from_rotvec(self.bases[index] @ values[:2]) + pivot = change.apply(self.bases[index] @ values[2:4]) + reference = self.anchors[index] + # At shared zero: t0 = pivot + R0 * mount. A free mount would + # reintroduce three competing estimates of the same known posture. + mount = (reference.inv().apply(self.zero_points[index]-pivot) + if self.share_translation else values[4:7]) + return change.apply(self.axes[index]), reference, pivot, mount + + def residual(self, parameters): + angles = parameters[self.static_parameter_count:].reshape(self.count, self.joints) + pixels, _ = self.project(parameters, angles, self.root_transforms, self.matrices, self.objects) + return (pixels-self.observed).ravel() + + def solve(self, limits): + width = 8*self.joints + sparsity = lil_matrix((self.count*width, len(self.initial)), dtype=int) + for frame in range(self.count): + for joint in range(self.joints): + rows = slice(frame*width+joint*8, frame*width+(joint+1)*8) + sparsity[rows, self.geometry_parameter_indices(joint)] = 1 + sparsity[rows, self.static_parameter_count+frame*self.joints+joint] = 1 + return least_squares(self.residual, self.initial, jac_sparsity=sparsity.tocsr(), + x_scale=self.scale, max_nfev=limits.maximum_solver_evaluations, + tr_options={"atol": 1e-10, "btol": 1e-10}, ftol=1e-9, xtol=1e-9, gtol=1e-7) + + def uncertainty_problem(self, fit): + """Evaluate full image sensitivity without treating the anchor as exact. + + Restore all orientation and translation freedoms for the uncertainty test. + This does not refit or alter the accepted model: one Jacobian evaluation + keeps its projected pixels identical. A conditioning prior must never + turn weak image geometry into a spuriously precise axis or pivot. + """ + from .hinge_uncertainty import unconstrained_hinge_linearization + angles = fit.x[self.static_parameter_count:].reshape(self.count, self.joints) + return unconstrained_hinge_linearization(self, fit, angles) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/source_hinge.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/source_hinge.py new file mode 100644 index 0000000..33ab8a1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/source_hinge.py @@ -0,0 +1,42 @@ +"""A measured hinge installation transported with its rigid physical Tag.""" + +from dataclasses import dataclass +import math +import numpy as np + +from .motion_evidence import MotionRelation +from .motion_image_types import ImageHingeGeometry + + +def hinge_from_dict(item): + return ImageHingeGeometry(MotionRelation(**item["relation"]), tuple(item["axis_parent_xyz"]), + tuple(item["reference_quaternion_xyzw"]), tuple(item["pivot_parent_xyz_m"]), + tuple(item["mount_child_xyz_m"])) + + +@dataclass(frozen=True) +class SourceHinge: + geometry: ImageHingeGeometry + evidence_id: str + model_sha256: str + child_axis_uncertainty_rad: float + child_point_uncertainty_m: float + + def __post_init__(self): + for identity in (self.evidence_id, self.model_sha256): + if not isinstance(identity, str) or len(identity) != 64 or any(c not in "0123456789abcdef" for c in identity): + raise ValueError("source_hinge_identity_invalid") + if any(not math.isfinite(v) or v <= 0 for v in ( + self.child_axis_uncertainty_rad, self.child_point_uncertainty_m)): + raise ValueError("source_hinge_uncertainty_missing") + g = self.geometry + if (not np.all(np.isfinite((*g.axis_parent_xyz, *g.reference_quaternion_xyzw, + *g.pivot_parent_xyz_m, *g.mount_child_xyz_m))) + or not np.isclose(np.linalg.norm(g.axis_parent_xyz), 1., atol=1e-7) + or not np.isclose(np.linalg.norm(g.reference_quaternion_xyzw), 1., atol=1e-7)): + raise ValueError("source_hinge_geometry_invalid") + + +def source_hinge_from_dict(value): + return SourceHinge(hinge_from_dict(value["geometry"]), value["evidence_id"], value["model_sha256"], + float(value["child_axis_uncertainty_rad"]), float(value["child_point_uncertainty_m"])) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py index c863b2b..7463c4f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py @@ -4,6 +4,8 @@ from __future__ import annotations from contextlib import ExitStack from copy import deepcopy +from ..frozen_evidence import image_model_evidence + from dataclasses import asdict, dataclass, field, replace from threading import RLock from typing import Sequence @@ -254,6 +256,17 @@ class SquareTagPoseTracker: self._image_observations_by_role.clear() self._image_model_sha256_by_role.clear() + def reset_roles(self, roles): + """Invalidate old image decisions before a declared new motion model.""" + with self._lock: + self._epoch += 1 + for role in roles: + for values in (self._branches.states, self._branches.corrections, + self.last_candidates_by_role, self.last_candidate_diagnostics_by_role, + self._motion_candidates_by_role, self._image_observations_by_role, + self._image_model_sha256_by_role): + values.pop(role, None) + def observation_snapshot(self, role, stamp_ns): """Copy one image's evidence atomically, never a subsequent callback.""" with self._lock: @@ -393,7 +406,7 @@ class SquareTagPoseTracker: diagnostics.update({ "pose_constraint": "image_motion_geometry", "pose_source": "image_constrained_current_image", - "frozen_image_model": asdict(image_decision.model), + "frozen_image_model": image_model_evidence(image_decision.model), "image_model_sha256": image_decision.model_sha256, "image_motion_quality": [asdict(item) for item in image_decision.quality], }) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py index 022e310..097e652 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py @@ -73,9 +73,13 @@ class SerializedJointMapping: def __init__(self, payload: Mapping[str, Any], *, input_kind=None) -> None: self.payload = payload + from .partial_scope import retained_from_payload + self.retained_joints = retained_from_payload(payload) self.measured = payload.get("format") in {"unified_calibration_v3", "unified_calibration_report_v3"} self.compact = payload.get("format") in {"unified_calibration_v2", "unified_calibration_v3"} - input_kind = input_kind or ("command" if self.compact else "feedback") + command_report = (payload.get("format") == "unified_calibration_report_v3" + and payload.get("quality", {}).get("release_basis") == "steady_command") + input_kind = input_kind or ("command" if self.compact or command_report else "feedback") declared_directions = payload.get("directional_command_joints", []) if (not isinstance(declared_directions, list) or any(not isinstance(n, str) for n in declared_directions) or len(set(declared_directions)) != len(declared_directions) @@ -363,8 +367,10 @@ def validate_compact_urdf_tables(payload, model): if sources is not None: validate_independent_output(model, {name: row["motor_index"] for name, row in mapping.joints.items()}, sources) moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"} - if set(mapping.joints) != moving: + if set(mapping.joints) | set(mapping.retained_joints) != moving: raise ValueError("compact JSON joints differ from the exported URDF") + from .partial_scope import retained_from_payload + retained_from_payload(payload, model) for name, row in mapping.joints.items(): joint = model.joints[name] values = np.asarray(row["angle_rad"]) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py index 4f4fbc7..f6fb32b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/image_acceptance.py @@ -89,6 +89,10 @@ def validate_serialized_image_holdout(*, corrected_urdf, payload, common_from_ba mapping = SerializedJointMapping(payload, input_kind=input_kind) active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed" and (mapping.measured or joint.mimic_joint is None)) + if mapping.retained_joints: + from .partial_scope import retained_from_payload, require_retained_pose + retained_from_payload(payload, model) + active = tuple(name for name in active if name not in mapping.retained_joints) base = rigid_matrix(common_from_base) training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids} independent = {} @@ -112,6 +116,8 @@ def validate_serialized_image_holdout(*, corrected_urdf, payload, common_from_ba mount = installations[row.role] if mount.role != row.role or mount.link not in model.links: raise ValueError("final_image_holdout_installation_invalid") + if mapping.retained_joints: + require_retained_pose(mapping.retained_joints, row.sdk_values, model=model, link=mount.link) angles = mapping.evaluate(row.sdk_values, "", active_joints=active, directions_by_joint=dict(row.directions)) link = np.eye(4) if mount.link == model.root_link else model.link_transform( diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py index a7402ad..1c24ca7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/limits.py @@ -7,6 +7,7 @@ adds tolerance padding, fits a curve, or consumes validation observations. from dataclasses import dataclass import math +from ..domain.capture_plan import training_cycles, LEGACY_TRAINING @dataclass(frozen=True) @@ -16,7 +17,7 @@ class MotionRangeEvidence: independently_measured: bool source_limits_cad_rad: tuple[float, float] output_limits_rad: tuple[float, float] - feedback_bounds_rad: tuple[float, float] + feedback_bounds_rad: tuple[float, float] | None command_bounds_rad: tuple[float, float] zero_offset_rad: float baseline_command: float @@ -29,6 +30,7 @@ class MotionRangeEvidence: safety_evidence_source: str command_training_cycles: tuple[int, ...] = (0, 1, 2) command_training_sample_ids: tuple[str, ...] = () + release_basis: str = "feedback_and_command" def _interval(value): @@ -39,17 +41,22 @@ def validate_range_evidence(name, original, delta, interval, evidence): """Validate trusted fit evidence at both planning and final read-back.""" 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 != (0, 1, 2) or not evidence.training_sample_ids - or evidence.command_training_cycles not in {(0, 1, 2), (4,)} + 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.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) or evidence.source_limits_cad_rad != (original.lower, original.upper) or evidence.zero_offset_rad != delta - or not _interval(evidence.feedback_bounds_rad) or not _interval(evidence.command_bounds_rad)): + or evidence.release_basis not in {"feedback_and_command", "steady_command"} + or (evidence.release_basis == "steady_command" and evidence.feedback_bounds_rad is not None) + or (evidence.release_basis == "feedback_and_command" and + (evidence.feedback_bounds_rad is None or not _interval(evidence.feedback_bounds_rad))) + or not _interval(evidence.command_bounds_rad)): raise ValueError(f"invalid_training_motion_range_evidence:{name}") - expected = (min(0.0, evidence.feedback_bounds_rad[0], evidence.command_bounds_rad[0]), - max(0.0, evidence.feedback_bounds_rad[1], evidence.command_bounds_rad[1])) + motion_bounds = evidence.feedback_bounds_rad or evidence.command_bounds_rad + expected = (min(0.0, motion_bounds[0], evidence.command_bounds_rad[0]), + max(0.0, motion_bounds[1], evidence.command_bounds_rad[1])) if evidence.output_limits_rad != expected or not _interval(interval) \ or any(abs(a-b) > 1e-10 for a, b in zip(interval, expected)): raise ValueError(f"motion_range_differs_from_training_support:{name}") @@ -70,33 +77,35 @@ def compile_motion_ranges(profile, model, fit): """ result = {} from ..domain.sampling import command_training_cycles - command_cycles = command_training_cycles(profile) for name, policy in profile.urdf_limit_policies.items(): source = profile.measurement.transferred_motion_sources.get(name, name) + command_cycles = command_training_cycles(profile, joint=source) + motion_cycles = training_cycles(profile, joint=source) curve = fit.curves[source] reference = fit.joint_zero_references[source] command_evidence = fit.command_holdout_metrics[source] - if (tuple(curve.circle.get("training_cycles", ())) != (0, 1, 2) + if (tuple(curve.circle.get("training_cycles", ())) != motion_cycles or tuple(command_evidence.get("training_cycles", ())) != command_cycles or not curve.circle.get("training_sample_ids") or not reference.samples): raise ValueError(f"motion_range_requires_training_provenance:{name}") - feedback, command = fit.output_mappings[name], fit.command_mappings[name] - if (feedback.transferred_from != (source if source != name else None) - or command.transferred_from != feedback.transferred_from + primary, command = fit.output_mappings[name], fit.command_mappings[name] + if (primary.transferred_from != (source if source != name else None) + or command.transferred_from != primary.transferred_from or abs(command.evaluate(reference.baseline_command)) > 1e-10): raise ValueError(f"motion_range_requires_shared_baseline_and_donor:{name}") - bounds = (min(0.0, feedback.bounds_rad[0], command.bounds_rad[0]), - max(0.0, feedback.bounds_rad[1], command.bounds_rad[1])) + bounds = (min(0.0, primary.bounds_rad[0], command.bounds_rad[0]), + max(0.0, primary.bounds_rad[1], command.bounds_rad[1])) original = model.joints[name] evidence = MotionRangeEvidence(name, source, source == name, - (original.lower, original.upper), bounds, feedback.bounds_rad, command.bounds_rad, - fit.zero_offsets_rad[name], reference.baseline_command, (0, 1, 2), + (original.lower, original.upper), bounds, + None if profile.command_based_release else primary.bounds_rad, command.bounds_rad, + fit.zero_offsets_rad[name], reference.baseline_command, motion_cycles, tuple(sorted(curve.circle["training_sample_ids"])), tuple(sorted(f"{sample.view}:{sample.image_stamp_ns}" for sample in reference.samples)), policy.source_limits, policy.evidence_source, policy.independent_safety_limits_cad_rad, policy.safety_evidence_source, command_cycles, - tuple(command_evidence.get("training_sample_ids", ()))) + tuple(command_evidence.get("training_sample_ids", ())), profile.measurement.release_basis) validate_range_evidence(name, original, fit.zero_offsets_rad[name], bounds, evidence) result[name] = evidence return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/partial_scope.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/partial_scope.py new file mode 100644 index 0000000..35a8036 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/partial_scope.py @@ -0,0 +1,95 @@ +"""Explicit retained-CAD scope shared by fitting, serialization and replay. + +An omitted curve never becomes an all-zero curve. CAD q=0 is used only as a +declared geometry premise while a relevant ancestor is held at its baseline. +""" + +import math + + +def profile_scope(profile): + if not profile.retained_joints: + return None + policy = profile.scope.retained_cad_scopes[profile.scope.default_scope] + return {"name": profile.scope.default_scope, "complete_hand": False, + "calibrated_joints": sorted(profile.zero.active_joints - profile.retained_joints), + "retained_joints": {name: {"sdk_channel": profile.command.command_index_by_joint[name], + "held_command": profile.command.baseline_values[profile.command.command_index_by_joint[name]], + "status": "not_calibrated", "urdf_policy": "preserve_source", + "geometry_assumption": "source_cad_zero_at_held_command", "reason": policy.reason} + for name in sorted(profile.retained_joints)}} + + +def retained_from_payload(payload, model=None): + scope = payload.get("calibration_scope") + if scope is None: + retained = {} + else: + if (payload.get("format") not in {"unified_calibration_v3", "unified_calibration_report_v3"} + or payload.get("transferred_motion_sources") + or payload.get("format") == "unified_calibration_report_v3" + and payload.get("quality", {}).get("release_basis") != "steady_command"): + raise ValueError("partial_scope_requires_independent_command_v3") + if (not isinstance(scope, dict) or set(scope) != { + "name", "complete_hand", "calibrated_joints", "retained_joints"} + or scope["complete_hand"] is not False or not isinstance(scope["name"], str) + or not scope["name"] or scope["name"] == "full" + or scope["calibrated_joints"] != sorted(payload.get("joints", {}))): + raise ValueError("invalid_partial_calibration_scope") + retained = scope["retained_joints"] + baseline = payload.get("baseline_command", ()) + if not isinstance(retained, dict) or not retained or retained.keys() & payload["joints"].keys(): + raise ValueError("invalid_retained_joint_partition") + channels = set() + for name, row in retained.items(): + if (not isinstance(row, dict) or set(row) != {"sdk_channel", "held_command", "status", + "urdf_policy", "geometry_assumption", "reason"}): + raise ValueError(f"invalid_retained_joint_contract:{name}") + index = row["sdk_channel"] + if (type(index) is not int or not 0 <= index < len(baseline) or index in channels + or row["held_command"] != baseline[index] + or not math.isfinite(float(row["held_command"])) + or row["status"] != "not_calibrated" or row["urdf_policy"] != "preserve_source" + or row["geometry_assumption"] != "source_cad_zero_at_held_command" + or not isinstance(row["reason"], str) or not row["reason"].strip()): + raise ValueError(f"invalid_retained_joint_contract:{name}") + channels.add(index) + mapped_channels = {row.get("sdk_channel", row.get("command_to_rad", {}).get("channel_index")) + for row in payload["joints"].values()} + if channels & mapped_channels: + raise ValueError("retained_channel_has_a_calibration_curve") + if retained.keys() & payload.get("urdf_correction", {}).get("joint_patches", {}).keys(): + raise ValueError("retained_joint_has_urdf_correction") + if model is not None: + moving = {n for n, j in model.joints.items() if j.kind != "fixed"} + if set(payload["joints"]) | set(retained) != moving: + raise ValueError("compact JSON joints differ from the exported URDF") + if any(model.joints[n].mimic_joint is not None for n in retained): + raise ValueError("retained scope only supports independent active joints") + return retained + + +def out_of_scope_ancestors(retained, sdk_values, *, model=None, link=None): + relevant = set(retained) + if model is not None and link is not None: + parent = model.parent_joint_by_child.get(link) + relevant &= set() if parent is None else {j.name for j in model._chain(parent)} + outside = [] + for name in sorted(relevant): + row = retained[name] + index = row["sdk_channel"] + if index >= len(sdk_values) or sdk_values[index] != row["held_command"]: + outside.append(name) + return tuple(outside) + + +def require_retained_pose(retained, sdk_values, *, model=None, link=None): + outside = out_of_scope_ancestors(retained, sdk_values, model=model, link=link) + if outside: + raise ValueError(f"uncalibrated_ancestor_not_at_declared_hold:{','.join(outside)}") + + +def require_profile_held_pose(profile, model, sdk_values, link): + scope = profile_scope(profile) + if scope: + require_retained_pose(scope["retained_joints"], sdk_values, model=model, link=link) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py index 4ee6d75..9b43676 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py @@ -61,6 +61,7 @@ def prepare_standard_result(profile: CalibrationProfile, source: Path, def prepare_measured_result(profile, model, source_sha256, fit): moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"} + moving -= profile.retained_joints missing = moving - fit.output_mappings.keys() geometry = moving - fit.zero_offsets_rad.keys() if missing or geometry: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py index ca4add6..14a2ca7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py @@ -58,6 +58,10 @@ def validate_serialized_tag_holdout( independent = mapping.measured and not mimic_approximation and independent_joint_sources(payload) is None if independent: active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed") + if mapping.retained_joints: + from .partial_scope import retained_from_payload, require_retained_pose + retained_from_payload(payload, model) + active = tuple(name for name in active if name not in mapping.retained_joints) training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids} seen = set() errors: dict[str, list[tuple[float, float]]] = {role: [] for role in roles} @@ -73,6 +77,8 @@ def validate_serialized_tag_holdout( mount = installations[row.role] if mount.role != row.role or mount.link not in model.links: raise ValueError("frozen Tag installation has a different role/link") + if mapping.retained_joints: + require_retained_pose(mapping.retained_joints, row.sdk_values, model=model, link=mount.link) angles = mapping.evaluate(row.sdk_values, "", active_joints=active, directions_by_joint=row.sdk_directions_by_joint) for name, value in model.resolve_angles(angles, independent_mimic_angles=independent).items(): diff --git a/src/linkerhand_calibration/linkerhand_calibration/product.py b/src/linkerhand_calibration/linkerhand_calibration/product.py index 836a750..5fe6d14 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/product.py +++ b/src/linkerhand_calibration/linkerhand_calibration/product.py @@ -235,6 +235,7 @@ class ProductConfig: sdk_package_sha256: str = "" profile_config: Path | None = None profile_config_sha256: str = "" + resume_mode: str = "verify" @property def session_root(self) -> Path: @@ -300,13 +301,21 @@ def load_product_config( raise ValueError("can_interface is required for SocketCAN products") if check_can and not (Path("/sys/class/net") / can_interface).exists(): raise ValueError(f"CAN interface does not exist: {can_interface}") - elif sdk_transport != "hcan": - raise ValueError("sdk.transport must be socketcan or hcan") + elif sdk_transport not in {"hcan", "libcanbus"}: + raise ValueError("sdk.transport must be socketcan, hcan or libcanbus") + if sdk_transport == "libcanbus" and contract.typed_profile.sdk_adapter != "o30_ros": + raise ValueError("libcanbus requires its O30 ROS SDK adapter") + if contract.typed_profile.sdk_adapter == "o30_ros" and sdk_transport != "libcanbus": + raise ValueError("O30 calibration launch currently requires sdk.transport: libcanbus") sdk_setup: Path | None = None sdk_config: Path | None = None sdk_config_hash = "" sdk_python_package, sdk_package_hash = None, "" + if sdk_raw.get("setup") and sdk_transport != "hcan": + sdk_setup = _resolve_path(sdk_raw["setup"], workspace=root, name="sdk.setup") + if not sdk_setup.is_file(): + raise ValueError("vendor SDK overlay must exist; build the external SDK in this workspace") if sdk_raw.get("python_package"): sdk_python_package = _resolve_path(sdk_raw["python_package"], workspace=root, name="sdk.python_package") sdk_package_hash = str(sdk_raw.get("package_sha256", "")).lower() @@ -449,6 +458,9 @@ def load_product_config( repeatability_deg = float(release.get("static_repeatability_deg", 1.0)) if not 0.0 < repeatability_deg <= 5.0: raise ValueError("static_repeatability_deg must be in (0, 5]") + resume_mode = str(raw.get("resume_mode", "verify")) + if resume_mode not in {"verify", "passed"}: + raise ValueError("resume_mode must be verify or passed") return ProductConfig( path=source, workspace=root, @@ -482,4 +494,5 @@ def load_product_config( sdk_package_sha256=sdk_package_hash, profile_config=profile_config, profile_config_sha256=profile_config_hash, + resume_mode=resume_mode, ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py index 635e824..be205c4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/diagnostics.py @@ -15,6 +15,11 @@ class DiagnosticCapturePreset: DIAGNOSTIC_CAPTURE_PRESETS = { + "o30_thumb_roll": DiagnosticCapturePreset( + ProfileKey("O30", "right", "o30_right_18", 1), + (("thumb_cmc_roll_front", ("thumb_cmc_roll",)),), + label_zh="拇指 roll 多标签诊断", + ), "o6_thumb": DiagnosticCapturePreset( ProfileKey("O6", "right", "o6_right_8", 1), (("thumb_yaw_top", ("rh_thumb_cmc_yaw",)), diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py index 85b4674..b4ce4f0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py @@ -30,6 +30,8 @@ def _plain(value: Any) -> Any: def hand_profile_payload(profile: CalibrationProfile) -> dict[str, Any]: """Return the complete YAML contract accepted by ``load_hand_profile``.""" + if profile.quality.task_training_cycles: + raise ValueError("executed training decisions belong to the journal, not a reusable profile") command = _plain(asdict(profile.command)) vision = _plain(asdict(profile.vision)) for view in vision["views"]: @@ -57,6 +59,7 @@ def hand_profile_payload(profile: CalibrationProfile) -> dict[str, Any]: }, "joint_coverage": _plain(profile.joint_coverage), } + payload["quality"].pop("task_training_cycles") return payload diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py index 29c4e05..9cbb61f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py @@ -8,7 +8,8 @@ import re from typing import Any, Mapping import yaml -from ..core.domain.profile import ReferenceWaypoint, JointZeroSpec, KnownBaselineGeometry, JointLimitPolicy +from ..core.domain.profile import ReferenceWaypoint, ModelPreparation, ParentReferenceSpec, JointZeroSpec, KnownBaselineGeometry, JointLimitPolicy, RetainedCadScope +from ..core.domain.motion_path import MotionSegment from ..core import ( AcquisitionPolicy, @@ -56,7 +57,11 @@ def _validate_keys(payload): ("quality", QualityPolicy), ("scope", ScopePolicy), ("artifacts", ArtifactPolicy), ("acquisition", AcquisitionPolicy)): _keys(payload.get(name, {}), {f.name for f in fields(contract)}, name) + if "task_training_cycles" in payload.get("quality", {}): + raise ValueError("task_training_cycles are journal decisions, not profile configuration") _keys(payload.get("urdf", {}), {"authorized_fields", "limit_policies"}, "urdf") + for policy in payload.get("scope", {}).get("retained_cad_scopes", {}).values(): + _keys(policy, {f.name for f in fields(RetainedCadScope)}, "retained CAD scope") for policy in payload.get("urdf", {}).get("limit_policies", {}).values(): _keys(policy, {f.name for f in fields(JointLimitPolicy)}, "URDF limit policy") for view in payload.get("vision", {}).get("views", ()): @@ -65,6 +70,12 @@ def _validate_keys(payload): _keys(tag, {"id", "role", "fixed_reference", "link", "size_m"}, "Tag") for task in payload.get("motion", {}).get("tasks", ()): _keys(task, {f.name for f in fields(TaskSpec)}, "task") + if task.get("parent_reference") is not None: + _keys(task["parent_reference"], {f.name for f in fields(ParentReferenceSpec)}, "parent reference") + for preparation in task.get("model_preparations", ()): + _keys(preparation, {f.name for f in fields(ModelPreparation)}, "model preparation") + for segment in task.get("segments", ()): + _keys(segment, {f.name for f in fields(MotionSegment)}, "motion segment") for waypoint in payload.get("motion", {}).get("resume_verification_waypoints", ()): _keys(waypoint, {f.name for f in fields(ReferenceWaypoint)}, "reference waypoint") for spec in payload.get("motion", {}).get("joint_zero_references", {}).values(): @@ -77,7 +88,7 @@ def _validate_keys(payload): raise ValueError("measurement key and joint field differ") -def load_bundled_hand_profile(layout: str) -> CalibrationProfile: +def load_bundled_hand_profile(layout: str, *, scope: str | None = None) -> CalibrationProfile: """Resolve installed or source-tree YAML without constructing a model. No cache: a product hash check and the subsequently loaded contract must @@ -87,18 +98,19 @@ def load_bundled_hand_profile(layout: str) -> CalibrationProfile: raise ValueError("invalid bundled profile layout name") source = Path(__file__).resolve().parents[2] / "config" / "profiles" / f"{layout}.yaml" if source.is_file(): - return load_hand_profile(source) + return load_hand_profile(source, scope=scope) from ament_index_python.packages import get_package_share_directory - return load_hand_profile(Path(get_package_share_directory("linkerhand_calibration")) / "config" / "profiles" / f"{layout}.yaml") + return load_hand_profile(Path(get_package_share_directory("linkerhand_calibration")) / "config" / "profiles" / f"{layout}.yaml", scope=scope) -def load_hand_profile(path: str | Path) -> CalibrationProfile: +def load_hand_profile(path: str | Path, *, scope: str | None = None) -> CalibrationProfile: """Load the complete extension contract; no Python model hook is used.""" source = Path(path).expanduser().resolve() payload = _map(yaml.safe_load(source.read_text(encoding="utf-8")), str(source)) if int(payload.get("schema_version", 0)) != 1: raise ValueError("hand profile schema_version must be 1") _validate_keys(payload) + selected_scope = scope command = _map(payload.get("command"), "command") vision = _map(payload.get("vision"), "vision") motion = _map(payload.get("motion"), "motion") @@ -208,6 +220,18 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: preparation_groups=tuple(tuple(int(index) for index in group) for group in task.get("preparation_groups", ())), entry_waypoints=tuple(tuple((int(index), float(value)) for index, value in waypoint) for waypoint in task.get("entry_waypoints", ())), + exit_waypoints=tuple(tuple((int(index), float(value)) for index, value in waypoint) + for waypoint in task.get("exit_waypoints", ())), + model_preparations=tuple(ModelPreparation(str(item["joint"]), + tuple(map(float, item["command"])), + tuple(tuple(map(float, pose)) for pose in item["approach_commands"])) + for item in task.get("model_preparations", ())), + parent_reference=(ParentReferenceSpec(**task["parent_reference"]) + if task.get("parent_reference") is not None else None), + segments=tuple(MotionSegment(str(segment["key"]), + tuple((int(i), float(v)) for i, v in segment["start_commands"]), + tuple((int(i), float(v)) for i, v in segment["end_commands"]), + tuple(map(str, segment["joints"]))) for segment in task.get("segments", ())), ) for task in motion.get("tasks", ()) ) @@ -260,7 +284,8 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: {str(view): tuple(map(int, ids)) for view, ids in row["tag_ids_by_view"].items()}) for row in motion.get("resume_verification_waypoints", ())), {str(name): JointZeroSpec(str(row["task_key"]), tuple(map(float, row["command"])), - tuple(tuple(map(float, command)) for command in row.get("approach_commands", ()))) + tuple(tuple(map(float, command)) for command in row.get("approach_commands", ())), + str(row.get("evidence_scope", "arrival"))) for name, row in motion.get("joint_zero_references", {}).items()}, ), measurement=MeasurementPolicy( @@ -273,6 +298,7 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: _set(measurement.get("candidate_selection_tasks", ())), str(measurement.get("input_domain", "")), dict(measurement.get("transferred_motion_sources", {})), + str(measurement.get("release_basis", "feedback_and_command")), ), zero=ZeroSolvePolicy( _set(zero.get("active_joints")), @@ -300,6 +326,7 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: _set(quality.get("hard_threshold_keys")), dict(quality.get("retry_metric_scope", {})), bool(quality.get("isolated_holdout", False)), + str(quality.get("training_policy", "fixed")), ), scope=ScopePolicy( { @@ -310,7 +337,10 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: str(name): _set(values) for name, values in _map(scope.get("frozen_joints", {}), "scope.frozen_joints").items() }, - str(scope.get("default_scope", "full")), + str(selected_scope or scope.get("default_scope", "full")), + {str(name): RetainedCadScope(str(row["reason"]), tuple(row["root_anchor_joints"]), + str(row["orientation_anchor_joint"]), tuple(row.get("directed_base_axis_joints", ()))) + for name, row in scope.get("retained_cad_scopes", {}).items()}, ), artifacts=ArtifactPolicy( int(artifacts["output_schema_version"]), @@ -343,6 +373,8 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile: safety_evidence_source=str(row.get("safety_evidence_source", ""))) for name, row in _map(urdf.get("limit_policies", {}), "urdf.limit_policies").items()}, ) + from .scope import compile_capture_scope + profile = compile_capture_scope(profile) validate_profile(profile) return profile diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py index 5d2934c..caf3b51 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py @@ -77,3 +77,31 @@ def compile_parallel_axis_geometry(profile, model, relations): result.append(ParallelAxisGeometry(parent.joint, child.joint, 1 if axis_a @ axis_b > 0 else -1, distance)) return tuple(result) + + +def compile_tag_feedback_channels(profile, model): + """Feedback channels able to move each physical Tag, including held ancestors. + + A distal joint or a sibling finger cannot move an upstream Tag. Passive + joints inherit their declared SDK source. This uses topology and mounting + links only; it does not assume CAD zero angles or a command/angle map. + """ + from ..core.fitting.motion_fit import channel_for_joint + + channels = { + profile.command.urdf_joint_by_joint.get(name, name): channel_for_joint(profile, name) + for name in (*profile.zero.active_joints, *profile.zero.passive_joints) + } + result = {} + for role, link in compile_tag_links(profile, model).items(): + parent = model.parent_joint_by_child.get(link) + chain = () if parent is None else model._chain(parent) + dependencies = set() + for joint in chain: + if joint.kind == "fixed": + continue + if joint.name not in channels: + raise ValueError(f"Tag motion dependency has no SDK source:{role}:{joint.name}") + dependencies.add(channels[joint.name]) + result[role] = frozenset(dependencies) + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py new file mode 100644 index 0000000..3838d8a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/scope.py @@ -0,0 +1,65 @@ +"""Compile a declared partial scope once, before scheduling or fitting. + +The SDK layout and avoidance paths remain complete. Excluded joints have no +motion table or measured zero; their CAD geometry is an explicit prerequisite +for the held-pose partial geometry checks, never an independently measured fact. +""" + +from dataclasses import replace +from pathlib import PurePath +import re + + +def compile_capture_scope(profile): + name = profile.scope.default_scope + policy = profile.scope.retained_cad_scopes.get(name) + if policy is None: + return profile + if re.fullmatch(r"[a-z][a-z0-9_]*", name) is None: + raise ValueError("invalid partial scope name") + retained = profile.scope.frozen_joints[name] + selected = profile.scope.calibrate_joints[name] + if retained & selected or retained | selected != profile.zero.active_joints: + raise ValueError("partial scope must partition active joints") + tasks = [] + for task in profile.motion.tasks: + if set(task.joints) & retained: + if not set(task.joints) <= retained: + raise ValueError("partial scope cannot split a coupled motion task") + continue + if any(p.joint in retained for p in task.model_preparations): + raise ValueError("partial scope removes a required preparation measurement") + if task.parent_reference is not None and retained & { + task.parent_reference.pose_joint, task.parent_reference.geometry_joint}: + raise ValueError("partial scope removes a required parent reference source") + tasks.append(task) + def keep(mapping): + return {key: value for key, value in mapping.items() if key not in retained} + zero = profile.zero + spatial = dict(zero.spatial) + spatial.update(root_anchor_joints=list(policy.root_anchor_joints), + orientation_anchor_joint=policy.orientation_anchor_joint, parallel_root_pattern=False, + directed_base_axis_joints=list(policy.directed_base_axis_joints)) + spatial["axis_order"] = [j for j in spatial.get("axis_order", zero.axis_joints) if j not in retained] + for key in ("axis_parent_joint", "phase_parent_joint", "offset_observer_joint"): + spatial[key] = {j: parent for j, parent in spatial.get(key, {}).items() + if j not in retained and parent not in retained} + def filename(value): + path = PurePath(value) + suffix = "_" + name + return value if path.stem.endswith(suffix) else path.stem + suffix + path.suffix + return replace(profile, + 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_frozen_joints=zero.cad_frozen_joints | retained, spatial=spatial), + artifacts=replace(profile.artifacts, + directional_command_joints=profile.artifacts.directional_command_joints - retained, + calibration_filename=filename(profile.artifacts.calibration_filename), + corrected_urdf_filename=filename(profile.artifacts.corrected_urdf_filename), + publication_pointer=f"latest_{name}_passed"), + urdf_authorized_fields=keep(profile.urdf_authorized_fields), + urdf_limit_policies=keep(profile.urdf_limit_policies), + joint_coverage={**profile.joint_coverage, **{j: "cad_nominal" for j in retained}}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py index 758bef8..9bc6398 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py @@ -4,6 +4,7 @@ from ..core.domain.profile import validate_profile from ..core.fitting.session import compile_spatial_profile from ..core.urdf.kinematics import UrdfKinematicModel from .observations import compile_tag_links +from ..core.domain.motion_path import task_segments def require_release_geometry(report): @@ -26,6 +27,7 @@ def validate_executable_profile(profile, source_urdf): model = UrdfKinematicModel(source_urdf) spatial = compile_spatial_profile(profile) links = compile_tag_links(profile, model) + _validate_parent_references(profile, model) for joint, observer in spatial.offset_observer_joint.items(): if joint == observer: raise ValueError(f"absolute_zero_unobservable:{joint}:a free Tag on its own hinge is not a CAD datum") @@ -69,11 +71,12 @@ def validate_executable_profile(profile, source_urdf): zero_transfers = {target for target, source in profile.zero.transferred_zero_sources.items() if transfers.get(target) == source} coverage = {"unmeasured_joints": sorted(moving-measured), - "unresolved_motion_joints": sorted(moving-measured-set(transfers)), + "unresolved_motion_joints": sorted(moving-measured-set(transfers)-profile.retained_joints), + "retained_uncalibrated_joints": sorted(profile.retained_joints), "transferred_motion_sources": dict(transfers), "missing_baseline_geometry": sorted((profile.zero.active_joints | profile.zero.passive_joints) -set(profile.zero.direct_zero_joints)-set(profile.zero.known_baseline_geometry) - -set(profile.zero.cad_zero_assumptions)-zero_transfers), + -set(profile.zero.cad_zero_assumptions)-zero_transfers-profile.retained_joints), "transferred_joints": sorted(set(transfers)|set(profile.zero.transferred_zero_sources) |set(profile.zero.transferred_mimic_sources))} return {"profile_id": profile.key.profile_id, "tag_links": links, @@ -90,6 +93,30 @@ def validate_executable_profile(profile, source_urdf): "arbitrary_multiaxis_validated": False} +def _validate_parent_references(profile, model): + from .observations import compile_parallel_axis_geometry, compile_tag_feedback_channels + from ..core.geometry.tag_pose.motion_evidence import MotionRelation + + channels = None + for task in profile.motion.tasks: + reference = task.parent_reference + if reference is None: + continue + if channels is None: + channels = compile_tag_feedback_channels(profile, model) + relations = tuple(MotionRelation(name, profile.measurement.measurements[name].parent_role, + profile.measurement.measurements[name].child_role) + for name in (reference.geometry_joint, *task.joints)) + constraints = compile_parallel_axis_geometry(profile, model, relations) + if len(constraints) != 1 or constraints[0].parent_joint != reference.geometry_joint: + raise ValueError(f"parent_reference_requires_adjacent_parallel_axes:{task.key}") + source = profile.motion.joint_zero_references[reference.pose_joint].command + target = profile.motion.joint_zero_references[task.joints[0]].command + role = profile.measurement.measurements[reference.pose_joint].child_role + if any(source[index] != target[index] for index in channels[role]): + raise ValueError(f"parent_reference_holding_pose_changed:{task.key}") + + def _validate_transfer_geometry(profile, model): """Check the declared strategy against CAD signs and actual joint types.""" from ..core.fitting.motion_fit import joint_input_direction @@ -114,6 +141,9 @@ def _validate_separable_observations(profile, model, links): for name in task.joints: spec = profile.measurement.measurements[name] relative_chain = chain(links[spec.parent_role]) ^ chain(links[spec.child_role]) - moving = {j for j in relative_chain if channel_for_joint(profile, j) == task.command_index} - if moving != {name}: - raise ValueError(f"joint_zero_motion_not_separable:{name}:coupled_between_tags={sorted(moving)}") + for segment in task_segments(task): + if name not in segment.joints: + continue + moving = {j for j in relative_chain if channel_for_joint(profile, j) in segment.moving_channels} + if moving != {name}: + raise ValueError(f"joint_zero_motion_not_separable:{name}:coupled_between_tags={sorted(moving)}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py index 0b7ad99..2578107 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py @@ -9,16 +9,20 @@ from typing import Any, Mapping, Sequence from ..core.domain.profile import CalibrationProfile from ..core.fitting.observed_motion import image_identity +from ..core.domain.motion_path import record_scan_identity, task_segment +from ..core.fitting.motion_fit import channel_for_joint -def load_capture(path): +def load_capture(path, *, verify_evidence=True): + from ..core.geometry.frozen_evidence import EvidenceDecoder + decoder = EvidenceDecoder(verify_hashes=verify_evidence) rows = [] with Path(path).open(encoding="utf-8") as stream: for number, line in enumerate(stream, 1): if not line.strip(): continue try: - row = json.loads(line) + row = decoder.loads(line) except json.JSONDecodeError as error: raise ValueError(f"invalid calibration JSONL at line {number}") from error if not isinstance(row, dict): @@ -39,7 +43,7 @@ def accepted_joint_records(profile: CalibrationProfile, records: Sequence[Mappin samples = [row for row in records if row.get("joint") in expected and "relative_quaternion_xyzw" in row and row.get("sample_phase", "sweep") == sample_phase] def unit(row): - return str(row["task_name"]), int(row["cycle"]), str(row["direction"]) + return record_scan_identity(row) latest = {} for row in records: if not all(key in row for key in ("task_name", "cycle", "direction")): @@ -56,6 +60,15 @@ def accepted_joint_records(profile: CalibrationProfile, records: Sequence[Mappin task = tasks.get(str(row["task_name"])) if task is None or row["joint"] not in task.joints: raise ValueError("retained observation is not authorized by its task") + if task.segments: + segment = task_segment(task, row.get("segment_key", "")) + channel = channel_for_joint(profile, row["joint"]) + if row["joint"] not in segment.joints or row.get("motor_index") != channel or row["direction"] != segment.direction: + raise ValueError("retained segment observation differs from its motor or direction") + vector = row[f"command_vector_{profile.command.unit}"] + expected = segment.commands_at(vector[segment.command_index], quantize=profile.command.unit == "u8") + if any(abs(vector[i]-value) > 1 for i, value in expected.items()): + raise ValueError("retained command differs from declared multi-channel path") if directions_are_task_relative and task.end_value > task.start_value: row["direction"] = {"increasing": "decreasing", "decreasing": "increasing"}[row["direction"]] row["sample_id"] = image_identity(row) @@ -63,7 +76,11 @@ def accepted_joint_records(profile: CalibrationProfile, records: Sequence[Mappin if identity in seen: raise ValueError(f"duplicate retained image:{identity}") seen.add(identity) - if profile.command.unit == "u8": + if profile.vision_motion: + value = row[f"command_{profile.command.unit}"] + if not math.isfinite(float(value)) or not profile.command.minimum_values[channel_for_joint(profile, row["joint"])] <= value <= profile.command.maximum_values[channel_for_joint(profile, row["joint"])]: + raise ValueError("retained command outside declared domain") + elif profile.command.unit == "u8": value = float(row[profile.curve_input_domain]) if not math.isfinite(value) or not 0 <= value <= 255: raise ValueError("retained feedback outside byte domain") @@ -96,11 +113,11 @@ def accepted_secondary_records(profile, records, *, directions_are_task_relative for row in records: if not all(key in row for key in ("task_name", "cycle", "direction")): continue - key = (row["task_name"], int(row["cycle"]), row["direction"]) + key = record_scan_identity(row) latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) output, seen = {}, set() for row in samples: - key = (row["task_name"], int(row["cycle"]), row["direction"]) + key = record_scan_identity(row) if int(row.get("attempt", 1)) != latest[key]: continue name = row["observation_joint"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/command_seed.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/command_seed.py new file mode 100644 index 0000000..58fb411 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/command_seed.py @@ -0,0 +1,61 @@ +"""Read the native command register before the runner starts its SDK owner. + +Never initialize posture, change a setting, or infer commands from positions. +The live health identity must match this read-only startup observation. +""" + +import argparse +import json +import math +from pathlib import Path +import time + +from ...storage import atomic_write_json + + +def read_o30_command(*, side, controller_factory=None): + if controller_factory is None: + from linker_hand_o30_ros2_sdk.core.canfd.linker_hand_o30_control import LinkerHandO30Controller + controller_factory = LinkerHandO30Controller + controller = controller_factory(hand_type=side, canfd_device=0, + comm_type="libcanbus", probe_sensor=False) + try: + identity = controller.hand_info + target = controller.get_target_position() + if (not isinstance(identity, dict) or "O30" not in str(identity.get("产品型号", "")) + or identity.get("左右手") != side.upper() or not identity.get("设备唯一标识")): + raise ValueError("initial_command_device_identity_invalid") + if (target is None or len(target) != 20 + or any(not math.isfinite(v) or not 0 <= v <= 255 for v in target)): + raise ValueError("initial_command_register_unavailable") + return dict(schema_version=1, source="sdk_target_position_register", + commands_sent=False, created_unix=time.time(), model="O30", side=side, + device_uid=identity["设备唯一标识"], values=list(target)) + finally: + controller.close() + + +def load_command_seed(path, profile): + if not path: + return (), "" + row = json.loads(Path(path).read_text()) + values = tuple(float(v) for v in row["values"]) + if (row.get("source") != "sdk_target_position_register" or row.get("commands_sent") is not False + or row.get("model") != profile.key.model or row.get("side") != profile.key.side + or not row.get("device_uid") or len(values) != profile.command.command_count + or any(not math.isfinite(v) or not low <= v <= high + for v, low, high in zip(values, profile.command.minimum_values, profile.command.maximum_values))): + raise ValueError("initial_command_seed_invalid") + return values, str(row["device_uid"]) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--side", choices=("left", "right"), required=True) + parser.add_argument("--output", required=True) + args = parser.parse_args() + atomic_write_json(args.output, read_o30_command(side=args.side)) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py new file mode 100644 index 0000000..0e43a78 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o30_ros.py @@ -0,0 +1,67 @@ +"""Read-only boundary to the separately installed O30 ROS SDK.""" + +import json +import math + +from .base import HardwareHealth +from .legacy_byte_sdk import LegacyByteSdkAdapter + + +HARDWARE_FAULTS = frozenset({"执行器异常", "执行器离线", "执行器过流", "执行器过温"}) + + +def launch_parameters(*, side="right", transport="libcanbus", device=0): + return {"hand_type": side, "hand_joint": "O30", "comm_type": transport, + "canfd_device": device, "auto_init_pose": False, "is_touch": False, + "init_velocity": 200, "init_torque": 200, + "strict_device_check": True, "ignore_joint_faults": False, + "joint_limit_min": [0]*20, "joint_limit_max": [255]*20, + "cmd_timeout": 0.0, "state_rate": 30.0, "info_rate": 1.0, + "publish_velocity": False, "publish_effort": False} + + +class O30RosAdapter(LegacyByteSdkAdapter): + """Keep native bytes and validate the SDK's existing diagnostic identity.""" + + def __init__(self, profile, ports, *, publish, set_speed): + self.profile, self.ports = profile, ports + self.info, self.info_at, self.info_error = None, float("-inf"), "" + super().__init__(profile.command, publish=publish, set_speed_callback=set_speed, + health_callback=self._health_report) + ports.subscribe_health(self.receive_info) + + def receive_info(self, payload): + try: + data = json.loads(payload) + if (not isinstance(data, dict) or data.get("hand_type") != self.profile.key.side + or "O30" not in str(data.get("model", "")).upper() + or self.profile.key.side.upper() not in str(data.get("side", "")).upper() + 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") + if self.info is not None and data["uid"] != self.info["uid"]: + raise ValueError("o30_sdk_device_changed") + faults = data.get("joint_faults", {}) + if not isinstance(faults, dict) or any(not isinstance(entries, list) + or not all(isinstance(value, str) for value in entries) for entries in faults.values()): + raise ValueError("o30_sdk_fault_report_invalid") + self.info, self.info_at, self.info_error = data, self.ports.monotonic(), "" + except (ValueError, TypeError, AttributeError) as error: + self.info_error = str(error) + + def parse_feedback(self, names, values): + if not names or len(set(names)) != 20 or set(names) != set(self.command_layout.names): + return None + result = super().parse_feedback(names, values) + return result if result is not None and all(math.isfinite(v) and 0 <= v <= 255 for v in result) else None + + def _health_report(self): + fresh = self.ports.feedback_fresh() + if self.info_error or self.info is None or self.ports.monotonic()-self.info_at > 3.0: + return HardwareHealth(False, True, feedback_fresh=fresh, + diagnostic=self.info_error or "waiting_for_o30_sdk_info") + faults = tuple(f"{name}:{fault}" for name, entries in self.info.get("joint_faults", {}).items() + for fault in entries if fault in HARDWARE_FAULTS) + return HardwareHealth(self.info["online"], True, active_faults=faults, feedback_fresh=fresh, + diagnostic=json.dumps(self.info, ensure_ascii=False)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py index 0c17292..30fa15f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py @@ -7,6 +7,7 @@ from typing import Callable from .base import HardwareHealth from .legacy_byte_sdk import LegacyByteSdkAdapter from .o12_hcan_sdk import O12HcanSdkAdapter +from .o30_ros import O30RosAdapter @dataclass(frozen=True) @@ -56,7 +57,9 @@ def _hcan(profile, ports, publish, set_speed): health_callback=health, clock=ports.monotonic) -FACTORIES = {"legacy_byte_sdk": _byte, "o12_hcan_sdk": _hcan} +FACTORIES = {"legacy_byte_sdk": _byte, "o12_hcan_sdk": _hcan, + "o30_ros": lambda profile, ports, publish, set_speed: O30RosAdapter( + profile, ports, publish=publish, set_speed=set_speed)} def bind_ros_sdk(profile, ports: SdkBindingPorts, *, publish, set_speed): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py index 42ca38d..7357b12 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py @@ -9,10 +9,15 @@ from ...core.domain.profile import CalibrationProfile class SdkTopics: command: str feedback: str + setting: str = "" + health: str = "" def sdk_topics(profile: CalibrationProfile) -> SdkTopics: prefix, side = f"/{profile.key.model.lower()}", profile.key.side + if profile.sdk_adapter == "o30_ros": + return SdkTopics(f"/cb_{side}_hand_control_cmd", f"/cb_{side}_hand_state", + f"/cb_{side}_hand_setting_cmd", f"/cb_{side}_hand_info") if profile.sdk_adapter == "legacy_byte_sdk": return SdkTopics(f"{prefix}/cb_{side}_hand_control_cmd", f"{prefix}/cb_{side}_hand_state") if profile.sdk_adapter == "o12_hcan_sdk": diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py new file mode 100644 index 0000000..30762bd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/capture_validation.py @@ -0,0 +1,103 @@ +"""Capture integrity shared by live completion and offline publication. + +This gate runs at finalization, never on the fast passed-unit resume path. +It reads retained evidence; it does not command the hand or refit old images. +""" + +from collections import defaultdict +import hashlib +import json + +from ...core.domain.motion_path import record_scan_identity +from ..diagnostic_capture import reject_diagnostic_capture +from ..engine import CalibrationEngine +from ..resume import ResumeVerifier, fingerprint_from_mapping +from ..scan_quality import evaluate_capture_unit + + +def validate_capture_provenance(profile, serial_number, hashes, records): + """A journal cannot substitute today's hashes for old evidence.""" + reject_diagnostic_capture(records) + 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: + raise ValueError("capture_provenance:requires_unique_session_and_locked_reference") + header, reference = headers[0], references[0] + engine = CalibrationEngine(profile) + if (not engine.capture_compatible(header) or header.get("serial_number") != serial_number + or header.get("curve_input_domain") != profile.curve_input_domain): + raise ValueError("capture_provenance:obsolete_policy_or_coordinate_identity; fresh capture required") + for key, value in hashes.items(): + if header.get(key) != value or reference.get("protected_hashes", {}).get(key) != value: + raise ValueError(f"capture_provenance:protected_input_changed:{key}") + fingerprint = fingerprint_from_mapping(reference) + checker = ResumeVerifier(required_fixed_views=profile.vision.view_names, + required_fixed_poses=profile.vision.view_names, required_hashes=(*hashes, "intrinsics_sha256")) + evidence = checker.compare(fingerprint, fingerprint) + if not evidence.reuse or fingerprint.profile_id != profile.key.profile_id: + raise ValueError(f"capture_provenance:invalid_locked_reference:{evidence.incompatible_fields}") + models = {} + # Only preview models preceding the formal reference are relevant. Later + # camera changes are rejected online and cannot authorize a mixed replay. + for row in records: + if row.get("kind") == "fixed_base_reference_locked": + break + if row.get("kind") == "rectified_camera_model": + if row.get("matrix_source") != "CameraInfo.P[:3,:3]" or row.get("input_is_rectified") is not True: + raise ValueError("capture_provenance:unverified_rectified_projection") + models[row["view"]] = row["camera_matrix"] + intrinsic_hash = hashlib.sha256(json.dumps(dict(sorted(models.items())), sort_keys=True).encode()).hexdigest() + if set(models) != set(profile.vision.view_names) or intrinsic_hash != fingerprint.protected_hashes["intrinsics_sha256"]: + raise ValueError("capture_provenance:intrinsics_evidence_changed_or_missing") + + +def validate_capture_units(profile, records): + """Offline and resumed data must satisfy the actual post-sweep policy.""" + from ...core.fitting.command_sampling import validate_sampling_plans + from ..training import resolve_capture_plan + profile = resolve_capture_plan(profile, records, require_complete=True) + plans = validate_sampling_plans(profile, records) + by_unit, by_task = defaultdict(list), defaultdict(list) + for row in records: + by_unit[record_scan_identity(row)].append(row) + by_task[row.get("task_name")].append(row) + first_spans = {} + for unit in CalibrationEngine(profile).scan_units(): + key = unit.identity + rows = by_unit[key] + attempt = max((int(row.get("attempt", 1)) for row in rows), default=1) + completions = [row for row in rows if row.get("kind") == "scan_unit_complete" + and int(row.get("attempt", 1)) == attempt] + complete = bool(completions) and all(row.get("passed") is True for row in completions) + quality = evaluate_capture_unit(profile, unit, attempt, rows, first_cycle_spans=first_spans) + if not complete or not quality.passed: + failures = list(quality.failures) + if not complete: + failures.insert(0, "latest_attempt_not_completed" if not completions + else "latest_attempt_failed_or_conflicting") + raise ValueError(f"capture_incomplete:{key}:attempt={attempt}:{tuple(failures)}") + if profile.artifacts.output_schema_version >= 3: + from ..task_quality import evaluate_task_input_support + for task in profile.motion.tasks: + quality = evaluate_task_input_support(profile, task, by_task[task.key]) + if not quality.passed: + raise ValueError(f"task_input_support_failed:{task.key}:{quality.failures}") + return plans + + +def validate_capture_for_release(profile, serial_number, protected_inputs, records): + """Check the same immutable inputs and complete capture at either entry.""" + missing = profile.artifacts.protected_input_fields - protected_inputs.keys() + if missing: + raise ValueError(f"capture_provenance:protected_inputs_missing:{sorted(missing)}") + # Intrinsics are locked after the header; raw-journal hashes are produced + # after recording. Neither is a startup protected-file hash. + hashes = {key: value for key, value in protected_inputs.items() + if key not in {"intrinsics_sha256", "raw_samples_sha256"}} + validate_capture_provenance(profile, serial_number, hashes, records) + reference = next(row for row in records if row.get("kind") == "fixed_base_reference_locked") + expected_intrinsics = protected_inputs.get("intrinsics_sha256") + if (expected_intrinsics is not None + and expected_intrinsics != reference["protected_hashes"]["intrinsics_sha256"]): + raise ValueError("capture_provenance:protected_input_changed:intrinsics_sha256") + return validate_capture_units(profile, records) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py index 9e83166..e77f687 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py @@ -47,6 +47,11 @@ class FinalizationController: self._publisher = ArtifactPublisher(session_dir.parent, profile.artifacts.publication_pointer) self.started = False + @property + def uses_journal(self): + """Whether complete evidence is read from the durable journal.""" + return self._isolated + def start(self, inputs: FinalizationInputs) -> None: transport = {} if inputs.camera_extrinsics_file is not None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py index b2e807d..ed3d31b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/direction.py @@ -15,9 +15,13 @@ def next_input_directions(values, previous, directions, *, resolution, channels, if previous is None: if values[index] != baseline: raise ValueError(f"directional_mapping_requires_baseline_initialization:channel={index}") - if baseline not in (lower, upper): - raise ValueError("directional_mapping_requires_endpoint_baseline") - result[index] = "increasing" if baseline == upper else "decreasing" + if not lower <= baseline <= upper: + raise ValueError("directional_mapping_baseline_outside_support") + # Both measured branches are anchored to zero at the recorded + # baseline. An interior baseline has no inferred arrival direction; + # the first actual change establishes it, and holds retain it. + result[index] = ("increasing" if baseline == upper else + "decreasing" if baseline == lower else "") if previous is not None: for index, (old, new) in enumerate(zip(previous, values)): tolerance = resolution @@ -30,7 +34,7 @@ def next_input_directions(values, previous, directions, *, resolution, channels, if abs(new-old) <= tolerance: continue direction = "increasing" if new > old else "decreasing" - if index in channels and direction != result[index]: + if index in channels and result[index] and direction != result[index]: _, lower, upper = channels[index] if min(abs(old-lower), abs(old-upper)) > tolerance: raise ValueError(f"directional_mapping_interior_reversal_unvalidated:channel={index}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py index 9b7aaac..61a0ad9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py @@ -7,6 +7,7 @@ from typing import Any, Mapping, Sequence from ...core.domain.profile import CalibrationProfile +from ...core.domain.capture_plan import training_cycles from ...core.domain.result import JointMapping, evaluate_mappings from ...core.fitting.tag_installation import ( FrozenTagInstallation, TagTrainingPose, fit_tag_installations, matrix_tuple, register_base_translation, @@ -50,9 +51,12 @@ def _replay_directions(mappings, row, task, *, directions_are_task_relative=Fals valid = all(value in {"", "increasing", "decreasing"} for value in history) if not covered or not valid: raise ValueError("Tag replay has an invalid SDK direction history") + from ...core.domain.motion_path import task_segment + moving_channels = (task_segment(task, row["segment_key"]).moving_channels + if row.get("segment_key") else (task.command_index,)) directions = {} for joint, mapping in mappings.items(): - if mapping.motor_index == task.command_index: + if mapping.motor_index in moving_channels: directions[joint] = direction else: directions[joint] = history[mapping.motor_index] if history else "" @@ -109,13 +113,16 @@ def prepare_tag_replay( name = str(row.get("joint", "")) if name not in specs or specs[name].view is None: continue - if int(row.get("cycle", -1)) not in {0, 1, 2, 3}: + if int(row.get("cycle", -1)) not in (*training_cycles(profile, joint=name), profile.quality.holdout_cycle): continue spec = specs[name] if row.get("task_name") not in tasks: raise ValueError("Tag replay record has no declared task") task = tasks[str(row["task_name"])] role = str(spec.child_role) + if profile.retained_joints: + from ...core.urdf.partial_scope import require_profile_held_pose + require_profile_held_pose(profile, source_model, row[f"command_vector_{profile.command.unit}"], links[role]) if row.get("view") != spec.view: raise ValueError("Tag replay record has a different view") identity = str(row.get("sample_id", "")) @@ -141,7 +148,7 @@ def prepare_tag_replay( raise ValueError(f"conflicting duplicate capture:{role}:{identity}") continue unique[key] = signature - if cycle == 3: + if cycle == profile.quality.holdout_cycle: holdout.append(TagHoldout(identity, role, cycle, sdk, directions, pose)) continue angles = evaluate_mappings(output_mappings, sdk, directions) @@ -167,5 +174,8 @@ def prepare_tag_replay( registered_base = register_base_translation(source_model=source_model, common_from_base=common_from_base, link_by_role=selected_links, observations=training) mounts = fit_tag_installations(source_model=source_model, common_from_base=registered_base, - link_by_role=selected_links, observations=training) + link_by_role=selected_links, observations=training, + training_cycles_by_role={role: tuple(sorted({cycle for name, spec in specs.items() + if spec.child_role == role and spec.view is not None for cycle in training_cycles(profile, joint=name)})) + for role in required}) return TagReplayEvidence(registered_base, mounts, tuple(holdout), tuple(sorted(required))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py index 3941ebb..79a55d4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py @@ -7,6 +7,7 @@ from pathlib import Path from typing import Callable from ...core.artifacts.storage import atomic_write_json +from ...core.domain.capture_plan import CapturePlan, training_cycles from ...core.fitting.session import fit_profile_calibration from ...core.fitting.command_mapping import fit_command_mappings from ...core.fitting.motion_fit import channel_for_joint @@ -41,6 +42,7 @@ def _write_artifact_pair(profile, fit, prepared, directory, source, serial_numbe if profile.artifacts.output_schema_version >= 3: from .serializers.measured_v3 import serialize_report as report_writer, from_report as command_writer report = report_writer(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs) + report["capture_plan"] = CapturePlan.from_profile(profile).as_dict() report["urdf_correction"] = serialize_urdf_correction(prepared.plan) if prepared.plan.range_evidence: report["motion_range_evidence"] = {name: asdict(item) for name, item in prepared.plan.range_evidence.items()} @@ -81,8 +83,28 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf if cancelled(): raise ValueError("operator_abort:no_artifact_publication") check_cancelled() - from ...core.fitting.command_sampling import validate_sampling_plans - sampling_plans = validate_sampling_plans(profile, records) + from ..training import resolve_capture_plan + profile = resolve_capture_plan(profile, records, source_urdf=source, + verify_decisions=True, require_complete=True) + from ..motion_provenance import uses_motion_evidence + capture_evidence_required = (require_motion_evidence or uses_motion_evidence(records) + or any(row.get("kind") == "session_start" for row in records)) + try: + if capture_evidence_required: + from .capture_validation import validate_capture_for_release + sampling_plans = validate_capture_for_release(profile, serial_number, protected_inputs, records) + else: + # Legacy pose-only callers retain their numerical contract. + # Both product entry points explicitly require capture evidence. + from ...core.fitting.command_sampling import validate_sampling_plans + sampling_plans = validate_sampling_plans(profile, records) + except ValueError as error: + atomic_write_json(directory / "capture_validation.json", { + "passed": False, "reason": str(error), "publication_allowed": False}) + raise + if capture_evidence_required: + atomic_write_json(directory / "capture_validation.json", { + "passed": True, "scope": "provenance_and_complete_latest_attempts"}) if sampling_plans: atomic_write_json(directory / "command_sampling_plans.json", sampling_plans) from ..motion_provenance import validate_source_geometry @@ -96,6 +118,7 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf try: records, report = resolve_chain_observations(records, task_key=task.key, require_frozen_branches=True, + training_cycles=training_cycles(profile, task), holdout_cycle=profile.quality.holdout_cycle, fixed_roles={tag.role for view in profile.vision.views for tag in view.tags if tag.fixed_reference}, role_pairs={name: (profile.measurement.measurements[name].parent_role, profile.measurement.measurements[name].child_role) for name in task.joints}) @@ -127,11 +150,14 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf "geometry_alignment_certified": False, "publication_allowed": False, "curves": {name: asdict(curve) for name, curve in motion.observed_curves.items()}, "holdout_errors_rad": motion.holdout_errors_rad}) - fit = fit_profile_calibration(profile, source, accepted, cross_view_records=secondary, - zero_references=references, motion_fitted=save_relative_motion) steady = accepted_joint_records(profile, records, sample_phase="steady", directions_are_task_relative=directions_are_task_relative) - fit = fit_command_mappings(profile, fit, steady) + fit = fit_profile_calibration(profile, source, accepted, cross_view_records=secondary, + zero_references=references, steady_records=steady, motion_fitted=save_relative_motion) + if not profile.command_based_release: + fit = fit_command_mappings(profile, fit, steady) + if fit.feedback_diagnostics: + atomic_write_json(directory / "feedback_mapping_diagnostics.json", fit.feedback_diagnostics) if profile.artifacts.output_schema_version >= 3: from ...core.fitting.motion_transfer import expand_profile_transfers fit = expand_profile_transfers(profile, fit) @@ -156,7 +182,7 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf profile, fit, prepared, directory, source, serial_number, protected_inputs) phase_changed("artifacts_built") zero = fit.spatial_zero - replay_records = [row for rows in accepted.values() for row in rows] + replay_records = [row for rows in (steady if profile.command_based_release else accepted).values() for row in rows] replay_records.extend(dict(row, joint=profile.measurement.cross_view_sources[name]) for name, rows in secondary.items() for row in rows) from ...core.fitting.tag_installation import TagInstallationFailure @@ -181,26 +207,29 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf steady_rows.extend(dict(r, joint=profile.measurement.cross_view_sources[name]) for name, rows in steady_secondary.items() for r in rows) command_holdout = prepare_command_holdout(profile, fit.command_mappings, steady_rows) - from ..motion_provenance import uses_motion_evidence image_inputs = {} - if require_motion_evidence or uses_motion_evidence(records): + if capture_evidence_required: if camera_extrinsics_file is None: raise ValueError("final_image_holdout_requires_protected_camera_file") from .image_evidence import ImageEvidence image_evidence = ImageEvidence(profile, records, camera_extrinsics_file=camera_extrinsics_file, - protected_inputs=protected_inputs) + protected_inputs=protected_inputs, source_model=UrdfKinematicModel(source)) image_inputs = { - "image_observations": image_evidence.all_view_holdout(replay.holdout, - mappings=fit.output_mappings, input_kind="feedback", holdout_cycle=profile.quality.holdout_cycle), + "image_observations": (() if profile.command_based_release else image_evidence.all_view_holdout(replay.holdout, + mappings=fit.output_mappings, input_kind="feedback", holdout_cycle=profile.quality.holdout_cycle)), "command_image_observations": image_evidence.all_view_holdout(command_holdout, mappings=fit.command_mappings, input_kind="command", holdout_cycle=command_validation_cycle), "require_image_holdout": True, } + image_inputs["out_of_scope_images"] = tuple(image_evidence.out_of_scope_images) + from ...core.urdf.partial_scope import profile_scope validator = FrozenTagArtifactValidator(profile.key.profile_id, source, protected_inputs["source_urdf_sha256"], profile.urdf_authorized_fields, fit.zero_offsets_rad, replay.common_from_base, replay.installations, replay.holdout, replay.required_roles, standard_loader, protected_inputs=dict(protected_inputs), - command_observations=command_holdout, + command_observations=command_holdout, release_basis=profile.measurement.release_basis, + calibration_scope=profile_scope(profile), + capture_plan=CapturePlan.from_profile(profile).as_dict(), joint_observations=tuple(r for rows in steady.values() for r in rows if r["cycle"] == command_validation_cycle), command_holdout_cycle=command_validation_cycle, joint_curves=fit.curves, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py index c07fdd9..7be3f8b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/image_evidence.py @@ -7,6 +7,7 @@ from pathlib import Path import numpy as np +from ...core.domain.motion_path import record_scan_identity from ...core.geometry.extrinsics import camera_info_fingerprint, load_camera_extrinsics from ...core.urdf.image_acceptance import TagImageObservation from ..image_capture import IMAGE_OBSERVATION_POLICY @@ -21,11 +22,15 @@ class ImageEvidence: without manufacturing a PnP pose or fitting against a holdout image. """ - def __init__(self, profile, records, *, camera_extrinsics_file, protected_inputs): + def __init__(self, profile, records, *, camera_extrinsics_file, protected_inputs, source_model=None): path = Path(camera_extrinsics_file) if hashlib.sha256(path.read_bytes()).hexdigest() != protected_inputs["camera_extrinsics_sha256"]: raise ValueError("final_image_camera_extrinsics_changed") self.profile = profile + if profile.retained_joints and source_model is None: + raise ValueError("partial_image_scope_requires_source_topology") + self.source_model = source_model + self.out_of_scope_images = [] self.extrinsics = load_camera_extrinsics(path, required_views=profile.vision.view_names, reference_view=profile.vision.extrinsic_reference_view, quality_limits=profile.vision.extrinsics_quality_limits, @@ -41,7 +46,7 @@ class ImageEvidence: target = self.frames if kind == "pnp_candidate_frame" else self.raw_frames self._bind(target, f"{row['view']}:{row['image_stamp_ns']}", row) elif kind == "scan_unit_complete": - key = (row["task_name"], row["cycle"], row["direction"]) + key = record_scan_identity(row) self.completed[key] = row for view, model in self.models.items(): identity = self.extrinsics.cameras[view] @@ -110,13 +115,16 @@ class ImageEvidence: for sample_id, frame in self.raw_frames.items(): if frame.get("cycle") != holdout_cycle or frame.get("sample_phase") != phase: continue - unit = self.completed.get((frame["task_name"], holdout_cycle, frame["direction"])) + unit = self.completed.get(record_scan_identity(frame)) if unit is None or not unit["passed"] or unit["attempt"] != frame["attempt"]: continue if frame.get("policy") != IMAGE_OBSERVATION_POLICY or frame.get("command_unit") != self.profile.command.unit: raise ValueError("final_image_raw_policy_or_unit_changed") vectors = [frame.get("command_vector"), frame.get("feedback_vector")] skew = frame.get("state_image_sync_error_ns") + if self.profile.vision_motion and input_kind == "command": + vectors = [frame.get("command_vector")] + skew = frame.get("command_image_skew_ns") if any(values is None for values in vectors) or skew is None: continue if (not isinstance(skew, Real) or isinstance(skew, bool) @@ -136,6 +144,17 @@ class ImageEvidence: directions = _replay_directions(mappings, frame, task) values = frame["feedback_vector" if input_kind == "feedback" else "command_vector"] for role in sorted(roles.intersection(frame["tags"])): + if self.profile.retained_joints: + from ...core.urdf.partial_scope import profile_scope, out_of_scope_ancestors + outside = out_of_scope_ancestors(profile_scope(self.profile)["retained_joints"], + values, model=self.source_model, link=self.tags[role][1].link) + if outside: + if (role, sample_id) in expected: + raise ValueError("primary_image_has_uncalibrated_moving_ancestor") + self.out_of_scope_images.append({"sample_id": sample_id, "role": role, + "task_name": frame["task_name"], "retained_ancestors": list(outside), + "reason": "unmeasured_ancestor_outside_declared_hold"}) + continue image = self._image(frame, sample_id=sample_id, role=role, cycle=holdout_cycle, sdk_values=values, directions=directions) identity = (role, sample_id) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py index ba37f90..2ba35a4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/process_finalizer.py @@ -23,6 +23,8 @@ def read_journal_prefix(path, size): """ if not isinstance(size, int) or isinstance(size, bool) or size <= 0: raise ValueError("invalid_finalization_journal_size") + from ...core.geometry.frozen_evidence import EvidenceDecoder + decoder = EvidenceDecoder() rows = [] remaining = size with Path(path).open("rb") as stream: @@ -31,7 +33,7 @@ def read_journal_prefix(path, size): if not line or not line.endswith(b"\n"): raise ValueError("incomplete_finalization_journal_prefix") remaining -= len(line) - row = json.loads(line) + row = decoder.loads(line) if not isinstance(row, dict): raise ValueError("invalid_finalization_journal_rows") rows.append(row) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py index 4702409..ced0157 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py @@ -115,6 +115,10 @@ class FrozenTagArtifactValidator: image_observations: tuple = () command_image_observations: tuple = () require_image_holdout: bool = False + release_basis: str = "feedback_and_command" + calibration_scope: Mapping | None = None + out_of_scope_images: tuple = () + capture_plan: Mapping | None = None def __call__(self, json_path: Path, urdf_path: Path) -> Mapping[str, Any]: payload = json.loads(json_path.read_text(encoding="utf-8")) @@ -124,6 +128,8 @@ class FrozenTagArtifactValidator: if payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT}: feedback_payload = json.loads((json_path.parent / REPORT_FILENAME).read_text(encoding="utf-8")) _validate_identity(feedback_payload, self.profile_id) + if self.capture_plan is not None and feedback_payload.get("capture_plan") != self.capture_plan: + raise ValueError("capture plan differs from frozen session decisions") if (measured_from_report if measured else from_report)(feedback_payload) != payload: raise ValueError("compact JSON differs from the frozen calibration report") if measured: @@ -147,10 +153,12 @@ class FrozenTagArtifactValidator: from .urdf_from_json import validate_json_urdf_correspondence validate_json_urdf_correspondence(payload, self.source_urdf, urdf_path, expected_profile_id=self.profile_id) - metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path, - payload=feedback_payload, common_from_base=self.common_from_base, - installations=self.installations, observations=self.observations, - required_roles=self.required_roles, minimum_samples=self.minimum_samples) + metrics = {} + if self.release_basis == "feedback_and_command": + metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path, + payload=feedback_payload, common_from_base=self.common_from_base, + installations=self.installations, observations=self.observations, + required_roles=self.required_roles, minimum_samples=self.minimum_samples) command_metrics = {} if payload.get("format") in {"unified_calibration_v1", COMPACT_FORMAT, MEASURED_FORMAT}: try: @@ -170,10 +178,11 @@ class FrozenTagArtifactValidator: image_policy = FINAL_IMAGE_ACCEPTANCE_POLICY common = dict(corrected_urdf=urdf_path, common_from_base=self.common_from_base, installations=self.installations, required_roles=self.required_roles) - image_metrics["feedback"] = validate_serialized_image_holdout(**common, - payload=feedback_payload, observations=self.image_observations, - independent_observations=self.observations, - input_kind="feedback", holdout_cycle=3, minimum_samples=self.minimum_samples) + if self.release_basis == "feedback_and_command": + image_metrics["feedback"] = validate_serialized_image_holdout(**common, + payload=feedback_payload, observations=self.image_observations, + independent_observations=self.observations, + input_kind="feedback", holdout_cycle=3, minimum_samples=self.minimum_samples) image_metrics["command"] = validate_serialized_image_holdout(**common, payload=payload, observations=self.command_image_observations, independent_observations=self.command_observations, @@ -221,7 +230,16 @@ class FrozenTagArtifactValidator: evidence["cad_zero_assumptions"] = dict(self.cad_zero_assumptions) acceptance_kind = ("measured_and_transferred_json_urdf_v3" if self.transferred_motion_sources else "independently_measured_json_urdf_v3") if measured else "serialized_standard_urdf_tag_replay_v1" - return {"acceptance": acceptance_kind, + if self.calibration_scope is not None: + evidence["calibration_scope"] = self.calibration_scope + evidence["out_of_scope_images"] = self.out_of_scope_images + extra.update(calibration_scope=self.calibration_scope, + out_of_scope_images=self.out_of_scope_images) + acceptance_kind = "partial_measured_json_urdf_v3" + elif self.out_of_scope_images: + raise ValueError("full_calibration_cannot_exclude_ancestor_image_evidence") + return {"acceptance": acceptance_kind, "release_basis": self.release_basis, + "feedback_mapping_required": self.release_basis == "feedback_and_command", "final_file_image_holdout": image_metrics, "final_file_image_holdout_policy": image_policy, "final_file_image_holdout_verified": self.require_image_holdout, @@ -240,6 +258,10 @@ class FrozenTagArtifactValidator: def _validate_report_provenance(self, report): """The report cannot grant itself new transfer or CAD authorizations.""" + if report.get("calibration_scope") != self.calibration_scope: + raise ValueError("v3 partial scope differs from the protected Profile") + if report.get("quality", {}).get("release_basis", "feedback_and_command") != self.release_basis: + raise ValueError("v3 release basis differs from the protected Profile") if report.get("quality", {}).get("command_holdout_cycle", 3) != self.command_holdout_cycle: raise ValueError("v3 command validation partition differs from the Profile") ranges = json.loads(json.dumps({name: asdict(item) for name, item in self.range_evidence.items()})) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py index 91a18ee..a69fc6f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py @@ -36,11 +36,14 @@ class UnifiedCommandMapper: self.active = tuple(name for name, j in self.urdf.joints.items() if j.kind != "fixed" and j.mimic_joint is None) if self.measured: self.active = tuple(name for name, j in self.urdf.joints.items() if j.kind != "fixed") + self.active = tuple(name for name in self.active if name not in self.mapping.retained_joints) if compact: validate_compact_urdf_tables(payload, self.urdf) if not self.mapping.compact and set(self.mapping.joints) != set(self.active): raise ValueError("JSON active joints differ from the exported URDF") - self.urdf_joint_names = tuple(sorted(name for name, j in self.urdf.joints.items() if j.kind != "fixed")) + self.urdf_joint_names = tuple(sorted(name for name, j in self.urdf.joints.items() + if j.kind != "fixed" and name not in self.mapping.retained_joints)) + self.retained_joints = tuple(sorted(self.mapping.retained_joints)) self.command_names = tuple(metadata["command_names"]) if any(row["motor_index"] >= len(self.command_names) for row in self.mapping.joints.values()): raise ValueError("JSON SDK channel exceeds the recorded SDK layout") @@ -63,6 +66,9 @@ class UnifiedCommandMapper: values = tuple(float(v) for v in positions) if len(values) != len(self.command_names) or not all(math.isfinite(v) for v in values): raise ValueError("SDK input requires a complete finite channel vector") + if self.mapping.retained_joints: + from ...core.urdf.partial_scope import require_retained_pose + require_retained_pose(self.mapping.retained_joints, values) if names and not self.feedback_by_index and self.input_domain.startswith("feedback_"): names = tuple(self.feedback_name_aliases.get(n, n) for n in names) if len(names) != len(values) or set(names) != set(self.command_names): @@ -118,6 +124,11 @@ def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="com raise ValueError("v3 release directional mapping differs from its acceptance") transfers = report.get("transferred_motion_sources", {}) expected_kind = "measured_and_transferred_json_urdf_v3" if transfers else "independently_measured_json_urdf_v3" + scope = payload.get("calibration_scope") + if scope != report.get("calibration_scope") or scope != validation.get("calibration_scope"): + raise ValueError("v3 release partial scope differs from its report or acceptance") + if scope is not None: + expected_kind = "partial_measured_json_urdf_v3" observed = {name for name, row in report["joints"].items() if row.get("independently_measured") is True} if (validation.get("acceptance") != expected_kind or not observed or set(validation.get("independent_mapping_holdout", {})) != observed diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py index cf9b3ea..6e7cd75 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py @@ -1,80 +1,22 @@ """Offline use of exactly the online finalizer, for any declared Profile.""" from pathlib import Path -import hashlib -import json from ..acquisition import load_capture -from ..diagnostic_capture import reject_diagnostic_capture -from ..engine import CalibrationEngine -from ..resume import ResumeVerifier, fingerprint_from_mapping -from ..scan_quality import evaluate_capture_unit +from .capture_validation import validate_capture_provenance, validate_capture_units from .finalization import finalize_profile_session -def validate_capture_provenance(profile, serial_number, hashes, records): - """An offline journal cannot substitute today's hashes for old evidence.""" - reject_diagnostic_capture(records) - 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: - raise ValueError("capture_provenance:requires_unique_session_and_locked_reference") - header, reference = headers[0], references[0] - engine = CalibrationEngine(profile) - if (not engine.capture_compatible(header) or header.get("serial_number") != serial_number - or header.get("curve_input_domain") != profile.curve_input_domain): - raise ValueError("capture_provenance:obsolete_policy_or_coordinate_identity; fresh capture required") - for key, value in hashes.items(): - if header.get(key) != value or reference.get("protected_hashes", {}).get(key) != value: - raise ValueError(f"capture_provenance:protected_input_changed:{key}") - fingerprint = fingerprint_from_mapping(reference) - checker = ResumeVerifier(required_fixed_views=profile.vision.view_names, - required_fixed_poses=profile.vision.view_names, required_hashes=(*hashes, "intrinsics_sha256")) - evidence = checker.compare(fingerprint, fingerprint) - if not evidence.reuse or fingerprint.profile_id != profile.key.profile_id: - raise ValueError(f"capture_provenance:invalid_locked_reference:{evidence.incompatible_fields}") - models = {} - # Only preview models preceding the formal reference are relevant. Later - # camera changes are rejected online and cannot authorize a mixed replay. - for row in records: - if row is reference: - break - if row.get("kind") == "rectified_camera_model": - if row.get("matrix_source") != "CameraInfo.P[:3,:3]" or row.get("input_is_rectified") is not True: - raise ValueError("capture_provenance:unverified_rectified_projection") - models[row["view"]] = row["camera_matrix"] - intrinsic_hash = hashlib.sha256(json.dumps(dict(sorted(models.items())), sort_keys=True).encode()).hexdigest() - if set(models) != set(profile.vision.view_names) or intrinsic_hash != fingerprint.protected_hashes["intrinsics_sha256"]: - raise ValueError("capture_provenance:intrinsics_evidence_changed_or_missing") - - -def validate_capture_units(profile, records): - """Offline and resumed data must satisfy the actual post-sweep policy.""" - from ...core.fitting.command_sampling import validate_sampling_plans - validate_sampling_plans(profile, records) - first_spans = {} - for unit in CalibrationEngine(profile).scan_units(): - key = (unit.task_key, unit.cycle, unit.direction) - rows = [row for row in records - if (row.get("task_name"), row.get("cycle"), row.get("direction")) == key] - attempt = max((int(row.get("attempt", 1)) for row in rows), default=1) - complete = any(row.get("kind") == "scan_unit_complete" and row.get("passed") is True - and int(row.get("attempt", 1)) == attempt for row in rows) - quality = evaluate_capture_unit(profile, unit, attempt, rows, first_cycle_spans=first_spans) - if not complete or not quality.passed: - raise ValueError(f"capture_incomplete:{key}:{quality.failures}") - - def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool): + from ...product import sha256_file from ..runner_support import create_session_directory, protected_inputs records = load_capture(raw_path) profile = config.calibration_contract.typed_profile hashes = protected_inputs(config) - validate_capture_provenance(profile, config.serial_number, hashes, records) - validate_capture_units(profile, records) - reference = next(row for row in records if row.get("kind") == "fixed_base_reference_locked") - hashes["intrinsics_sha256"] = reference["protected_hashes"]["intrinsics_sha256"] - hashes["raw_samples_sha256"] = hashlib.sha256(Path(raw_path).read_bytes()).hexdigest() + reference = next((row for row in records if row.get("kind") == "fixed_base_reference_locked"), {}) + if "intrinsics_sha256" in reference.get("protected_hashes", {}): + hashes["intrinsics_sha256"] = reference["protected_hashes"]["intrinsics_sha256"] + hashes["raw_samples_sha256"] = sha256_file(raw_path) if output is None: output = create_session_directory(config.session_root) else: @@ -84,5 +26,5 @@ def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool profile=profile, session_dir=output, serial_number=config.serial_number, source_urdf=config.source_urdf, protected_inputs=hashes, records=records, publish=publish, - camera_extrinsics_file=config.camera_extrinsics) + camera_extrinsics_file=config.camera_extrinsics, require_motion_evidence=True) print(f"离线回放完成：schema {payload['schema_version']}，URDF {correction.path}") 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 5c543f8..a87ce11 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 @@ -5,6 +5,7 @@ import numpy as np from .unified_v1 import _mapping from ....core.urdf.joint_control import PRESERVE_SOURCE_MIMIC, uses_source_mimic +from ....core.urdf.partial_scope import profile_scope, retained_from_payload FORMAT = "unified_calibration_v3" REPORT_FORMAT = "unified_calibration_report_v3" @@ -33,8 +34,11 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) row = {"urdf_joint": name, "independently_measured": donor is None, "zero_offset_rad": fit.zero_offsets_rad[name], "zero_method": fit.zero_method_by_joint[name], "command_to_rad": _mapping(fit.command_mappings[name]), - "feedback_to_rad": _mapping(fit.output_mappings[name]), "passive": name in passive_sources} + if profile.command_based_release: + row["feedback_diagnostic"] = fit.feedback_diagnostics[name] + else: + row["feedback_to_rad"] = _mapping(fit.output_mappings[name]) if donor is None: row.update(zero_reference=fit.joint_zero_references[name].as_record(), applicability=fit.command_applicability[name], @@ -73,6 +77,7 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) row["zero_transferred_from_joint"] = profile.zero.transferred_zero_sources[name] rows[name] = row return {"format": REPORT_FORMAT, "schema_version": 3, + **({"calibration_scope": profile_scope(profile)} if profile.retained_joints else {}), **({"directional_command_joints": sorted(profile.artifacts.directional_command_joints)} if profile.artifacts.directional_command_joints else {}), "profile_id": profile.key.profile_id, "model": profile.key.model, "side": profile.key.side, @@ -82,9 +87,20 @@ def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs) "feedback_name_aliases": dict(profile.command.feedback_name_aliases), "coordinate_convention": ZERO_CONVENTION, "protected_inputs": dict(protected_inputs), "joints": rows, **({"transferred_motion_sources": transfers} if transfers else {}), - "quality": {"training_cycles": [0, 1, 2], "holdout_cycle": 3, + "quality": {"release_basis": profile.measurement.release_basis, + "motion_observation": profile.acquisition.motion_observation, + "fixed_reference_mode": profile.acquisition.fixed_reference_mode, + "position_feedback_required": not profile.vision_motion, + "feedback_mapping_required": not profile.command_based_release, + "training_cycles": sorted({c for task in profile.motion.tasks + for c in profile.quality.task_training_cycles.get(task.key, profile.quality.training_cycles)}), + "holdout_cycle": profile.quality.holdout_cycle, + "task_training_cycles": {task.key: list(profile.quality.task_training_cycles.get(task.key, + profile.quality.training_cycles)) for task in profile.motion.tasks}, + "training_policy": profile.quality.training_policy, "command_capture_mode": profile.acquisition.command_capture_mode, - "command_training_cycles": list(command_training_cycles(profile)), + "command_training_cycles": sorted({c for task in profile.motion.tasks + for c in command_training_cycles(profile, task)}), "command_holdout_cycle": command_holdout_cycle(profile), "precision_scope": "observed_travel_directions_and_held_poses", "independently_measured_joints": sorted(observed), "transferred_joints": sorted(transfers), @@ -96,7 +112,19 @@ def from_report(report): if report.get("format") != REPORT_FORMAT or report.get("coordinate_convention") != ZERO_CONVENTION: raise ValueError("v3 requires its original measured report; legacy promotion is forbidden") mapping = SerializedJointMapping(report, input_kind="command") - feedback = SerializedJointMapping(report, input_kind="feedback") + retained_from_payload(report) + basis = report.get("quality", {}).get("release_basis", "feedback_and_command") + if basis not in {"feedback_and_command", "steady_command"}: + raise ValueError("invalid_report_release_basis") + feedback = SerializedJointMapping(report, input_kind="feedback") if basis == "feedback_and_command" else None + if basis == "steady_command": + if report.get("transferred_motion_sources") or report['quality'].get('feedback_mapping_required') is not False: + raise ValueError('invalid_command_release_contract') + for name, row in report['joints'].items(): + diagnostic = row.get('feedback_diagnostic', {}) + if ('feedback_to_rad' in row or diagnostic.get('required_for_release') is not False + or diagnostic.get('input_domain') != f"feedback_{report['command_unit']}"): + raise ValueError(f'invalid_feedback_diagnostic:{name}') unit, baseline, rows = report["command_unit"], report["baseline_command"], {} directional = set(report.get("directional_command_joints", ())) if not directional <= mapping.joints.keys(): @@ -127,7 +155,7 @@ def from_report(report): or evidence.get("zero_offset_rad") != zero_donor.get("zero_offset_rad")): raise ValueError(f"transferred_zero_provenance_differs:{name}") index = data["motor_index"] - if index >= len(baseline) or feedback.joints[name]["motor_index"] != index: + if index >= len(baseline) or (feedback is not None and feedback.joints[name]["motor_index"] != index): raise ValueError(f"joint_input_channel_differs:{name}") donor = transfers.get(name) if donor is None: @@ -184,6 +212,8 @@ def from_report(report): if unit == "u8" else values.tolist()) rows[name] = row return {"format": FORMAT, "schema_version": 3, + **({"capture_plan": report["capture_plan"]} if "capture_plan" in report else {}), + **({"calibration_scope": report["calibration_scope"]} if "calibration_scope" in report else {}), **({"urdf_correction": report["urdf_correction"]} if "urdf_correction" in report else {}), **({"directional_command_joints": sorted(directional)} if directional else {}), **{key: report[key] for key in ("profile_id", "model", "side", "serial_number")}, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py index 73add70..b0a4c85 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py @@ -42,6 +42,6 @@ def serialize(profile, fit, prepared, *, serial_number, protected_inputs): "feedback_name_aliases": dict(profile.command.feedback_name_aliases), "protected_inputs": dict(protected_inputs), "joints": rows, "coordinate_convention": "q_CAD=q_output+delta; JSON already emits q_output", - "quality": {"training_cycles": [0, 1, 2], "holdout_cycle": 3, + "quality": {"training_cycles": list(profile.quality.training_cycles), "holdout_cycle": profile.quality.holdout_cycle, "command_and_feedback_fits_passed": True, "final_file_acceptance": "release_manifest.json", "arbitrary_multiaxis_validated": False}} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py index 2d083fe..e7e9b13 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_initialization.py @@ -7,6 +7,7 @@ training/holdout observations to revise an existing reference. """ from dataclasses import asdict, dataclass +from collections import deque import hashlib import json import math @@ -18,6 +19,10 @@ from ..core.geometry.tag_pose.motion_evidence import ( from ..core.geometry.tag_pose.types import SquareTagPose from ..core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation from ..core.geometry.tag_pose.cad_hinge import ParallelAxisGeometry +from ..core.geometry.tag_pose.source_hinge import SourceHinge +from ..core.geometry.tag_pose.pose_bridge import ( + SharedTagPoseBridge, shared_pose_selection_policy, +) from ..core.geometry.tag_pose.production_image_motion import ( ImageMotionModel, ImageMotionResolution, resolve_image_motion, ) @@ -53,6 +58,12 @@ class BranchInitializationRequest: image_geometry: bool = False geometry_constraints: tuple[ParallelAxisGeometry, ...] = () source_urdf_sha256: str = "" + source_motion_versions: tuple[tuple[str, int], ...] = () + source_hinges: tuple[SourceHinge, ...] = () + parent_reference_sha256: str = "" + pose_bridges: tuple[SharedTagPoseBridge, ...] = () + pose_bridge_records: tuple[dict, ...] = () + rejected_observations: tuple[tuple[str, int], ...] = () def _pose(payload): @@ -60,6 +71,13 @@ def _pose(payload): tuple(payload["translation_xyz_m"]), float(payload["reprojection_error_px"])) +def _image_references(record, roles): + from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence + return tuple(reference_from_evidence(item, stamp_ns=record["image_stamp_ns"], + camera_matrix=record["camera_matrix"]) for item in record["tags"].values() + if item.get("pose_source") == IMAGE_REFERENCE_SOURCE and item.get("tag_role") in roles) + + def _finite_report(value): if isinstance(value, float) and not math.isfinite(value): return None @@ -76,7 +94,8 @@ def solve_initialization(request: BranchInitializationRequest): return MotionBranchResolution(False, request.invalid_reason) if request.image_geometry: return resolve_image_motion(request.image_frames, request.relations, - geometry_constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256) + geometry_constraints=request.geometry_constraints, source_urdf_sha256=request.source_urdf_sha256, + source_hinges=request.source_hinges, pose_bridges=request.pose_bridges) return resolve_motion_branches(request.frames, request.relations) @@ -96,14 +115,27 @@ def initialization_artifacts(request, resolution): "constraints": [asdict(model) for model in constraints], "hypotheses": _finite_report([asdict(item) for item in resolution.hypotheses]), "evidence_ids": {}, "evidence_payloads": {}, + "preparation_observation_rejections": dict(request.rejected_observations), } + if request.source_motion_versions: + record.update(schema_version=2, approach_evidence_scope="path", + source_motion_versions=dict(request.source_motion_versions)) if image_model is not None: - payload = asdict(image_model) + from ..core.geometry.tag_pose.image_motion_model import image_model_payload + payload = image_model_payload(image_model) record["frozen_image_model"] = payload record["image_model_sha256"] = hashlib.sha256(json.dumps( payload, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest() + winner = next(item for item in resolution.hypotheses if item.branches == image_model.branches) + record["geometry_uncertainty"] = [list(item) for item in winner.child_frame_uncertainty] + if request.parent_reference_sha256: + record["parent_reference_sha256"] = request.parent_reference_sha256 + if request.pose_bridge_records: + record["shared_tag_pose_bridges"] = list(request.pose_bridge_records) if request.image_geometry: record["image_model_selection_policy"] = IMAGE_MODEL_SELECTION_POLICY + if request.pose_bridges: + record["image_model_selection_policy"] = shared_pose_selection_policy(request.pose_bridges) record["image_geometry_constraints"] = [asdict(item) for item in request.geometry_constraints] record["source_urdf_sha256"] = request.source_urdf_sha256 if not resolution.resolved: @@ -117,9 +149,16 @@ def initialization_artifacts(request, resolution): "task_name", "view", "zero_joints", "session_epoch", "motion_version", "source_image_stamps", "source_frame_hashes", "constraints", )} + if request.source_motion_versions: + payload.update(approach_evidence_scope="path", + source_motion_versions=record["source_motion_versions"]) if image_model is not None: payload.update(frozen_image_model=record["frozen_image_model"], - image_model_sha256=record["image_model_sha256"]) + image_model_sha256=record["image_model_sha256"], geometry_uncertainty=record["geometry_uncertainty"]) + if request.parent_reference_sha256: + payload["parent_reference_sha256"] = request.parent_reference_sha256 + if request.pose_bridge_records: + payload["shared_tag_pose_bridges"] = record["shared_tag_pose_bridges"] payload["tag_role"] = role encoded = json.dumps(payload, sort_keys=True, separators=(",", ":"), allow_nan=False) record["evidence_ids"][role] = hashlib.sha256(encoded.encode()).hexdigest() @@ -130,18 +169,28 @@ def initialization_artifacts(request, resolution): class BranchInitialization: - """At most 129 command bins per view from the final approach segment.""" + """Bounded pre-zero evidence from an arrival or an explicitly declared path. + + Path capture begins only after reaching its first declared pose. Holding + channels, task, joint group and session epoch remain fixed across its legs. + Each selected frame keeps its original motion version and content hash. + """ def __init__(self, profile, *, image_geometry=True, source_urdf=None): self.profile = profile self.image_geometry = image_geometry self.source_model = None self.source_urdf_sha256 = "" + self.tag_feedback_channels = None if source_urdf is not None: from ..core.urdf.kinematics import UrdfKinematicModel + from ..profiles.observations import compile_tag_feedback_channels self.source_model = UrdfKinematicModel(source_urdf) self.source_urdf_sha256 = hashlib.sha256(self.source_model.source.read_bytes()).hexdigest() + self.tag_feedback_channels = compile_tag_feedback_channels(profile, self.source_model) self.generation = 0 + from .parent_reference import ParentReferenceRegistry + self.parent_references = ParentReferenceRegistry(profile, self.tag_feedback_channels) self.clear() def clear(self): @@ -152,15 +201,33 @@ class BranchInitialization: self.session_epoch = None self._frames = {} self._source_records = {} + self._endpoint_records = {} + self._rejected_observations = {} self._existing_ids = {} self._held_feedback = None self._invalid_reason = "" self._requested = set() + self.reference_spec = None self.reports = {} + self.path_index = None def begin(self, motion, *, session_epoch, motion_version): - if motion.phase == "zero_approach": + if motion.reference_reuse: self.clear() + return + if motion.phase == "zero_approach": + index = motion.reference_path_index + identity = (motion.task_key, motion.reference_joints, session_epoch) + previous = (self.task_name, self.zero_joints, self.session_epoch) + continuing = (index is not None and index > 0 and identity == previous + and self.path_index is not None and index in {self.path_index, self.path_index + 1} + and self.motion_version is not None and motion_version > self.motion_version) + if not continuing: + self.clear() + if index is not None and index != 0: + self._invalid_reason = "motion_approach_path_discontinuous" + self.path_index = index + self.reference_spec = (motion.reference_command, motion.reference_approach) if motion.reference_only else None self.task_name = motion.task_key self.zero_joints = motion.reference_joints self.motion_version = motion_version @@ -172,44 +239,71 @@ class BranchInitialization: self.task_name, self.zero_joints = motion.task_key, motion.zero_joints self.session_epoch = session_epoch self._invalid_reason = "motion_approach_evidence_missing" + elif self.path_index is not None: + spec = self.profile.motion.joint_zero_references[motion.zero_joints[0]] + if spec.evidence_scope != "path" or self.path_index != len(spec.approach_commands): + self._invalid_reason = "motion_approach_path_incomplete" def _relations(self, view): - task = next(task for task in self.profile.motion.tasks if task.key == self.task_name) - return tuple(MotionRelation(name, spec.parent_role, spec.child_role) - for _, name, spec in observation_streams(self.profile, task) - if name in self.zero_joints and spec.view == view) + relations = [] + for name in self.zero_joints: + measurements = [self.profile.measurement.measurements[name]] + secondary = self.profile.measurement.cross_view_sources.get(name) + if secondary is not None: + measurements.append(self.profile.measurement.measurements[secondary]) + relations.extend(MotionRelation(name, spec.parent_role, spec.child_role) + for spec in measurements if spec.view == view) + return tuple(relations) def observe(self, record): if (record["task_name"], tuple(record["zero_joints"]), record["motion_version"], record["session_epoch"]) != ( self.task_name, self.zero_joints, self.motion_version, self.session_epoch): return - command, feedback = record["command_vector"], record["feedback_vector"] - if command is None or feedback is None or not self.zero_joints: + if self.path_index is not None and (self.path_index == 0 + or record.get("reference_path_index") != self.path_index): return - spec = self.profile.motion.joint_zero_references[self.zero_joints[0]] - task = next(task for task in self.profile.motion.tasks if task.key == self.task_name) - index = task.command_index + command, feedback = record["command_vector"], record["feedback_vector"] + if command is None or (feedback is None and not self.profile.vision_motion) or not self.zero_joints: + return + from ..core.domain.profile import JointZeroSpec + spec = (JointZeroSpec(self.task_name, *self.reference_spec) if self.reference_spec else + self.profile.motion.joint_zero_references[self.zero_joints[0]]) + if (spec.evidence_scope == "path") != (self.path_index is not None): + self._invalid_reason = "motion_approach_scope_mismatch" + return + from ..core.fitting.motion_fit import channel_for_joint + index = channel_for_joint(self.profile, self.zero_joints[0]) + moving_channels = {i for i, v in enumerate(spec.approach_commands[-1]) if v != spec.command[i]} # The current measured channel moves; every other declared command # must already be in this reference's actual clearance/holding pose. if any(abs(value - spec.command[i]) > 1e-9 - for i, value in enumerate(command) if i != index): + for i, value in enumerate(command) if i not in moving_channels): return - if self._held_feedback is None: - self._held_feedback = tuple(feedback) - layout = self.profile.command - for i, value in enumerate(feedback): - tolerance = max(1.0 if layout.unit == "u8" else 0.002, - 0.005 * (layout.maximum_feedback_values[i] - layout.minimum_feedback_values[i])) - if i != index and abs(value - self._held_feedback[i]) > tolerance: - self._invalid_reason = "motion_held_feedback_changed" - return relations = self._relations(record["view"]) if not relations: return + # Full commands above retain the declared avoidance posture. Only + # ancestors of the observed Tags can invalidate their hinge geometry. + # Historical callers without source topology retain the full check. + relevant_channels = (set(range(self.profile.command.command_count)) + if self.tag_feedback_channels is None else set().union(*( + self.tag_feedback_channels[role] for relation in relations + for role in (relation.parent_role, relation.child_role)))) + held_channels = relevant_channels - moving_channels + if feedback is not None and self._held_feedback is None: + self._held_feedback = tuple(feedback) + layout = self.profile.command + for i, value in enumerate(() if self.profile.vision_motion else feedback): + tolerance = max(1.0 if layout.unit == "u8" else 0.002, + 0.005 * (layout.maximum_feedback_values[i] - layout.minimum_feedback_values[i])) + if i in held_channels and abs(value - self._held_feedback[i]) > tolerance: + self._invalid_reason = "motion_held_feedback_changed" + return roles = tuple(dict.fromkeys(role for relation in relations for role in (relation.parent_role, relation.child_role))) items, existing = [], self._existing_ids.setdefault(record["view"], {}) + frame_evidence_ids = {} for role in roles: item = record["tags"].get(role) if item is None: @@ -227,18 +321,37 @@ class BranchInitialization: if existing.get(role, evidence_id) != evidence_id: self._invalid_reason = "motion_frozen_evidence_changed" return - existing[role] = evidence_id + frame_evidence_ids[role] = evidence_id else: candidates = tuple(_pose(p) for p in item["reprojection_valid_candidates"]) items.append(RoleCandidates(role, candidates, frozen)) + frame = MotionEvidenceFrame(record["image_stamp_ns"], tuple(items)) + from ..core.geometry.tag_pose.motion_evidence import motion_frame_rejection + rejection = motion_frame_rejection(frame, relations, require_frozen_roots=self.image_geometry) + if rejection: + counts = self._rejected_observations.setdefault(record["view"], {}) + counts[rejection] = counts.get(rejection, 0)+1 + return + existing.update(frame_evidence_ids) lower, upper = layout.minimum_values[index], layout.maximum_values[index] bin_index = round(128 * (command[index] - lower) / (upper - lower)) - self._frames.setdefault(record["view"], {})[bin_index] = MotionEvidenceFrame( - record["image_stamp_ns"], tuple(items)) + # A dropout must not erase an earlier eligible image of this same + # motion interval. Every retained image still needs full model and + # held-out validation; admission itself does not authorize a branch. + self._frames.setdefault(record["view"], {})[bin_index] = frame self._source_records.setdefault(record["view"], {})[bin_index] = record + # Keep a separate bounded stationary tail. Repeated endpoint images + # must not inflate the arc's motion bins or enter its holdout split. + tail = self._endpoint_records.setdefault(record['view'], deque( + maxlen=max(10, self.profile.acquisition.fixed_reference_minimum_frames)+1)) + if tuple(command) == tuple(spec.command): + if not tail or record['image_stamp_ns'] > tail[-1]['image_stamp_ns']: + tail.append(record) + else: + tail.clear() - def request(self, view, motion, *, session_epoch, motion_version): - if motion is None or motion.phase != "joint_zero" or view in self._requested: + def request(self, view, motion, *, session_epoch, motion_version, zero_references=None): + if motion is None or motion.reference_reuse or motion.phase != "joint_zero" or view in self._requested: return None if (self.task_name, self.zero_joints, self.session_epoch) != ( motion.task_key, motion.zero_joints, session_epoch): @@ -262,20 +375,42 @@ class BranchInitialization: float(by_stamp[frame.stamp_ns]["tags"][item.role]["tag_size_m"]), tuple(tuple(float(v) for v in point) for point in by_stamp[frame.stamp_ns]["tags"][item.role]["corners_xy"])) - for item in frame.roles)) for frame in frames) + for item in frame.roles), + _image_references(by_stamp[frame.stamp_ns], {item.role for item in frame.roles})) + for frame in frames) except (KeyError, TypeError, ValueError): reason = reason or "image_motion_preparation_pixels_missing" geometry_constraints = () + pose_bridges, pose_bridge_records = (), () + if self.image_geometry and zero_references and not motion.reference_only: + from .shared_tag_reference import prepare_pose_bridges + try: + pose_bridges, pose_bridge_records = prepare_pose_bridges(self, view, zero_references, + self._endpoint_records.get(view, ()), {f.stamp_ns for f in frames}, session_epoch) + except (ValueError, KeyError, TypeError, IndexError) as error: + reason = reason or 'shared_pose_bridge_invalid:' + str(error) + source_hinges, parent_reference_sha256 = (), "" + task = next(task for task in self.profile.motion.tasks if task.key == self.task_name) + if task.parent_reference is not None: + try: + source_hinges, parent_reference_sha256 = self.parent_references.geometry(task, session_epoch) + except ValueError as error: + reason = reason or str(error) if self.image_geometry and self.source_model is not None: from ..profiles.observations import compile_parallel_axis_geometry - geometry_constraints = compile_parallel_axis_geometry(self.profile, self.source_model, relations) + geometry_constraints = compile_parallel_axis_geometry(self.profile, self.source_model, + tuple(item.geometry.relation for item in source_hinges) + relations) return BranchInitializationRequest(self.task_name, view, self.zero_joints, session_epoch, self.motion_version if self.motion_version is not None else motion_version, self.generation, frames, relations, tuple(sorted(self._existing_ids.get(view, {}).items())), moving, tuple(sorted((str(row["image_stamp_ns"]), source_frame_sha256(row)) for row in sources)), reason, image_frames, self.image_geometry, - geometry_constraints, self.source_urdf_sha256 if geometry_constraints else "") + geometry_constraints, self.source_urdf_sha256 if geometry_constraints else "", + tuple(sorted((str(row["image_stamp_ns"]), row["motion_version"]) + for row in sources)) if self.path_index is not None else (), source_hinges, parent_reference_sha256, + pose_bridges, pose_bridge_records, + tuple(sorted(self._rejected_observations.get(view, {}).items()))) def pending(self): """A bounded preparation solve is running outside the state lock.""" @@ -285,6 +420,8 @@ class BranchInitialization: if request.generation != self.generation: return None model, report = initialization_artifacts(request, resolution) + if model is not None: + self.parent_references.remember(model, report) self.reports[request.view] = report return model, report diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py index 4f252c9..fbc7efc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/branch_tracking.py @@ -22,10 +22,12 @@ class CommittedPoseEvidence: return item.candidates, dict(item.diagnostics), item.corrections -def select_motion_poses(frame, model, tracker, selected, *, base, cached, projection_worker=None): +def select_motion_poses(frame, model, tracker, selected, *, base, cached, + projection_worker=None, reference_tracker=None): """Called outside the session lock; tracker commits check image versions.""" if model.image_model is not None: - return _select_image_poses(frame, model, tracker, selected, projection_worker=projection_worker) + return _select_image_poses(frame, model, tracker, selected, + projection_worker=projection_worker, reference_tracker=reference_tracker) ids = dict(model.evidence_ids) roles = tuple(dict.fromkeys(role for constraint in model.constraints for role in (constraint.relation.parent_role, constraint.relation.child_role))) @@ -59,7 +61,7 @@ def select_motion_poses(frame, model, tracker, selected, *, base, cached, projec return {item.snapshot.role: item.pose for item in result.selections}, {}, evidence -def _select_image_poses(frame, model, tracker, selected, *, projection_worker=None): +def _select_image_poses(frame, model, tracker, selected, *, projection_worker=None, reference_tracker=None): """The model owns its children; already frozen parents keep their identity.""" image_model = model.image_model children = {item.relation.child_role for item in image_model.geometry} @@ -78,9 +80,20 @@ def _select_image_poses(frame, model, tracker, selected, *, projection_worker=No elif role in selected: candidates.append(RoleCandidates(role, (selected[role],), frozen=True)) image_frame = ImageMotionFrame(MotionEvidenceFrame(frame.stamp_ns, tuple(candidates)), - tuple(tuple(float(v) for v in row) for row in frame.camera_matrix), tuple(observations)) + tuple(tuple(float(v) for v in row) for row in frame.camera_matrix), tuple(observations), + image_model.reference_frames) + roots = () + if image_model.reference_frames: + reference_roles = {ref.role for ref in image_model.reference_frames} + root_snapshots = [] + for role in sorted(set(dict(image_model.tag_sizes)) - children): + provider = reference_tracker if role in reference_roles else tracker + snapshot = None if provider is None else provider.motion_snapshot(role, frame.stamp_ns) + if snapshot is not None: + root_snapshots.append(snapshot) + roots = tuple(root_snapshots) decision = SquareTagPoseTracker.verify_image_motion_frame(tuple(snapshots.values()), - image_model, image_frame, ids, projection_worker=projection_worker) + image_model, image_frame, ids, projection_worker=projection_worker, root_snapshots=roots) if not decision.accepted: reason = "pose_branch_" + decision.reason rejected = SquareTagPoseTracker.reject_motion_frame(tuple(snapshots.values()), reason) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py index cb0d6a3..13de8e9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py @@ -1,12 +1,15 @@ """Profile observation capture shared by every SDK and ROS camera host.""" from collections import deque -from dataclasses import asdict, dataclass +from contextlib import ExitStack +from dataclasses import asdict, dataclass, replace from threading import RLock import numpy as np from scipy.spatial.transform import Rotation +from ..core.domain.motion_path import segment_metadata +from ..core.fitting.motion_fit import channel_for_joint from ..core.geometry.extrinsics import matrix_payload, transform_matrix from ..core.geometry.pnp import SquareTagPose, SquareTagPoseTracker from ..core.geometry.tag_pose.fixed_reference import FixedTagReference @@ -37,6 +40,7 @@ class CaptureFrame: command_directions: tuple[str, ...] = () observed_roles: frozenset[str] = frozenset() timing: CaptureTiming | None = None + command_skew_ns: int | None = None class _FramePoseEvidence: @@ -74,6 +78,13 @@ class ObservationCapture: self.last_filtered = {} self._branch_events_active = set() self._motion_models = {} + self._preparation_identity = None + from .image_reference import ImageReferenceTracker + self._image_references = ({view.name: ImageReferenceTracker( + profile.acquisition.fixed_reference_minimum_frames) for view in profile.vision.views} + if profile.acquisition.fixed_reference_mode == "stationary_image" else {}) + self._model_image_references = {} + self._active_image_references = dict(self._image_references) def reset(self): with self._metadata_lock: @@ -89,12 +100,68 @@ class ObservationCapture: self.last_filtered.clear() self._branch_events_active.clear() self._motion_models.clear() + for tracker in self._model_image_references.values(): + tracker.reset() + self._model_image_references.clear() + self._active_image_references = dict(self._image_references) + self._preparation_identity = None for tracker in self.trackers.values(): tracker.reset() + for tracker in self._image_references.values(): + tracker.reset() + + def begin_motion(self, motion): + """Reauthorize reused Tags when their driven hinge or holding pose changes. + + Only the declared task scope permits replacement. Recorded evidence and + frozen joint zeros remain immutable; dependent image models are dropped + together, so an old child can never use a newly interpreted parent. + """ + if self.profile.acquisition.motion_model_scope != "task" or motion.phase != "zero_approach": + return + identity = (motion.task_key, motion.reference_joints) + with self._metadata_lock: + if identity == self._preparation_identity: + return + self._preparation_identity = identity + requested = {self.profile.measurement.measurements[name].child_role + for name in motion.reference_joints} + self._release_model_roles(requested) + + def _release_model_roles(self, requested): + """Caller owns metadata lock; invalidate all downstream interpretations.""" + removed, roles = set(), set(requested) + while True: + affected = {key for key, model in self._motion_models.items() + if self._model_children(model) & roles or any( + g.relation.parent_role in roles for g in ( + model.constraints if model.image_model is None else model.image_model.geometry))} + if affected <= removed: + break + removed.update(affected) + roles.update(role for key in affected for role in self._model_children(self._motion_models[key])) + for key in removed: + del self._motion_models[key] + self._metadata_generation += 1 + for view in self.profile.vision.views: + reset = roles & {tag.role for tag in view.tags if not tag.fixed_reference} + if reset: + self.trackers[view.name].reset_roles(reset) + + def activate_reference_model(self, model): + """Reinstall immutable evidence for a declared stationary pose check.""" + if self.profile.acquisition.motion_model_scope != "task": + raise ValueError("parent_reference_requires_task_model_scope") + with self._metadata_lock: + self._release_model_roles(self._model_children(model)) + self._preparation_identity = None + self.install_motion_model(model) def install_motion_model(self, model: MotionBranchModel): """Add evidence before zero capture without replacing existing roles.""" key = (model.task_name, model.view) + if self.profile.acquisition.motion_model_scope == "task": + key += (tuple(sorted(self._model_children(model))),) with self._metadata_lock: owned = self._model_children(model) for installed in self._motion_models.values(): @@ -125,20 +192,63 @@ class ObservationCapture: if previous != model: self._metadata_generation += 1 self._motion_models[key] = model + if model.image_model is not None and model.view in self._image_references: + from .image_reference import ImageReferenceTracker + for reference in model.image_model.reference_frames: + reference_key = (model.view, reference.identity) + if reference_key not in self._model_image_references: + self._model_image_references[reference_key] = ImageReferenceTracker( + self.profile.acquisition.fixed_reference_minimum_frames, + reference=reference) + + def _scheduled_image_reference(self, view, models): + """Choose one coordinate convention for this frame's complete ancestry. + + Inactive models may belong to different sessions. Active models must + agree on the fixed root; mixing coordinates in a persisted frame would + invalidate its geometry and replay evidence. The caller owns metadata. + """ + references = {ref.identity: ref for model in models if model.image_model is not None + for ref in model.image_model.reference_frames} + if len(references) > 1: + return None + current = self._image_references.get(view) + if not references or current is None: + return current + identity, reference = next(iter(references.items())) + if current.reference == reference: + return current + return self._model_image_references[(view, identity)] @staticmethod def _model_children(model): geometry = model.constraints if model.image_model is None else model.image_model.geometry return {item.relation.child_role for item in geometry} + def _observed_joint_names(self, motion): + if motion is None: + return () + task = next((task for task in self.profile.motion.tasks if task.key == motion.task_key), None) + return (motion.zero_joints or motion.reference_joints or motion.observed_joints + or (() if task is None else task.joints)) + + def _observed_roles(self, motion): + names = set(self._observed_joint_names(motion)) + names.update(self.profile.measurement.cross_view_sources[name] for name in tuple(names) + if name in self.profile.measurement.cross_view_sources) + return {role for name in names for role in ( + self.profile.measurement.measurements[name].parent_role, + self.profile.measurement.measurements[name].child_role)} + def _motion_model_schedule(self, view, motion): """Keep installed identities while any task, return or idle pose changes. A model-authorized Tag can never return to independent IPPE selection. - Tracking all installed models also keeps continuity through clearance - and baseline returns; sample collection remains gated by the task below. + Session scope tracks the installed chain through clearance and return. + Task scope selects the current observations and their parent models; + inactive frozen roles are not passed to independent pose selection. """ - del motion # Collection scope does not determine physical Tag identity. + available = {key: model for key, model in self._motion_models.items() if model.view == view} if not available: return () @@ -161,7 +271,13 @@ class ObservationCapture: visited.add(key) ordered.append(model) - for key in sorted(available): + selected_keys = set(available) + if self.profile.acquisition.motion_model_scope == "task": + selected_keys = set() + if motion is not None and motion.task_key: + roles = self._observed_roles(motion) + selected_keys = {owners[role] for role in roles if role in owners} + for key in sorted(selected_keys): include(key) return tuple(ordered) @@ -204,6 +320,7 @@ class ObservationCapture: observations[role] = item return {"kind": "pnp_candidate_frame", "task_name": task.key, "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + **segment_metadata(motion.segment_key), "view": frame.view, "image_stamp_ns": frame.stamp_ns, "tag_size_m": self.tag_size_m, "sample_phase": motion.phase if motion.phase in {"steady", "joint_zero"} else "sweep", "camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]", @@ -222,8 +339,15 @@ class ObservationCapture: self._latest_stamp_by_view[frame.view] = frame.stamp_ns generation = self._metadata_generation models = () if fixed_reference_only else self._motion_model_schedule(frame.view, motion) + 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 self.profile.acquisition.motion_model_scope == "task" and (motion is not None or self._motion_models): + relevant = self._observed_roles(motion) + relevant.update(role for model in models for role, _ in model.evidence_ids) + relevant.update(g.relation.parent_role for model in models for g in ( + model.constraints if model.image_model is None else model.image_model.geometry)) + tags = tuple(tag for tag in tags if tag.fixed_reference or tag.role in relevant) base = next(tag for tag in view.tags if tag.fixed_reference) cached = self.locked_reference(frame.view) fixed_reference = None if cached is None else FixedTagReference(*cached) @@ -234,16 +358,35 @@ class ObservationCapture: points = frame.corners.get(tag.role) if points is None: continue - pose, reason = self.trackers[frame.view].estimate(tag.role, points, - tag_size_m=tag.size_m, camera_matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns, - fixed_reference=fixed_reference if tag.fixed_reference else None, - **({"motion_managed": True} if tag.role in managed_roles else {})) + if tag.fixed_reference and frame.view in self._image_references: + reference_tracker = self._image_references[frame.view] + pose, reason = reference_tracker.estimate(tag.role, points, + size=tag.size_m, matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns, + locking=self.reference_lock.locking_enabled) + # Keep the session's independent drift witness live. A restored + # model additionally checks this same image against its own + # original root, preserving its geometry, hash and provenance. + if image_reference is None: + pose, reason = None, "image_reference_model_binding_conflict" + elif image_reference is not reference_tracker: + reference_tracker = image_reference + pose, reason = reference_tracker.estimate(tag.role, points, + size=tag.size_m, matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns, + locking=False) + evidence_tracker.register((tag.role,), reference_tracker) + else: + pose, reason = self.trackers[frame.view].estimate(tag.role, points, + tag_size_m=tag.size_m, camera_matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns, + fixed_reference=fixed_reference if tag.fixed_reference else None, + **({"motion_managed": True} if tag.role in managed_roles else {})) reasons[tag.role] = reason if pose is not None: selected[tag.role] = pose for model in models: poses, rejected, model_evidence = select_motion_poses(frame, model, self.trackers[frame.view], selected, base=base, cached=cached, + **({"reference_tracker": image_reference} + if frame.view in self._image_references else {}), **({"projection_worker": self.projection_worker} if self.projection_worker is not None else {})) evidence_tracker.register(self._model_children(model), model_evidence) for role in rejected: @@ -266,7 +409,7 @@ class ObservationCapture: pose, reason = None, "pose_branch_unconfirmed" evidence[tag.role] = item tracking_rejected = pose is None and ( - reason.startswith("pose_branch") or reason == "fixed_reference_pose_unverified") + reason.startswith(("pose_branch", "image_reference")) or reason == "fixed_reference_pose_unverified") if pose is None: selected.pop(tag.role, None) filtered.append(tag.tag_id) @@ -302,6 +445,8 @@ class ObservationCapture: return (), None self.last_missing[frame.view] = tuple(tag.tag_id for tag in view.tags if tag.role not in frame.corners) self.last_filtered[frame.view] = tuple(filtered) + if frame.view in self._image_references and not fixed_reference_only: + self._active_image_references[frame.view] = image_reference movement = self._observe_fixed_reference(frame, base, selected, evidence) # Include the last locking frame exactly once; never update mounting # fingerprints during task motion or while the worker is fitting. @@ -312,6 +457,7 @@ class ObservationCapture: events.append({"kind": "motion_branch_observation", "view": frame.view, "image_stamp_ns": frame.stamp_ns, "task_name": motion.task_key, "zero_joints": list(motion.reference_joints), "sample_phase": "zero_approach", + "reference_only": motion.reference_only, "command_unit": self.profile.command.unit, "command_vector": None if frame.command is None else list(frame.command), "feedback_vector": None if frame.feedback is None else list(frame.feedback), @@ -326,6 +472,7 @@ class ObservationCapture: events.append({"kind": "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, @@ -344,15 +491,23 @@ class ObservationCapture: task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key) streams = [item for item in observation_streams(self.profile, task) if item[2].view == frame.view] if motion.phase == "joint_zero": - streams = [item for item in streams if item[1] in motion.zero_joints] + if motion.reference_only: + streams = [item for item in observation_streams(self.profile, replace(task, joints=motion.zero_joints)) + if item[2].view == frame.view] + else: + 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 diagnostic: streams = [item for item in streams if item[1] in (accepted_joints or ())] - if not streams or frame.feedback is None or frame.command is None: + if not streams or (frame.feedback is None and not self.profile.vision_motion) or frame.command is None: return tuple(events), movement + from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE locked = [role for role, item in evidence.items() - if role in selected and item["pose_source"] == "verified_fixed_reference"] + if role in selected and item["pose_source"] in {"verified_fixed_reference", IMAGE_REFERENCE_SOURCE}] cached = self.locked_reference(frame.view) - if (base.role not in frame.corners and base.role not in frame.observed_roles + if (frame.view not in self._image_references + and base.role not in frame.corners and base.role not in frame.observed_roles and cached is not None and self.trackers[frame.view].freeze_roles({base.role: cached[1]})): selected[base.role] = cached[0] @@ -365,7 +520,8 @@ class ObservationCapture: image_geometry = any(has_image_model(evidence.get(role, {})) for role in roles) if image_geometry: roles = self._image_model_roles(roles, evidence) - if any(role not in selected or role not in frame.corners for role in roles): + if (self.profile.acquisition.motion_model_scope == "session" + and any(role not in selected or role not in frame.corners for role in roles)): # Every ancestor is verified in this image. A partial chain or # cached root cannot become an accepted constrained observation. return tuple(events), movement @@ -376,6 +532,11 @@ class ObservationCapture: common_from_view = self.extrinsics.transform(frame.view) unit = self.profile.command.unit for field, joint, spec in streams: + channel = channel_for_joint(self.profile, joint) + if image_geometry: + joint_roles = self._image_model_roles((spec.parent_role, spec.child_role), evidence) + if any(role not in selected or role not in frame.corners for role in joint_roles): + continue if spec.parent_role not in selected or spec.child_role not in selected: continue parent, child = selected[spec.parent_role], selected[spec.child_role] @@ -386,13 +547,15 @@ class ObservationCapture: "profile_id": self.profile.key.profile_id, field: joint, "task_name": task.key, "view": frame.view, "image_stamp_ns": frame.stamp_ns, "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + **segment_metadata(motion.segment_key), "sample_phase": motion.phase if motion.phase in {"steady", "joint_zero"} else "sweep", "command_direction_by_index": list(frame.command_directions), - "motor_index": task.command_index, - f"feedback_{unit}": frame.feedback[task.command_index], - f"command_{unit}": frame.command[task.command_index], - f"state_{unit}": list(frame.feedback), f"command_vector_{unit}": list(frame.command), - "state_image_sync_error_ms": abs(frame.skew_ns)/1e6, + "motor_index": channel, + f"feedback_{unit}": None if frame.feedback is None else frame.feedback[channel], + f"command_{unit}": frame.command[channel], + f"state_{unit}": None if frame.feedback is None else list(frame.feedback), f"command_vector_{unit}": list(frame.command), + "command_image_skew_ns": frame.command_skew_ns, + "state_image_sync_error_ms": None if frame.feedback is None else abs(frame.skew_ns)/1e6, "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(p), "child_pose_common": matrix_payload(c), @@ -404,7 +567,7 @@ class ObservationCapture: "child": evidence[spec.child_role]}} if motion.phase == "steady": row.update(steady_index=motion.steady_index, - steady_target=motion.target[task.command_index]) + steady_target=motion.target[channel]) if motion.sampling_plan_sha256: row.update(steady_training_nodes=list(motion.steady_training_nodes), sampling_plan_sha256=motion.sampling_plan_sha256) @@ -434,6 +597,13 @@ class ObservationCapture: if not self.reference_lock.locking_enabled: return movement revision = evidence[base.role]["candidate_diagnostics"]["branch_revision"] + if frame.view in self._image_references: + # This cache is a coordinate convention. No physical Tag normal or + # IPPE branch is selected/frozen by its image stationarity check. + with self._metadata_lock: + self.locked_poses.setdefault(frame.view, selected[base.role]) + self.locked_revisions.setdefault(frame.view, revision) + return movement with self._metadata_lock: if frame.view in self.locked_poses: return movement @@ -489,6 +659,43 @@ class ObservationCapture: return None return self.locked_poses[view], self.locked_revisions[view] + def image_reference_payloads(self): + """Declared coordinate frames, kept separate from physical Tag poses.""" + result = {} + for view, tracker in self._image_references.items(): + with tracker._lock: + if tracker.reference is not None: + result[view] = asdict(tracker.reference) + return result + + def freeze_reference_branches(self, revisions): + """Freeze physical branches with the same atomic fixed-image check.""" + with self._metadata_lock: + return self._freeze_reference_branches(revisions) + + def _freeze_reference_branches(self, revisions): + """Hold the selected reference context until physical branches freeze.""" + fixed = {(view.name, tag.role) for view in self.profile.vision.views + for tag in view.tags if tag.fixed_reference} + image_refs, physical, moving = [], [], [] + for (view, role), revision in revisions.items(): + if (view, role) in fixed and view in self._image_references: + tracker = self._active_image_references[view] + if tracker is None: + return False + image_refs.append((tracker, role, revision)) + else: + physical.append((self.trackers[view], role, revision)) + if (view, role) not in fixed: + moving.append((self.trackers[view], role)) + with ExitStack() as stack: + for tracker in sorted({tracker for tracker, _, _ in (*image_refs, *physical)}, key=id): + stack.enter_context(tracker._lock) + if not all(tracker.reference_confirmed(role, revision) for tracker, role, revision in image_refs): + return False + return (SquareTagPoseTracker.freeze_references(physical, require_motion_evidence=moving) + if physical else bool(image_refs)) + def locked_reference_views(self): with self._metadata_lock: return frozenset(self.locked_poses) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py new file mode 100644 index 0000000..ac46d2a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture_index.py @@ -0,0 +1,63 @@ +"""Live acquisition checks need an index, not a second copy of the journal. + +The complete image and pose evidence is written to JSONL by the coordinator. +Journal-backed finalization reads those bytes independently. Keep only fields +used by coverage, retry identity and training-knot selection in its live index. +In-process finalizers and diagnostic reports still request complete records. +""" + +from collections.abc import Sequence + + +class CaptureRecordIndex(Sequence): + def __init__(self, *, retain_complete, retain_training_geometry=False): + self.retain_complete = retain_complete + self.retain_training_geometry = retain_training_geometry + self._rows = [] + self._attempts = set() + + def __len__(self): + return len(self._rows) + + def __getitem__(self, index): + return self._rows[index] + + def reset(self, rows=()): + self._rows.clear() + self._attempts.clear() + self.extend(rows) + + def extend(self, rows): + for row in rows: + self.append(row) + + def append(self, row): + if self.retain_complete: + self._rows.append(row) + return + kind = row.get('kind') + if kind in {'joint_sample', 'secondary_joint_sample'}: + # Scalars contain native command/feedback and all scan identities. + # These two short sequences are the only non-scalar inputs to the + # live training-grid selector. Pose provenance stays in the journal. + indexed = {key: value for key, value in row.items() + if value is None or isinstance(value, (str, int, float, bool))} + for key in ('relative_quaternion_xyzw', 'steady_training_nodes'): + if key in row: + indexed[key] = tuple(row[key]) + if self.retain_training_geometry: + from ..core.fitting.training_quality import TRAINING_FIELDS + indexed.update({key: row[key] for key in TRAINING_FIELDS if key in row}) + self._rows.append(indexed) + elif kind in {'command_sampling_plan', 'training_decision'}: + self._rows.append(row) + elif self.retain_training_geometry and kind in {'session_start', 'joint_zero_reference', 'scan_unit_complete'}: + self._rows.append(row) + elif all(key in row for key in ('task_name', 'cycle', 'direction')): + # An unsuccessful later attempt can contain images but no valid + # joint sample. Preserve its identity so it cannot expose old data. + keys = ('task_name', 'cycle', 'direction', 'segment_key', 'attempt') + identity = tuple(row.get(key) for key in keys) + if identity not in self._attempts: + self._attempts.add(identity) + self._rows.append({key: row[key] for key in keys if key in row}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/checkpoint_extension.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/checkpoint_extension.py new file mode 100644 index 0000000..87ee9e6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/checkpoint_extension.py @@ -0,0 +1,138 @@ +"""Explicit, audited extension of a partial checkpoint with new trailing tasks. + +Ordinary resume remains strict. This offline operation verifies both protected +profiles and both replay contracts before creating a separate derived journal. +It does not authorize reuse of any physical zero or publish a calibration. +""" + +from dataclasses import dataclass, replace +import hashlib +import json +from pathlib import Path + +from ..core.domain.profile import CalibrationProfile +from ..core.urdf.partial_scope import profile_scope +from ..profiles import load_hand_profile +from .engine import CalibrationEngine +from .joint_resume import JointResume, PreparedResume + + +@dataclass(frozen=True) +class ScopeExtension: + source: CalibrationProfile + target: CalibrationProfile + source_yaml: bytes + target_yaml: bytes + + @classmethod + def load(cls, source_path, target_path): + source, target = load_hand_profile(source_path), load_hand_profile(target_path) + expanded = load_hand_profile(source_path, scope=target.scope.default_scope) + if not source.retained_joints > target.retained_joints: + raise ValueError("checkpoint_extension_requires_larger_scope") + if source.motion.tasks != target.motion.tasks[:len(source.motion.tasks)]: + raise ValueError("checkpoint_extension_requires_unchanged_task_prefix") + # Expanding the original protected file must yield exactly the target + # contract, allowing only the placement of the previously omitted tasks. + reordered = replace(target, motion=replace(target.motion, tasks=expanded.motion.tasks)) + if expanded != reordered: + raise ValueError("checkpoint_extension_changes_measurement_contract") + if CalibrationEngine(source).capture_schedule_version != CalibrationEngine(target).capture_schedule_version: + raise ValueError("checkpoint_extension_changes_capture_schedule") + return cls(source, target, Path(source_path).read_bytes(), Path(target_path).read_bytes()) + + def metadata(self, checkpoint, protected_inputs, source_path, source_digest): + old_hash = hashlib.sha256(self.source_yaml).hexdigest() + new_hash = hashlib.sha256(self.target_yaml).hexdigest() + if protected_inputs.get("profile_config_sha256") != new_hash: + raise ValueError("checkpoint_extension_target_profile_hash_invalid") + expected = {**protected_inputs, "profile_config_sha256": old_hash} + for row in (checkpoint.header, checkpoint.fixed_reference): + actual = row.get("protected_hashes", row) + if any(actual.get(key) != value for key, value in expected.items()): + raise ValueError("checkpoint_extension_source_inputs_changed") + header = {**checkpoint.header, **protected_inputs, "calibration_scope": profile_scope(self.target), + "resume_checkpoint_requested": True} + if "protected_hashes" in header: + header["protected_hashes"] = {**header["protected_hashes"], **protected_inputs} + reference = {**checkpoint.fixed_reference, "protected_hashes": { + **checkpoint.fixed_reference["protected_hashes"], **protected_inputs}} + audit = {"kind": "capture_scope_extension", "policy": "unchanged_prefix_scope_extension_v1", + "source_raw_samples": str(Path(source_path).resolve()), "source_raw_sha256": source_digest, + "source_profile_sha256": old_hash, "target_profile_sha256": new_hash, + "original_header": checkpoint.header, "original_fixed_reference": checkpoint.fixed_reference, + "target_scope": profile_scope(self.target), + "reused_task_prefix": [task.key for task in self.source.motion.tasks], + "appended_tasks": [task.key for task in self.target.motion.tasks[len(self.source.motion.tasks):]], + "physical_reference_reverification_required": True} + return header, reference, audit + + +def _file_sha256(path): + with Path(path).open("rb") as stream: + return hashlib.file_digest(stream, "sha256").hexdigest() + + +def extend_checkpoint(extension, source_path, output_dir, *, serial_number, + protected_inputs, image_replay_workers=1): + """Preserve sample lines verbatim; only session metadata gains a new scope.""" + source_path, output_dir = Path(source_path), Path(output_dir) + if output_dir.exists(): + raise FileExistsError(output_dir) + source_digest = _file_sha256(source_path) + checkpoint = PreparedResume.load(extension.source, CalibrationEngine(extension.source), + source_path, serial_number, image_replay_workers=image_replay_workers) + header, reference, audit = extension.metadata(checkpoint, protected_inputs, source_path, source_digest) + replacement = {"session_start": header, "fixed_base_reference_locked": reference} + rows = tuple(replacement.get(row.get("kind"), row) for row in checkpoint.rows) + (audit,) + verified = JointResume(extension.target, image_replay_workers=image_replay_workers) + verified.stage(CalibrationEngine(extension.target), rows, checkpoint.completed_units) + if (verified.units != checkpoint.joints.units or verified.references != checkpoint.joints.references + or verified.first_cycle_spans != checkpoint.joints.first_cycle_spans): + raise ValueError("checkpoint_extension_changes_reusable_evidence") + output_dir.mkdir(parents=True, exist_ok=False) + temporary = output_dir / "raw_samples.jsonl.tmp" + digest = hashlib.sha256() + with source_path.open("rb") as source, temporary.open("xb") as target: + for line in source: + digest.update(line) + row = json.loads(line) if line.strip() else {} + if row.get("kind") in replacement: + target.write((json.dumps(replacement[row["kind"]], ensure_ascii=False) + "\n").encode()) + else: + target.write(line) + target.write(("\n" + json.dumps(audit, ensure_ascii=False) + "\n").encode()) + if digest.hexdigest() != source_digest: + temporary.unlink() + raise ValueError("checkpoint_extension_source_changed_during_replay") + (output_dir / "source_profile.yaml").write_bytes(extension.source_yaml) + (output_dir / "target_profile.yaml").write_bytes(extension.target_yaml) + (output_dir / "scope_extension.json").write_text(json.dumps({**audit, + "reusable_units": len(verified.units), "result": "STATIC_REPLAY_PASSED"}, + ensure_ascii=False, indent=2) + "\n") + temporary.rename(output_dir / "raw_samples.jsonl") + return output_dir + + +def main(): + import argparse + from ..product import load_product_config + from .image_replay import checkpoint_replay_workers + from .runner_support import protected_inputs + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--source-profile", required=True) + parser.add_argument("--target-product", required=True) + parser.add_argument("--source-checkpoint", required=True) + parser.add_argument("--output-dir", required=True) + args = parser.parse_args() + product = load_product_config(args.target_product, check_can=False) + extension = ScopeExtension.load(args.source_profile, product.profile_config) + result = extend_checkpoint(extension, args.source_checkpoint, args.output_dir, + serial_number=product.serial_number, protected_inputs=protected_inputs(product), + image_replay_workers=checkpoint_replay_workers()) + print(f"已创建扩展范围断点：{result}；实机基准和逐关节零位仍须重新验证。", flush=True) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py index 4a11122..dc6a381 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -8,7 +8,8 @@ and pose estimation never hold the state lock while doing expensive work. from __future__ import annotations from collections import deque -from dataclasses import asdict +from dataclasses import asdict, replace +from ..core.domain.motion_path import record_scan_identity import hashlib import json import threading @@ -20,6 +21,7 @@ from ..acquisition import StateSample, interpolate_state_u8 from ..core.domain.profile import CalibrationProfile from ..core.domain.reference import build_joint_zero_reference from ..core.geometry.pnp import POSE_TRACKING_POLICY_VERSION, SquareTagPoseTracker +from ..core.urdf.partial_scope import profile_scope from ..storage import append_jsonl, append_jsonl_many, atomic_write_json, sync_jsonl from .engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION from .reference_lock import ReferenceLock @@ -40,6 +42,7 @@ from .resume import ResumeDecision, ResumeVerifier, fingerprint_from_mapping from .safety import SafetyPolicy, SafetySample from .session import CalibrationPhase as Phase from .snapshot import DeviceReadiness, CameraReadiness, RuntimeSnapshot, build_snapshot, legacy_state +from ..core.domain.capture_plan import CapturePlan, ADAPTIVE _TERMINAL_STATES = frozenset({"PASSED", "DIAGNOSTIC_COMPLETE", "PAUSED", "ABORTED", "FAILED"}) @@ -50,6 +53,9 @@ class CalibrationCoordinator: ports: RuntimePorts, adapter_factory: AdapterFactory, *, finalization=None, initialization_solver=None, projection_worker=None): self.profile, self.parameters, self.ports = profile, parameters, ports + from .timing_diagnostics import StageTiming + self.stage_timing = StageTiming(ports.monotonic()) + self._timing_saved = False self.initialization_solver = initialization_solver self.projection_worker = projection_worker self.step_data_lock = threading.RLock() @@ -64,6 +70,13 @@ class CalibrationCoordinator: not parameters.commands_enabled or parameters.resume_raw_samples_path is not None): raise ValueError("diagnostic_capture_requires_fresh_command_enabled_session") self.execution = SessionExecution(profile, diagnostic_capture=self.diagnostic_capture) + self.training_worker = None + self._training_request = None + if profile.quality.training_policy == ADAPTIVE: + if self.diagnostic_capture is not None: + raise ValueError("adaptive_training_cannot_run_as_diagnostic") + from .training import TrainingWorker + self.training_worker = TrainingWorker() self.safety = SafetyPolicy(profile) self.reference_lock = self._new_reference_lock() self.trackers = parameters.new_trackers(self.required_observation_views) @@ -76,7 +89,11 @@ class CalibrationCoordinator: self.command_history: deque[StateSample] = deque(maxlen=2000) self.command_direction_history = deque(maxlen=2000) self._command_directions = [""]*self.command_count - self.raw_records = [] + self.finalization = finalization or FinalizationController(profile, parameters.session_dir) + from .capture_index import CaptureRecordIndex + self.capture_index = CaptureRecordIndex(retain_complete=( + self.diagnostic_capture is not None or not getattr(self.finalization, "uses_journal", False)), + retain_training_geometry=profile.quality.training_policy == ADAPTIVE) self._image_record_stamps = {} self.last_command = None self.commanded_speed = None @@ -95,18 +112,31 @@ class CalibrationCoordinator: self._zero_geometry_started_at = None self._zero_geometry_completed_at = None self._zero_capture_started_at = None - self._resume_message = "diagnostic_capture_no_resume" if self.diagnostic_capture else "not_requested" + self._resume_message = ("diagnostic_capture_no_resume" if self.diagnostic_capture else + "已请求按通过任务续采，不复核已完成关节零位" if parameters.resume_raw_samples_path is not None + and parameters.resume_mode == "passed" else + "waiting_for_reference_verification" if parameters.resume_raw_samples_path is not None else + "not_requested") self._resumed_count = 0 self._resume_import = None + self._reference_import = None + self._passed_reference_import = None + from .visual_motion import VisualMotionObserver + self.visual_motion = VisualMotionObserver(profile) if profile.vision_motion else None + self._visual_wait_started_at = None from .joint_resume import JointResume, PreparedResume self.joint_resume = JointResume(profile) self._resume_checkpoint, self._resume_checkpoint_error = None, "" if parameters.resume_raw_samples_path is not None: + self.stage_timing.enter("resume_loading", ports.monotonic()) + from .image_replay import checkpoint_replay_workers try: self._resume_checkpoint = PreparedResume.load(profile, self.execution.session.engine, - parameters.resume_raw_samples_path, parameters.serial_number) + parameters.resume_raw_samples_path, parameters.serial_number, + image_replay_workers=checkpoint_replay_workers(), resume_mode=parameters.resume_mode) except (OSError, ValueError, KeyError, TypeError) as error: self._resume_checkpoint_error = str(error) + self.stage_timing.enter("startup", ports.monotonic()) self._unit_rows = [] self._capture_unit_key = None self._steady_rows = [] @@ -130,24 +160,39 @@ class CalibrationCoordinator: self.raw_path = parameters.session_dir / "raw_samples.jsonl" self._session_header = { "kind": "session_start", "sample_schema_version": profile.artifacts.output_schema_version, + "calibration_scope": profile_scope(profile), "profile_id": profile.key.profile_id, "serial_number": parameters.serial_number, "curve_input_domain": profile.curve_input_domain, "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "capture_schedule_version": self.execution.session.engine.capture_schedule_version, + "capture_plan": CapturePlan.from_profile(profile).as_dict(), "pose_tracking_policy_version": POSE_TRACKING_POLICY_VERSION, "required_observation_views": list(self.required_observation_views), **parameters.protected_inputs, "resume_checkpoint_requested": parameters.resume_raw_samples_path is not None, + "resume_mode": parameters.resume_mode, + "motion_observation": profile.acquisition.motion_observation, + "fixed_reference_mode": profile.acquisition.fixed_reference_mode, + "initial_command_observation": {"values": list(parameters.initial_command), + "device_uid": parameters.initial_device_uid, + "source": "sdk_target_position_register" if parameters.initial_command else "adapter_feedback"}, **({"diagnostic_capture": self.diagnostic_capture.as_dict()} if self.diagnostic_capture else {}), } if profile.acquisition.command_capture_mode == "separate" and self.diagnostic_capture is None: from ..core.fitting.command_sampling import SAMPLING_POLICY self._session_header["command_sampling_policy"] = SAMPLING_POLICY append_jsonl(self.raw_path, self._session_header) - self.raw_records.append(self._session_header) + self.capture_index.append(self._session_header) self.cameras = CameraObservations(parameters.extrinsics, self.raw_path, ports.monotonic) self.sdk_adapter = adapter_factory(self._write_position, self._write_speed, self.feedback_fresh) - self.finalization = finalization or FinalizationController(profile, parameters.session_dir) + + @property + def raw_records(self): + """Explicit full-evidence access; live checks use capture_index instead.""" + if self.capture_index.retain_complete: + return list(self.capture_index) + from .acquisition import load_capture + return load_capture(self.raw_path) @property def state(self) -> str: @@ -179,16 +224,21 @@ class CalibrationCoordinator: self.capture = ObservationCapture(self.profile, reference_lock=self.reference_lock, extrinsics=self.parameters.extrinsics, tag_size_m=self.parameters.tag_size_m, trackers=self.trackers, projection_worker=self.projection_worker) + if self.profile.vision_motion: + from .visual_motion import VisualMotionObserver + self.visual_motion = VisualMotionObserver(self.profile) + self._visual_wait_started_at = None self.state_history.clear() self.command_history.clear() self.command_direction_history.clear() self._command_directions = [""]*self.command_count # Both the in-memory and process finalizers consume a self-contained # capture, including the source identity used by CAD constraints. - self.raw_records[:] = [self._session_header] + self.capture_index.reset((self._session_header,)) self._image_record_stamps.clear() self._branch_observation_reasons.clear() self.branch_initialization.clear() + self.branch_initialization.parent_references.clear() self.execution.session.start(resume_requested=self.parameters.resume_raw_samples_path is not None) self._started_at = self.ports.monotonic() self._observation_epoch += 1 @@ -225,7 +275,33 @@ class CalibrationCoordinator: request, event, stamp_ns=self.ports.clock_ns(), monotonic_time=self.ports.monotonic(), **details) + def receive_motion_observation(self, observation: DetectionInput) -> None: + """Observe raw pixels before any potentially slow geometry worker. + + The ROS callback and offline host use the same idempotent boundary. + A branch solve cannot make current camera evidence appear stale. + """ + if self.visual_motion is None: + return + from .detection_filter import filter_detections + view, stamp = observation.view, observation.stamp_ns + with self.step_data_lock: + if (view not in self.cameras.image_sizes or self.finalization.started + or self.state in _TERMINAL_STATES + or not 0 <= self.ports.clock_ns()-stamp <= self.profile.acquisition.visual_stale_seconds*1e9 + or (self._motion is not None and stamp < self._motion_started_ns)): + return + width, height = self.cameras.image_sizes[view] + corners, _, _ = filter_detections(self._view(view), self.parameters.detection, + observation, width, height, self._motion) + command = next((row.position_u8 for row in reversed(self.command_history) + if 0 <= stamp-row.stamp_ns <= self.parameters.maximum_state_image_skew_ns), None) + self.visual_motion.observe(view, stamp, corners, now=self.ports.monotonic(), + epoch=self._observation_epoch, version=self._segment_number, command=command) + self._flush_visual_evidence() + def receive_detections(self, observation: DetectionInput) -> None: + self.receive_motion_observation(observation) view, stamp = observation.view, observation.stamp_ns received_stamp = self.ports.clock_ns() with self.step_data_lock: @@ -241,7 +317,8 @@ class CalibrationCoordinator: epoch = self._observation_epoch motion_version = self._segment_number initialization = self.branch_initialization.request(view, motion, - session_epoch=epoch, motion_version=motion_version) + session_epoch=epoch, motion_version=motion_version, + zero_references=self.execution.zero_references) if initialization is not None and self._zero_geometry_started_at is None: self._zero_geometry_started_at = self.ports.monotonic() accepted_joints = self._diagnostic_accepted_joints(motion) @@ -286,7 +363,7 @@ class CalibrationCoordinator: return model, report = accepted append_jsonl(self.raw_path, report) - self.raw_records.append(report) + self.capture_index.append(report) if not self.branch_initialization.pending(): self._zero_geometry_completed_at = self.ports.monotonic() if model is not None: @@ -302,32 +379,19 @@ class CalibrationCoordinator: accepted_joints = self._diagnostic_accepted_joints(motion) self._record_solver_event(initialization, "commit_accepted", resolved=resolution.resolved) - roles = {tag.tag_id: tag.role for tag in self._view(view).tags} - corners, quality_events = {}, [] - observed_roles = frozenset(roles[item.tag_id] for item in observation.detections if item.tag_id in roles) - for detection in observation.detections: - points = np.asarray(detection.corners, dtype=float) - if detection.border_quality is not None and not detection.border_quality["passed"]: - quality_events.append({"kind": "detection_quality_rejected", "view": view, - "image_stamp_ns": stamp, "tag_id": detection.tag_id, - "corners_xy": points.tolist(), "quality": dict(detection.border_quality), - "task_name": None if motion is None else motion.task_key, - "motion_phase": None if motion is None else motion.phase}) - continue - if (detection.tag_id not in roles or points.shape != (4, 2) or not np.all(np.isfinite(points)) - or detection.hamming > self.parameters.detection.maximum_hamming or detection.decision_margin < self.parameters.detection.minimum_decision_margin - or np.min(np.linalg.norm(points-np.roll(points, -1, axis=0), axis=1)) < self.parameters.detection.minimum_edge_pixels - or np.min(points) < 2 or np.max(points[:, 0]) > width-3 or np.max(points[:, 1]) > height-3): - continue - corners[roles[detection.tag_id]] = points + from .detection_filter import filter_detections + corners, quality_events, observed_roles = filter_detections(self._view(view), + self.parameters.detection, observation, width, height, motion) matched = interpolate_state_u8(feedback_history, stamp, maximum_skew_ns=self.parameters.maximum_state_image_skew_ns) # Commands are sample-and-held by the transport, NOT interpolated # from a future setpoint or read from the current motion timer. - command = next((row.position_u8 for row in reversed(command_history) + command_sample = next((row for row in reversed(command_history) if row.stamp_ns <= stamp and stamp-row.stamp_ns <= self.parameters.maximum_state_image_skew_ns), None) + command = None if command_sample is None else command_sample.position_u8 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) + 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) 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 @@ -357,8 +421,22 @@ class CalibrationCoordinator: if movement is not None: self._pause(f"fixed_reference_moved:{view}:{movement.drift_px}") return + if self.visual_motion is not None: + if self.segment is not None and motion is not None and motion.phase in {"steady", "joint_zero"}: + settled = self.visual_motion.settle_record(self.segment.finished_at, + self.ports.monotonic(), epoch=epoch, through_stamp=stamp) + if not settled: + rows = tuple(row for row in rows if row.get("kind") not in { + "joint_zero_sample", "joint_sample", "secondary_joint_sample", + "pnp_candidate_frame", "image_observation_frame"}) + self._flush_visual_evidence() + # A cached parent or a late image cannot borrow another + # exposure's motion/stability witness. + rows = tuple(row for row in rows if row.get("kind") not in { + "joint_zero_sample", "joint_sample", "secondary_joint_sample"} + or self.visual_motion.has_image(row.get("joint", row.get("observation_joint")), stamp)) branch_events = [row for row in rows if row.get("kind") == "pnp_branch_event"] - self.raw_records.extend(branch_events) + self.capture_index.extend(branch_events) # Per-record fsync while holding this lock stalls SDK feedback # and command ticks. Persist references and scan checkpoints; # flush continuous image evidence in batches between them. @@ -390,17 +468,22 @@ class CalibrationCoordinator: row["accepted_sample_window_open"] = record_motion is not None if row.get("kind") == "motion_branch_observation": row.update(session_epoch=epoch, motion_version=motion_version) + if motion.reference_path_index is not None: + row["reference_path_index"] = motion.reference_path_index self.branch_initialization.observe(row) - self.raw_records.append(row) + self.capture_index.append(row) continue for item in row.get("pnp_observation_evidence", {}).values(): diagnostics = item["candidate_diagnostics"] if (diagnostics.get("observation_stamp_ns") == row.get("image_stamp_ns") and diagnostics.get("branch_status") == "tracking"): self._branch_observation_reasons.pop((row["view"], item["tag_role"]), None) - self.raw_records.append(row) + self.capture_index.append(row) self._unit_rows.append(row) if motion is not None and motion.phase in {"steady", "joint_zero"}: + if self.visual_motion is not None and row.get("kind") in { + "joint_zero_sample", "joint_sample", "secondary_joint_sample"}: + row["visual_settled_evidence_id"] = self.visual_motion.window_id self._steady_rows.append(row) if row.get("kind") == "joint_zero_sample": row["capture_timing"] = { @@ -417,7 +500,8 @@ class CalibrationCoordinator: if self.diagnostic_capture is None: return None if motion is not None and motion.phase == "joint_zero": - return frozenset(motion.zero_joints) if self.branch_initialization.ready(motion) else frozenset() + ready = motion.reference_reuse or self.branch_initialization.ready(motion) + return frozenset(motion.zero_joints) if ready else frozenset() return frozenset(self.execution.zero_references) def _accept_branch_event(self, row): @@ -458,6 +542,15 @@ class CalibrationCoordinator: def tick(self) -> None: with self.step_data_lock: self._advance() + from .timing_diagnostics import acquisition_stage + self.stage_timing.enter(acquisition_stage(self), self.ports.monotonic()) + if self.state in _TERMINAL_STATES and not self._timing_saved: + self._save_stage_timing() + + def _save_stage_timing(self): + atomic_write_json(self.parameters.session_dir / "stage_timing.json", + self.stage_timing.as_dict(self.ports.monotonic())) + self._timing_saved = True def snapshot(self) -> RuntimeSnapshot: with self.step_data_lock: @@ -475,6 +568,9 @@ class CalibrationCoordinator: sync_jsonl(self.raw_path) finally: self.finalization.close() + if self.training_worker is not None: + self.training_worker.close() + self._save_stage_timing() def _advance(self): if self.state in _TERMINAL_STATES: @@ -495,11 +591,12 @@ class CalibrationCoordinator: return current = self.last_command if current is None: - if not self.state_history: + if not self.state_history and not self.profile.vision_motion: if now-self._started_at > self.profile.acquisition.feedback_stale_seconds: self._pause("feedback_stale:开始后超过一秒没有新鲜反馈") return # Require a post-Start observation, not a preview sample. - current = self.sdk_adapter.initial_command(self.latest_feedback) + current = (self.parameters.initial_command if self.profile.vision_motion + else self.sdk_adapter.initial_command(self.latest_feedback)) decision = self.safety.evaluate(SafetySample(now, self.state_receive_times[-1] if self.state_receive_times else None, tuple(current), tuple(self.latest_feedback), fresh, @@ -510,11 +607,37 @@ class CalibrationCoordinator: if not decision.safe: self._pause(f"{decision.code}:{decision.reason}:{decision.details}") return + if self.visual_motion is not None and self.segment is not None: + reason = self.visual_motion.check(self.segment, now) + if reason: + self._pause(reason) + return if self.finalization.started: self._publish_command(list(current)) self._poll_finalization() return phase = session.phase + if self._passed_reference_import is not None: + self._publish_command(list(current)) + pending = self._passed_reference_import + self._write_resume_batch(pending) + if pending.complete: + from .passed_resume import restore_parent_models, restored_model_record + binding = restored_model_record(pending.model_records, + self._observation_epoch, self.ports.clock_ns()) + append_jsonl(self.raw_path, binding) + self.capture_index.append(binding) + restore_parent_models(self.branch_initialization.parent_references, + pending.model_records, self._observation_epoch) + self.execution.zero_references.update(pending.references) + if self.visual_motion is not None: + self.visual_motion.freeze_zeros(pending.references) + self._passed_reference_import = None + return + if self._reference_import is not None and not self._reference_import.complete: + self._publish_command(list(current)) + self._write_resume_batch(self._reference_import) + return if self._resume_import is not None: self._publish_command(list(current)) self._import_resume_batch() @@ -523,7 +646,8 @@ class CalibrationCoordinator: used, imported = self.joint_resume.take_task(session.current_unit.task_key) if used: from .joint_resume import TaskResumeImport - self._resume_import = TaskResumeImport(session.current_unit.task_key, used, imported) + self._resume_import = TaskResumeImport(rows=imported, + task_key=session.current_unit.task_key, units=used) self._publish_command(list(current)) self._import_resume_batch() return @@ -542,17 +666,68 @@ class CalibrationCoordinator: return if phase == Phase.EVALUATE: action = self.execution.action - quality = self.execution.evaluate(self.raw_records) + training_decision = None + if self.execution.training_check_due(): + from .scan_quality import evaluate_capture_unit + preliminary = evaluate_capture_unit(self.execution.profile, action.scan_unit, action.retry + 1, + self.capture_index, first_cycle_spans=self.execution.first_cycle_spans) + if preliminary.passed: + # Close this motion's image identity before freezing the snapshot. + # Feedback and fixed-reference monitoring continue while fitting. + self._motion = None + if self._training_request is None: + from ..core.fitting.training_quality import training_snapshot + cycles = tuple(range(action.scan_unit.cycle + 1)) + self._training_request = dict(profile=self.execution.profile, + task_key=action.scan_unit.task_key, cycles=cycles, + rows=training_snapshot(self.execution.profile, self.capture_index, + action.scan_unit.task_key, cycles), + references=dict(self.execution.zero_references), source_urdf=self.parameters.source_urdf) + try: + training_decision = self.training_worker.poll(**self._training_request) + except Exception as error: + self._pause(f"training_assessment_failed:{error}") + return + if training_decision is None: + self.reason = "training_quality_assessment:" + action.scan_unit.task_key + self._publish_command(list(current)) + return + self._training_request = None + quality = self.execution.evaluate(self.capture_index, training_decision=training_decision) record = {"kind": "scan_unit_complete", "task_name": action.scan_unit.task_key, "cycle": action.scan_unit.cycle, "direction": action.scan_unit.direction, + **({"segment_key": action.scan_unit.segment_key} if action.scan_unit.segment_key else {}), "attempt": action.retry+1, "passed": quality.passed, + **({"capture_plan_sha256": CapturePlan.from_profile(self.execution.profile).sha256} + if action.retry else {}), "failures": list(quality.failures), "metrics": dict(quality.metrics), **({"quality_scope": "diagnostic_observation_coverage", "accepted_pose_quality": self.execution.last_diagnostic_quality.accepted_pose_report(), "observation_timing": dict(self.execution.last_diagnostic_quality.observation_timing)} if self.execution.last_diagnostic_quality is not None else {})} - self.raw_records.append(record) + self.capture_index.append(record) append_jsonl(self.raw_path, record) + if self.execution.last_training_decision is not None: + decision = self.execution.last_training_decision + append_jsonl(self.raw_path, decision) + self.capture_index.append(decision) + if decision["decision"] == "fail": + self._observation_epoch += 1 + self.branch_initialization.clear() + self.training_worker.close() + self._publish_command(list(current)) + self.reason = "training_quality_failed:" + ",".join(decision["failures"]) + return + task_quality = self.execution.last_task_quality + if task_quality is not None: + task_record = {"kind": "task_input_support_checked", "task_name": action.scan_unit.task_key, + "passed": task_quality.passed, "failures": list(task_quality.failures), + "metrics": dict(task_quality.metrics)} + self.capture_index.append(task_record) + append_jsonl(self.raw_path, task_record) + if not task_quality.passed: + self._pause("task_input_support_failed:"+",".join(task_quality.failures)) + return if session.phase == Phase.PAUSED: if "observation_timing" in record: pause = session.status().pause @@ -579,6 +754,17 @@ class CalibrationCoordinator: if self.segment is None and self._reuse_settled_preparation(motion, now): return if self.segment is None: + if self.visual_motion is not None: + names = self.visual_motion.names_for(motion, current) + if not self.visual_motion.available(names, now): + if self._visual_wait_started_at is None: + self._visual_wait_started_at = now + self._publish_command(list(current)) + self.reason = "waiting_for_visual_motion_tags:" + ",".join(names) + if now-self._visual_wait_started_at >= self.profile.acquisition.steady_timeout_seconds: + self._pause(self.reason) + return + self._visual_wait_started_at = None if self.commanded_speed != motion.speed: self._publish_speed(motion.speed) self.step_speed_ready_at = now+float(self.parameters.speed_settle_seconds) @@ -595,11 +781,20 @@ class CalibrationCoordinator: self._reference_capture_key = None self._reference_observation_started = None self._begin_motion_capture(motion) + if session.phase == Phase.PAUSED: + return + if self.visual_motion is not None: + self.visual_motion.begin(motion, current, now=now, version=self._segment_number) + self._flush_visual_evidence() self.segment = MotionExecution(self.profile, motion, initial_command=current, - initial_feedback=self.latest_feedback, now=now, identity=str(self._segment_number)) + initial_feedback=self.latest_feedback, now=now, identity=str(self._segment_number), + visual_observer=self.visual_motion) shaped = self.segment.sample(now) self._publish_command(list(shaped)) - self.segment.observe(self.latest_feedback, stamp=self.state_history[-1].stamp_ns, now=now) + self.segment.observe(self.latest_feedback, stamp=self.state_history[-1].stamp_ns if self.state_history else None, now=now) + if self.visual_motion is not None: + self.visual_motion.settle_record(self.segment.finished_at, now, epoch=self._observation_epoch) + self._flush_visual_evidence() self.reason = motion.phase+":"+str(motion.task_key) steady_complete = False if motion.phase == "steady": @@ -611,7 +806,8 @@ class CalibrationCoordinator: 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)) + 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) else: self._reset_steady_capture(motion, now) @@ -619,9 +815,16 @@ class CalibrationCoordinator: if self.segment.steady_ready(now): if self._steady_capture_after_ns is None: self._steady_capture_after_ns = self.ports.clock_ns() - steady_complete = self._lock_joint_zeros(motion) + steady_complete = (self._confirm_model_reference(motion) if motion.reference_only else + self._lock_joint_zeros(motion)) if session.phase == Phase.PAUSED: return + if self._reference_import is not None: + # The pose has already been measured and checked. Journal + # transfer is not an observation timeout; keep holding it + # while feedback and safety run between bounded writes. + self.reason = "joint_zero_resume_evidence_import:" + str(motion.task_key) + return else: self._reset_steady_capture(motion, now) if self.branch_initialization.pending(): @@ -681,7 +884,8 @@ class CalibrationCoordinator: # entire fresh capture. Never forge a matching fingerprint. self._reference_capture_key = None if self.segment.steady_ready(now) or ( - motion.phase not in {"steady", "joint_zero"} and self.segment.arrived(now)): + motion.phase not in {"steady", "joint_zero"} and not motion.defer_settling_to_hold + and self.segment.arrived(now)): self._settled_pose = (tuple(motion.target), tuple(self.latest_feedback), now) self.execution.motion_complete() if phase == Phase.REFERENCE_POSES and session.phase != phase: @@ -699,7 +903,7 @@ class CalibrationCoordinator: "feedback_window_seconds": self.profile.acquisition.steady_window_seconds, "feedback_history": [{"age_seconds": now-stamp, "values": list(values)} for stamp, values in list(self.segment.stamped_history)[-20:]]} - self.raw_records.append(record) + self.capture_index.append(record) append_jsonl_many(self.raw_path, (record,), durable=False) self._steady_capture_after_ns = None self._steady_rows.clear() @@ -711,7 +915,7 @@ class CalibrationCoordinator: try: sync_jsonl(self.raw_path) report = build_diagnostic_report(self.profile, self.diagnostic_capture, - raw_path=self.raw_path, records=self.raw_records, + raw_path=self.raw_path, records=self.capture_index, references=self.execution.zero_references, diagnostic_zero_attempts=self.execution.diagnostic_zero_attempts, completed_units=self.execution.completed_units, @@ -742,10 +946,66 @@ class CalibrationCoordinator: self.execution.record_diagnostic_zero_attempt(attempt) row = attempt.as_record() append_jsonl(self.raw_path, row) - self.raw_records.append(row) + self.capture_index.append(row) return True + def _confirm_model_reference(self, motion): + """Authorize a held parent in this pose without replacing its measured zero.""" + if not motion.reference_reuse and not self.branch_initialization.ready(motion): + return False + minimum = self.profile.acquisition.fixed_reference_minimum_frames + rows = [] + for name in motion.zero_joints: + specs = [self.profile.measurement.measurements[name]] + secondary = self.profile.measurement.cross_view_sources.get(name) + if secondary is not None: + specs.append(self.profile.measurement.measurements[secondary]) + for spec in specs: + frames = {row["image_stamp_ns"]: row for row in self._steady_rows + if row.get("kind") == "joint_zero_sample" and row.get("joint") == name + and row.get("view") == spec.view} + if len(frames) < minimum: + self._zero_reference_error = ( + f"parent_reference_samples_missing:{name}:{spec.view}:{len(frames)}/{minimum}") + return False + rows.extend(frames[stamp] for stamp in sorted(frames)[-minimum:]) + try: + revisions = reference_branch_revisions(rows, profile=self.profile, require_motion_evidence=True) + record = None + if motion.reference_reuse: + task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) + reference = self.execution.zero_references.get(task.parent_reference.pose_joint) + if reference is None: + raise ValueError("parent_reference_zero_missing") + record = self.branch_initialization.parent_references.confirm(task, + self._observation_epoch, self._segment_number, reference, rows) + except ValueError as error: + self._zero_reference_error = str(error) + return False + if not self._freeze_reference_branches(revisions): + self._zero_reference_error = "parent_reference_motion_branch_not_confirmed" + return False + if record is not None: + append_jsonl(self.raw_path, record) + self.capture_index.append(record) + return True + + def _freeze_reference_branches(self, revisions): + return self.capture.freeze_reference_branches(revisions) + def _lock_joint_zeros(self, motion): + pending = self._reference_import + if pending is not None: + if (pending.motion_version != self._segment_number + or set(pending.references) != set(motion.zero_joints)): + raise RuntimeError("resume_reference_motion_changed_during_import") + if not pending.complete: + return False + self.execution.zero_references.update(pending.references) + if self.visual_motion is not None: + self.visual_motion.freeze_zeros(pending.references) + self._reference_import = None + return True if not self.branch_initialization.ready(motion): self._zero_reference_error = (self.branch_initialization.error() or "motion_geometry_confirmation_pending") @@ -773,30 +1033,35 @@ class CalibrationCoordinator: return False if references.keys() & self.execution.zero_references.keys(): raise RuntimeError("a frozen joint reference cannot be replaced") + current_references = dict(references) try: references = {name: self.joint_resume.verify(reference) for name, reference in references.items()} except ValueError as error: self._pause(str(error)) return False - moving = {(view.name, tag.role) for view in self.profile.vision.views - for tag in view.tags if not tag.fixed_reference} - if not SquareTagPoseTracker.freeze_references([ - (self.trackers[view], role, revision) for (view, role), revision in revisions.items() - ], require_motion_evidence=tuple((self.trackers[view], role) - for view, role in revisions if (view, role) in moving)): + if not self._freeze_reference_branches(revisions): self._zero_reference_error = "joint_zero_motion_branch_not_confirmed" return False - self.execution.zero_references.update(references) + for reference in current_references.values(): + self.branch_initialization.parent_references.remember_zero(reference, self._observation_epoch) provenance = self.joint_resume.take_motion_provenance_records() - append_jsonl_many(self.raw_path, provenance) - self.raw_records.extend(provenance) - for reference in references.values(): - row = reference.as_record() - self.raw_records.append(row) - append_jsonl(self.raw_path, row) + reference_rows = tuple(reference.as_record() for reference in references.values()) + if provenance: + from .joint_resume import ReferenceResumeImport + self._reference_import = ReferenceResumeImport( + rows=(*provenance, *reference_rows), references=references, + motion_version=self._segment_number) + return False + append_jsonl_many(self.raw_path, reference_rows) + self.capture_index.extend(reference_rows) + self.execution.zero_references.update(references) + if self.visual_motion is not None: + self.visual_motion.freeze_zeros(references) return True def _reuse_settled_preparation(self, motion, now): + if self.profile.vision_motion: + return False # A repeated command still requires fresh visual stability. if motion.phase != "prepare" or self._settled_pose is None: return False command, feedback, stamp = self._settled_pose @@ -811,37 +1076,44 @@ class CalibrationCoordinator: record = {"kind": "preparation_reused_settled_pose", "task_name": motion.task_key, "cycle": motion.cycle, "direction": motion.direction, "stamp_ns": self.ports.clock_ns()} append_jsonl_many(self.raw_path, (record,), durable=False) - self.raw_records.append(record) + self.capture_index.append(record) return True def _prepare_command_sampling(self, task_key): if task_key in self.execution.sampling_plans: return from ..core.fitting.command_sampling import build_sampling_plan - existing = [row for row in self.raw_records if row.get("kind") == "command_sampling_plan" + existing = [row for row in self.capture_index if row.get("kind") == "command_sampling_plan" and row.get("task_name") == task_key] if existing: plan = existing[0] # PreparedResume already verified its source and content. else: task = next(task for task in self.profile.motion.tasks if task.key == task_key) - plan = build_sampling_plan(self.profile, task, self.raw_records) + plan = build_sampling_plan(self.profile, task, self.capture_index) append_jsonl(self.raw_path, plan) - self.raw_records.append(plan) + self.capture_index.append(plan) self.execution.sampling_plans[task_key] = plan def _recover_joint_zero(self, motion, now, geometry_error): if self.diagnostic_capture is not None: return False action = self.zero_recovery.take(motion, geometry_error=geometry_error, - reports=self.branch_initialization.reports, referenced_joints=self.execution.zero_references) + reports=self.branch_initialization.reports, + referenced_joints=() if motion.reference_only else self.execution.zero_references) if action is None: return False + from .zero_recovery import failure_category record = {"kind": "joint_zero_recovery", "task_name": motion.task_key, + "failure_category": failure_category(geometry_error or self._zero_reference_error).value, + "capture_plan_sha256": CapturePlan.from_profile(self.execution.profile).sha256, "joints": list(motion.zero_joints), "action": action, "reason": geometry_error or self._zero_reference_error, + "previous_session_epoch": self._observation_epoch, "session_epoch": self._observation_epoch+1, "attempt": 1, "stamp_ns": self.ports.clock_ns()} append_jsonl(self.raw_path, record) - self.raw_records.append(record) + self.capture_index.append(record) self._observation_epoch += 1 + self.branch_initialization.parent_references.advance_recovery_epoch( + self._observation_epoch-1, self._observation_epoch) self._steady_rows.clear() self._zero_capture_started_at = None self._steady_capture_after_ns = None @@ -859,6 +1131,8 @@ class CalibrationCoordinator: def _joint_zero_branch_details(self, motion): task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) + if motion.reference_only: + task = replace(task, joints=motion.zero_joints) return self._task_branch_details(task, joints=motion.zero_joints) def _task_branch_details(self, task, *, joints=None): @@ -887,10 +1161,20 @@ class CalibrationCoordinator: def _begin_motion_capture(self, motion): """Aggregate one direction across its moving and stationary segments.""" self._steady_rows = [] + self._zero_reference_error = "" + self.capture.begin_motion(motion) + if motion.reference_reuse: + task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) + try: + self.branch_initialization.parent_references.activate(task, self._observation_epoch, self.capture) + self._zero_geometry_completed_at = self.ports.monotonic() + except ValueError as error: + self._pause(str(error)) + return self.branch_initialization.begin(motion, session_epoch=self._observation_epoch, motion_version=self._segment_number) if motion.recording: - key = (motion.task_key, motion.cycle, motion.direction, motion.attempt) + key = (motion.task_key, motion.cycle, motion.direction, motion.attempt, motion.segment_key) if key != self._capture_unit_key: self._unit_rows = [] self._capture_unit_key = key @@ -926,8 +1210,16 @@ class CalibrationCoordinator: waiting = [] if not health.connected: waiting.append("SDK 未连接") - if len(self.latest_feedback) != self.command_count or not health.feedback_fresh: + if not self.profile.vision_motion and (len(self.latest_feedback) != self.command_count or not health.feedback_fresh): waiting.append("等待完整、新鲜的 SDK 反馈") + if self.profile.vision_motion and self.parameters.commands_enabled and len(self.parameters.initial_command) != self.command_count: + waiting.append("缺少启动前读取的 SDK 目标指令，不能用位置反馈替代") + if self.profile.vision_motion and self.parameters.initial_device_uid: + try: + if json.loads(health.diagnostic).get("uid") != self.parameters.initial_device_uid: + waiting.append("SDK 设备身份与启动目标读数不一致") + except (ValueError, TypeError): + waiting.append("等待设备身份确认") if not health.position_mode: waiting.append("SDK 尚未确认位置控制模式") if health.active_faults: @@ -951,6 +1243,11 @@ class CalibrationCoordinator: waiting.append(f"{view} 未收到新鲜的 Tag 检测消息（检查图像、整流和检测节点）") return DeviceReadiness(not waiting, tuple(waiting), cameras) + def _flush_visual_evidence(self): + rows = self.visual_motion.drain() + self.capture_index.extend(rows) + append_jsonl_many(self.raw_path, rows, durable=False) + def _accept_feedback(self, names, positions, stamp: int, *, channel_stamps_ns=()) -> StateSample | None: state = self.sdk_adapter.parse_feedback(names, positions) if state is None: @@ -981,7 +1278,7 @@ class CalibrationCoordinator: if value < self.feedback_lower[index] or value > self.feedback_upper[index] ), None) - if violation is not None: + if violation is not None and not self.profile.vision_motion: index, value = violation # Preserve the offending observation for the operator diagnostic, # while the pause path continues to hold the last safe command. @@ -1007,7 +1304,7 @@ class CalibrationCoordinator: record = {"kind": "sdk_feedback_sample", "stamp_ns": stamp, "received_stamp_ns": self.ports.clock_ns(), "source": "can_feedback_v1", "state_values": list(state), "channel_stamps_ns": list(channel_stamps_ns)} - self.raw_records.append(record) + self.capture_index.append(record) append_jsonl_many(self.raw_path, (record,), durable=False) return self.state_history[-1] @@ -1058,6 +1355,9 @@ class CalibrationCoordinator: if self.state in {"PASSED", "DIAGNOSTIC_COMPLETE", "ABORTED", "FAILED"} or self._pause_effects_applied: return self.finalization.cancel() + if self.training_worker is not None: + self.training_worker.close() + self._training_request = None self._observation_epoch += 1 self.branch_initialization.clear() self._pause_effects_applied = True @@ -1089,6 +1389,7 @@ class CalibrationCoordinator: "kind": "paused", "reason": self.reason, "command_unit": self.command_unit, + f"last_command_{self.command_unit}": list(self.last_command) if self.last_command is not None else [], f"latest_state_{self.command_unit}": list(self.latest_feedback), f"target_state_{self.command_unit}": target, f"channel_errors_{self.command_unit}": errors, @@ -1128,24 +1429,38 @@ class CalibrationCoordinator: json.dumps({v: self.cameras.matrices[v].tolist() for v in sorted(self.required_observation_views)}, sort_keys=True).encode()).hexdigest()}, "fixed_corners_by_view": self.reference_lock.fingerprint(), "fixed_poses": fixed, - "moving_tag_poses": moving} + "moving_tag_poses": moving, + "fixed_reference_mode": self.profile.acquisition.fixed_reference_mode, + "stationary_image_references": self.capture.image_reference_payloads()} append_jsonl(self.raw_path, {"kind": "fixed_base_reference_locked", **self._current_fingerprint}) + def _write_resume_batch(self, pending): + """Keep both zero-evidence and scan imports outside an unbounded tick.""" + rows = pending.take_batch() + append_jsonl_many(self.raw_path, rows, durable=False) + self.capture_index.extend(rows) + if pending.complete: + sync_jsonl(self.raw_path) + def _import_resume_batch(self): pending = self._resume_import - rows = pending.take_batch() - append_jsonl_many(self.raw_path, rows, durable=False) - self.raw_records.extend(rows) + self._write_resume_batch(pending) if pending.complete: # No scan is skipped until every imported row is durable. Between # batches the ordinary feedback, abort and safety callbacks run. - sync_jsonl(self.raw_path) self.execution.first_cycle_spans.update(self.joint_resume.spans_for_task(pending.task_key)) + cycles = self.joint_resume.profile.quality.task_training_cycles.get(pending.task_key) + if cycles is not None: + self.execution.session.select_training(pending.task_key, cycles) + self.execution.profile = self.execution.session.profile self.execution.authorize_task_reuse(pending.units) self._resumed_count += len(pending.units) self._resume_import = None def _verify_resume(self): + if self.parameters.resume_mode == "passed": + self._restore_passed_tasks() + return decision = ResumeDecision(False, "基准变化，已放弃旧断点并重新采集") rows = [] try: @@ -1181,15 +1496,30 @@ class CalibrationCoordinator: decision = self.execution.restore(decision, rows) if decision.reuse: used = set(decision.completed_units) - imported = [r for r in rows if (r.get("task_name"), r.get("cycle"), r.get("direction")) in used] + imported = [r for r in rows if record_scan_identity(r) in used] append_jsonl_many(self.raw_path, imported) - self.raw_records.extend(imported) + self.capture_index.extend(imported) self._resumed_count = len(used) self._resume_message = decision.reason append_jsonl(self.raw_path, {"kind": "resume_verification", "reuse": decision.reuse, "reason": decision.reason, "incompatible_fields": list(decision.incompatible_fields), "completed_units": list(decision.completed_units)}) + def _restore_passed_tasks(self): + checkpoint = self._resume_checkpoint + if checkpoint is None: + self._pause("passed_resume_unavailable:" + self._resume_checkpoint_error) + return + if checkpoint.fixed_reference.get("protected_hashes") != self._current_fingerprint.get("protected_hashes"): + self._pause("passed_resume_protected_inputs_changed") + return + self.joint_resume = checkpoint.joints + self._passed_reference_import = checkpoint.passed_import + # The existing bounded importer skips each task only after its original + # rows and zero references are durable. No old task is moved or scanned. + self._resume_message = "按已通过记录续采，跳过历史回放和已完成关节零位复核" + self.execution.restore(ResumeDecision(True, self._resume_message), ()) + def _view(self, name: str): return next(view for view in self.profile.vision.views if view.name == name) @@ -1215,7 +1545,8 @@ class CalibrationCoordinator: self.finalization.start(FinalizationInputs( self.parameters.session_dir, self.parameters.serial_number, self.parameters.source_urdf, self._current_fingerprint.get("protected_hashes", self.parameters.protected_inputs), - tuple(self.raw_records) + tuple(self.cameras.models.values()), require_motion_evidence=True, + (tuple(self.capture_index) + tuple(self.cameras.models.values()) + if self.capture_index.retain_complete else ()), require_motion_evidence=True, journal_path=self.raw_path, journal_size=self.raw_path.stat().st_size, camera_extrinsics_file=self.parameters.camera_extrinsics_file, )) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/detection_filter.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/detection_filter.py new file mode 100644 index 0000000..b3471e9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/detection_filter.py @@ -0,0 +1,26 @@ +"""Shared detector-quality filter for fast motion witnesses and pose fitting.""" + +import numpy as np + + +def filter_detections(view_spec, policy, observation, width, height, motion=None): + view, stamp = observation.view, observation.stamp_ns + roles = {tag.tag_id: tag.role for tag in view_spec.tags} + corners, quality_events = {}, [] + observed_roles = frozenset(roles[item.tag_id] for item in observation.detections if item.tag_id in roles) + for detection in observation.detections: + points = np.asarray(detection.corners, dtype=float) + if detection.border_quality is not None and not detection.border_quality["passed"]: + quality_events.append({"kind": "detection_quality_rejected", "view": view, + "image_stamp_ns": stamp, "tag_id": detection.tag_id, + "corners_xy": points.tolist(), "quality": dict(detection.border_quality), + "task_name": None if motion is None else motion.task_key, + "motion_phase": None if motion is None else motion.phase}) + continue + if (detection.tag_id not in roles or points.shape != (4, 2) or not np.all(np.isfinite(points)) + or detection.hamming > policy.maximum_hamming or detection.decision_margin < policy.minimum_decision_margin + or np.min(np.linalg.norm(points-np.roll(points, -1, axis=0), axis=1)) < policy.minimum_edge_pixels + or np.min(points) < 2 or np.max(points[:, 0]) > width-3 or np.max(points[:, 1]) > height-3): + continue + corners[roles[detection.tag_id]] = points + return corners, quality_events, observed_roles diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py index 87cbbd3..dc805ea 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_analysis.py @@ -46,6 +46,7 @@ def _pose(payload): def image_frames(records, relations): """Convert detached, same-image evidence; never substitute a later pose.""" from ..core.geometry.tag_pose.motion_image_diagnostics import ImageMotionFrame, ImageRoleObservation + from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence roots = {relation.parent_role for relation in relations} - {relation.child_role for relation in relations} roles = tuple(dict.fromkeys(role for relation in relations @@ -71,8 +72,11 @@ def image_frames(records, relations): observations.append(ImageRoleObservation(role, float(item["tag_size_m"]), tuple(tuple(point) for point in item["corners_xy"]))) else: + references = tuple(reference_from_evidence(row["tags"][role], stamp_ns=stamp, + camera_matrix=row["camera_matrix"]) for role in roles + if row["tags"][role].get("pose_source") == IMAGE_REFERENCE_SOURCE) frames.append(ImageMotionFrame(MotionEvidenceFrame(stamp, tuple(candidates)), - tuple(tuple(values) for values in row["camera_matrix"]), tuple(observations))) + tuple(tuple(values) for values in row["camera_matrix"]), tuple(observations), references)) return tuple(frames) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py index eb4faa7..f95bc77 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/diagnostic_reference.py @@ -69,7 +69,8 @@ def build_diagnostic_zero_attempt(profile, motion, records, *, reason, stamp = row.get("image_stamp_ns") command, feedback = row.get("command_vector"), row.get("feedback_vector") if (not isinstance(stamp, int) or isinstance(stamp, bool) or stamp <= 0 - or not _complete_vector(command, count) or not _complete_vector(feedback, count) + or not _complete_vector(command, count) + or (not profile.vision_motion and not _complete_vector(feedback, count)) or any(abs(a-b) > 1e-9 for a, b in zip(command, motion.target))): continue for name, spec in streams: @@ -97,4 +98,4 @@ def build_diagnostic_zero_attempt(profile, motion, records, *, reason, latest = max(selected.values(), key=lambda row: row["image_stamp_ns"]) return DiagnosticZeroAttempt(tuple(motion.zero_joints), motion.task_key, reason, session_epoch, motion_version, tuple(latest["command_vector"]), - tuple(latest["feedback_vector"]), tuple(sorted(selected)), tuple(sorted(frames))) + tuple(latest.get("feedback_vector") or ()), tuple(sorted(selected)), tuple(sorted(frames))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py index 53bc4ab..6506998 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py @@ -14,13 +14,43 @@ from .diagnostic_capture import DiagnosticCapturePlan from ..core.domain.profile import ACQUISITION_POLICY_VERSION -TRAINING_CYCLES = (0, 1, 2) -HOLDOUT_CYCLE = 3 -CAPTURE_SCHEDULE_VERSION = "unified_schedule_v2_single_pass" -SEPARATE_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v3_separate_mapping" +from ..core.domain.motion_path import task_segments, scan_identity +from ..core.domain.capture_plan import CapturePlan, ADAPTIVE, LEGACY_TRAINING, LEGACY_HOLDOUT +TRAINING_CYCLES = LEGACY_TRAINING +HOLDOUT_CYCLE = LEGACY_HOLDOUT +CAPTURE_SCHEDULE_VERSION = "unified_schedule_v7_single_pass_endpoint_images" +SEPARATE_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v5_separate_mapping_feedback_motion" +SEGMENTED_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v7_segmented_endpoint_images" +PATH_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v8_approach_path" +VISUAL_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v9_visual_motion" +PARENT_REFERENCE_SCHEDULE_VERSION = "unified_schedule_v10_verified_parent_geometry" +IMAGE_REFERENCE_SCHEDULE_VERSION = "unified_schedule_v11_stationary_image_reference" +ADAPTIVE_CAPTURE_SCHEDULE_VERSION = "unified_schedule_v12_adaptive_training" +SUPPORTED_CAPTURE_SCHEDULE_VERSIONS = frozenset((CAPTURE_SCHEDULE_VERSION, + ADAPTIVE_CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION, *(f"{SEGMENTED_CAPTURE_SCHEDULE_VERSION}_{mode}" + for mode in ("interleaved", "separate")), *(f"{PATH_CAPTURE_SCHEDULE_VERSION}_{mode}" + for mode in ("interleaved", "separate")), *(f"{VISUAL_CAPTURE_SCHEDULE_VERSION}_{mode}" + for mode in ("interleaved", "separate")), *(f"{PARENT_REFERENCE_SCHEDULE_VERSION}_{feedback}_{mode}" + for feedback in ("feedback", "visual") for mode in ("interleaved", "separate")), + *(f"{IMAGE_REFERENCE_SCHEDULE_VERSION}_{feedback}_{mode}" + for feedback in ("feedback", "visual") for mode in ("interleaved", "separate")))) def capture_schedule_version(profile): + if profile.quality.training_policy == ADAPTIVE: + return ADAPTIVE_CAPTURE_SCHEDULE_VERSION + if profile.acquisition.fixed_reference_mode == "stationary_image": + feedback = "visual" if profile.vision_motion else "feedback" + return f"{IMAGE_REFERENCE_SCHEDULE_VERSION}_{feedback}_{profile.acquisition.command_capture_mode}" + if any(task.parent_reference is not None for task in profile.motion.tasks): + feedback = "visual" if profile.vision_motion else "feedback" + return f"{PARENT_REFERENCE_SCHEDULE_VERSION}_{feedback}_{profile.acquisition.command_capture_mode}" + if profile.vision_motion: + return f"{VISUAL_CAPTURE_SCHEDULE_VERSION}_{profile.acquisition.command_capture_mode}" + if any(spec.evidence_scope == "path" for spec in profile.motion.joint_zero_references.values()): + return f"{PATH_CAPTURE_SCHEDULE_VERSION}_{profile.acquisition.command_capture_mode}" + if any(task.segments for task in profile.motion.tasks): + return f"{SEGMENTED_CAPTURE_SCHEDULE_VERSION}_{profile.acquisition.command_capture_mode}" return (SEPARATE_CAPTURE_SCHEDULE_VERSION if profile.acquisition.command_capture_mode == "separate" else CAPTURE_SCHEDULE_VERSION) @@ -34,6 +64,11 @@ class ScanUnit: end: float speed: float sample_phase: str = "sweep" + segment_key: str = "" + + @property + def identity(self): + return scan_identity(self.task_key, self.cycle, self.direction, self.segment_key) @dataclass(frozen=True) @@ -148,31 +183,20 @@ class CalibrationEngine: def scan_units(self) -> tuple[ScanUnit, ...]: units: list[ScanUnit] = [] plan = self.diagnostic_capture - cycles = plan.cycles if plan is not None else (*TRAINING_CYCLES, HOLDOUT_CYCLE) + capture_plan = CapturePlan.from_profile(self.profile) for task in self.profile.motion.tasks: if plan is not None and task.key not in plan.task_keys: continue speed = self._formal_speed(task) - forward = "increasing" if task.end_value > task.start_value else "decreasing" - reverse = "decreasing" if forward == "increasing" else "increasing" + cycles = plan.cycles if plan is not None else capture_plan.task(task.key).motion_cycles for cycle in cycles: - units.extend(( - ScanUnit( - task.key, cycle, forward, task.start_value, - task.end_value, speed, - ), - ScanUnit( - task.key, cycle, reverse, task.end_value, - task.start_value, speed, - ), - )) + units.extend(ScanUnit(task.key, cycle, segment.direction, segment.start, + segment.end, speed, segment_key=segment.key) for segment in task_segments(task)) if plan is None and self.profile.acquisition.command_capture_mode == "separate": from ..core.domain.sampling import command_training_cycles, command_holdout_cycle - for cycle in (*command_training_cycles(self.profile), command_holdout_cycle(self.profile)): - units.extend(( - ScanUnit(task.key, cycle, forward, task.start_value, task.end_value, speed, "steady"), - ScanUnit(task.key, cycle, reverse, task.end_value, task.start_value, speed, "steady"), - )) + for cycle in (*command_training_cycles(self.profile, task), command_holdout_cycle(self.profile)): + units.extend(ScanUnit(task.key, cycle, segment.direction, segment.start, + segment.end, speed, "steady", segment.key) for segment in task_segments(task)) return tuple(units) def mapping_probe_delta(self, task: TaskSpec) -> float | None: @@ -195,7 +219,8 @@ class CalibrationEngine: @staticmethod def permits_retry(stage: str, completed_retries: int) -> bool: """Only an under-sampled scan direction gets one same-speed retry.""" - return str(stage) == "sweep_acquisition" and int(completed_retries) == 0 + from .zero_recovery import permits_recovery + return str(stage) == "sweep_acquisition" and permits_recovery((stage,), int(completed_retries)) def evaluate_sweep( self, @@ -207,6 +232,7 @@ class CalibrationEngine: feedback_hz: float, detection_rate: float, bin_count: int = 256, + observed_command_progress_01: Sequence[float] | None = None, ) -> SweepQuality: """Judge fitting observability; ideal rates are diagnostics only.""" policy = self.profile.acquisition @@ -224,8 +250,19 @@ class CalibrationEngine: include_endpoint_gaps=False, ) if policy.require_endpoint_observations: - values = np.asarray(feedback_progress_01, dtype=float) + # Endpoints belong to the requested trajectory. An SDK whose + # command and encoder domains differ must prove those commands + # with synchronized images, without relabelling encoder values. + independent_feedback = self.profile.vision_motion or not policy.feedback_travel_matches_command + endpoint_values = (observed_command_progress_01 if independent_feedback + else feedback_progress_01) + values = np.asarray(() if endpoint_values is None else endpoint_values, dtype=float) values = values[np.isfinite(values) & (values >= 0) & (values <= 1)] + metrics = {**quality.metrics, + "endpoint_observation_domain": "command" if independent_feedback else "feedback", + "endpoint_observed_minimum_01": float(np.min(values)) if values.size else None, + "endpoint_observed_maximum_01": float(np.max(values)) if values.size else None} + quality = SweepQuality(quality.passed, quality.failures, quality.warnings, metrics) if not values.size or float(np.min(values)) > policy.endpoint_tolerance_01 or float(np.max(values)) < 1.0 - policy.endpoint_tolerance_01: return SweepQuality(False, (*quality.failures, "endpoint_observation"), quality.warnings, quality.metrics) return quality @@ -237,13 +274,19 @@ class CalibrationEngine: can still be evaluated on their own, but cannot be mixed into a new directional-waypoint session through resume. """ + from ..core.urdf.partial_scope import profile_scope return ( self.diagnostic_capture is None + and session_start.get("calibration_scope") == profile_scope(self.profile) and not session_start.get("diagnostic_capture") and session_start.get("acquisition_policy_version") == ACQUISITION_POLICY_VERSION and session_start.get("capture_schedule_version") in ( - (capture_schedule_version(self.profile),) if self.profile.acquisition.command_capture_mode == "separate" + (capture_schedule_version(self.profile),) if (self.profile.quality.training_policy == ADAPTIVE or self.profile.acquisition.fixed_reference_mode == "stationary_image" + or self.profile.vision_motion or any(task.segments for task in self.profile.motion.tasks) + or any(task.parent_reference is not None for task in self.profile.motion.tasks) + or any(spec.evidence_scope == "path" for spec in self.profile.motion.joint_zero_references.values()) + or self.profile.acquisition.command_capture_mode == "separate") else (None, CAPTURE_SCHEDULE_VERSION)) and session_start.get("pose_tracking_policy_version") == POSE_TRACKING_POLICY_VERSION diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py index 25ac0cc..84d6f73 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py @@ -8,12 +8,14 @@ from __future__ import annotations from dataclasses import replace import math +from statistics import median from .diagnostic_capture import DiagnosticCapturePlan from .diagnostic_reference import DiagnosticZeroAttempt from .session import CalibrationPhase as Phase, CalibrationSession from .motion_execution import MotionCommand from .scan_quality import evaluate_capture_unit +from ..core.domain.motion_path import task_segment, record_scan_identity from .trajectory import (build_calibration_preparation_waypoints, build_calibration_return_waypoints, build_calibration_motion_command, build_joint_reference_waypoints) @@ -26,6 +28,9 @@ class SessionExecution: self.first_cycle_spans = {} self.completed_units = set() self.last_quality = None + self.last_task_quality = None + self.last_training_decision = None + self._training_checks = set() self.last_diagnostic_quality = None self._effects = [] self._action_key = None @@ -52,6 +57,27 @@ class SessionExecution: params = self.profile.motion.speed_parameters baseline_speed = float(params.get("baseline_rad_s", 0.1) if self.profile.command.unit == "rad" else params.get("baseline_u8", params.get("preflight_u8", 1))) + exits = () + if action.phase in {Phase.PREPARE, Phase.RETURN_BASELINE}: + next_task = None if action.scan_unit is None else action.scan_unit.task_key + if self._entered_task is not None and self._entered_task != next_task: + previous = next(t for t in self.profile.motion.tasks if t.key == self._entered_task) + pose, effects = list(current), [] + for waypoint in previous.exit_waypoints: + for index, value in waypoint: + pose[index] = value + zero_returns = tuple(reference for reference in self.zero_references.values() + if reference.task_key == previous.key and reference.samples + and all(sample.command == tuple(pose) for sample in reference.samples)) + feedback_targets = () if self.profile.vision_motion else tuple((reference.channel, + float(median(sample.feedback[reference.channel] for sample in reference.samples))) + for reference in zero_returns) + effects.append(MotionCommand("task_exit", tuple(pose), baseline_speed, + task_key=previous.key, feedback_targets=feedback_targets, + visual_return_joints=tuple(r.joint for r in zero_returns) if self.profile.vision_motion else ())) + exits = tuple(effects) + current = tuple(pose) + self._entered_task = None if action.phase == Phase.REFERENCE_POSES: waypoints = self.profile.motion.resume_verification_waypoints result = [MotionCommand("reference_pose", waypoint.command, baseline_speed, task_key=waypoint.key) @@ -60,14 +86,18 @@ class SessionExecution: for target in build_calibration_return_waypoints(profile=self.profile, current_command=waypoints[-1].command)) return tuple(result) if action.phase in {Phase.BASELINE, Phase.RETURN_BASELINE}: - return tuple(MotionCommand("baseline" if action.phase == Phase.BASELINE else "return", target, baseline_speed) + return exits + tuple(MotionCommand("baseline" if action.phase == Phase.BASELINE else "return", target, baseline_speed) for target in build_calibration_return_waypoints(profile=self.profile, current_command=current)) unit = action.scan_unit if unit is None: return () task = next(t for t in self.profile.motion.tasks if t.key == unit.task_key) - common = dict(task_key=task.key, command_index=task.command_index, + segment = task_segment(task, unit.segment_key) if unit.segment_key else None + common = dict(task_key=task.key, command_index=segment.command_index if segment else task.command_index, cycle=unit.cycle, attempt=action.retry+1) + if segment: + common.update(segment_key=segment.key, observed_joints=segment.joints, + measured_channels=segment.moving_channels) if action.phase == Phase.PREPARE: entry = [] if self._entered_task != task.key: @@ -82,25 +112,49 @@ class SessionExecution: pending = {name: spec for name, spec in self.profile.motion.joint_zero_references.items() if spec.task_key == task.key and name not in self.zero_references and not (self.diagnostic_capture is not None and name in self.diagnostic_zero_attempts)} + if entry or not any(name in self.zero_references for name in task.joints): + if task.parent_reference is not None: + name = task.parent_reference.pose_joint + target = self.profile.motion.joint_zero_references[task.joints[0]].command + entry.extend(MotionCommand("clearance", point, unit.speed, **common) + for point in build_joint_reference_waypoints(task, target, profile=self.profile, + current_command=current)) + entry.append(MotionCommand("joint_zero", target, unit.speed, + zero_joints=(name,), reference_only=True, reference_reuse=True, **common)) + current = target + for preparation in task.model_preparations: + metadata = dict(common, reference_only=True, + reference_command=preparation.command, + reference_approach=preparation.approach_commands) + for pose in (*preparation.approach_commands, preparation.command): + entry.extend(MotionCommand("zero_approach", point, unit.speed, + reference_joints=(preparation.joint,), **metadata) + for point in build_joint_reference_waypoints(task, pose, profile=self.profile, + current_command=current)) + current = pose + entry.append(MotionCommand("joint_zero", preparation.command, unit.speed, + zero_joints=(preparation.joint,), **metadata)) groups = {} for name, spec in pending.items(): - groups.setdefault((spec.command, spec.approach_commands), []).append(name) - for (target, approach), names in groups.items(): - for pose in (*approach, target): + groups.setdefault((spec.command, spec.approach_commands, spec.evidence_scope, + name if task.segments else ""), []).append(name) + for (target, approach, scope, _), names in groups.items(): + for path_index, pose in enumerate((*approach, target)): entry.extend(MotionCommand("zero_approach", point, unit.speed, - reference_joints=tuple(sorted(names)), **common) + reference_joints=tuple(sorted(names)), + reference_path_index=path_index if scope == "path" else None, **common) for point in build_joint_reference_waypoints(task, pose, profile=self.profile, current_command=current)) current = pose entry.append(MotionCommand("joint_zero", target, unit.speed, zero_joints=tuple(sorted(names)), **common)) current = target - return tuple(entry) + tuple(MotionCommand("prepare", target, unit.speed, **common) + return exits + tuple(entry) + tuple(MotionCommand("prepare", target, unit.speed, **common) for target in build_calibration_preparation_waypoints(task, profile=self.profile, - current_command=current, start_value=unit.start)) + current_command=current, start_value=unit.start, segment_key=unit.segment_key)) if action.phase == Phase.MAPPING_PROBE: delta = self.session.engine.mapping_probe_delta(task) - start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile)) + start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile, segment_key=unit.segment_key)) target = list(start) # Probe toward this task's other endpoint. The complete two-leg # motion is <=3 degrees from the already prepared pose. @@ -112,7 +166,7 @@ class SessionExecution: if self.diagnostic_capture is None and self.profile.acquisition.command_capture_mode == "separate": if unit.sample_phase == "sweep": return (MotionCommand("sweep", tuple(build_calibration_motion_command(task, unit.end, - profile=self.profile)), unit.speed, direction=unit.direction, **common),) + profile=self.profile, segment_key=unit.segment_key)), unit.speed, direction=unit.direction, **common),) from .steady import steady_targets plan = self.sampling_plans.get(task.key) nodes = tuple(plan["training_nodes"]) if plan is not None else None @@ -121,22 +175,22 @@ class SessionExecution: # One move-and-hold effect per mapping point: arrival and the # stable image window share the same feedback history. return tuple(MotionCommand("steady", tuple(build_calibration_motion_command(task, value, - profile=self.profile)), unit.speed, direction=unit.direction, steady_index=index, **common, **metadata) + profile=self.profile, segment_key=unit.segment_key)), unit.speed, direction=unit.direction, steady_index=index, **common, **metadata) for index, value in enumerate(steady_targets(self.profile, unit, training_nodes=nodes))) if self.diagnostic_capture is not None: - target = tuple(build_calibration_motion_command(task, unit.end, profile=self.profile)) + target = tuple(build_calibration_motion_command(task, unit.end, profile=self.profile, segment_key=unit.segment_key)) sweep = MotionCommand("sweep", target, unit.speed, direction=unit.direction, **common) if not self.diagnostic_capture.hold_command_values: return (sweep,) # Compare the same declared commands after arrival from both # endpoints. Keep the ordinary safety/trajectory driver and # retain the complete hold for time-dependent diagnostics. - start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile)) + start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile, segment_key=unit.segment_key)) values = sorted(self.diagnostic_capture.hold_command_values, reverse=unit.direction == "decreasing") return (sweep, MotionCommand("steady_prepare", start, unit.speed, **common), *(MotionCommand("steady", - tuple(build_calibration_motion_command(task, value, profile=self.profile)), + tuple(build_calibration_motion_command(task, value, profile=self.profile, segment_key=unit.segment_key)), unit.speed, direction=unit.direction, steady_index=index, minimum_hold_seconds=self.diagnostic_capture.hold_seconds, **common) for index, value in enumerate(values))) @@ -145,11 +199,14 @@ class SessionExecution: # Traverse the direction once. Separate motion effects preserve # the original sweep/steady image identities and capture gates; # a held frame is never copied into the moving sample set. - for index, value in enumerate(steady_targets(self.profile, unit)): - point = tuple(build_calibration_motion_command(task, value, profile=self.profile)) - if index: - effects.append(MotionCommand("sweep", point, unit.speed, - direction=unit.direction, **common)) + targets = steady_targets(self.profile, unit) + for index, value in enumerate(targets): + point = tuple(build_calibration_motion_command(task, value, profile=self.profile, segment_key=unit.segment_key)) + # Both direction endpoints need their own motion-domain + # images, including a zero-distance hold before departure. + # Static mapping images remain a separate observation set. + effects.append(MotionCommand("sweep", point, unit.speed, + direction=unit.direction, defer_settling_to_hold=0 < index < len(targets)-1, **common)) effects.append(MotionCommand("steady", point, unit.speed, direction=unit.direction, steady_index=index, **common)) return tuple(effects) @@ -158,7 +215,7 @@ class SessionExecution: def motion_complete(self): if not self._effects: raise RuntimeError("cannot complete a motion that was not requested") - if any(name not in self.zero_references and not ( + if not self._effects[0].reference_only and any(name not in self.zero_references and not ( self.diagnostic_capture is not None and name in self.diagnostic_zero_attempts) for name in self._effects[0].zero_joints): raise RuntimeError("joint zero acquisition cannot complete without frozen evidence") @@ -185,16 +242,19 @@ class SessionExecution: """Replace only the current unreferenced preparation, preserving the scan.""" motion = self._effects[0] if self._effects else None if (self.session.phase != Phase.PREPARE or motion is None or motion.phase != "joint_zero" - or set(motion.zero_joints) & self.zero_references.keys()): + or (not motion.reference_only and set(motion.zero_joints) & self.zero_references.keys())): raise ValueError("zero_recovery_outside_unreferenced_preparation") task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) - specs = [self.profile.motion.joint_zero_references[name] for name in motion.zero_joints] + from ..core.domain.profile import JointZeroSpec + specs = ([JointZeroSpec(task.key, motion.reference_command, motion.reference_approach)] + if motion.reference_only else [self.profile.motion.joint_zero_references[name] for name in motion.zero_joints]) if any(spec != specs[0] for spec in specs): raise ValueError("zero_recovery_incompatible_group") effects = [] - for target in (*specs[0].approach_commands, specs[0].command): + for path_index, target in enumerate((*specs[0].approach_commands, specs[0].command)): effects.extend(replace(motion, phase="zero_approach", target=point, - zero_joints=(), reference_joints=motion.zero_joints) + zero_joints=(), reference_joints=motion.zero_joints, + reference_path_index=path_index if specs[0].evidence_scope == "path" else None) for point in build_joint_reference_waypoints(task, target, profile=self.profile, current_command=current)) current = target @@ -211,10 +271,23 @@ class SessionExecution: raise ValueError("diagnostic_joint_zero_attempt_replacement_forbidden") self.diagnostic_zero_attempts.update({name: attempt for name in attempt.joint_names}) - def evaluate(self, records): + def training_check_due(self): + from ..core.domain.capture_plan import ADAPTIVE + unit = self.action.scan_unit + if (self.profile.quality.training_policy != ADAPTIVE or self.diagnostic_capture is not None + or unit is None or unit.cycle not in (1, 2) + or (unit.task_key, unit.cycle) in self._training_checks): + return False + round_units = [u for u in self.session.engine.scan_units() + if u.task_key == unit.task_key and u.cycle == unit.cycle] + return unit.identity == round_units[-1].identity + + def evaluate(self, records, *, training_decision=None): if self.session.phase != Phase.EVALUATE: raise RuntimeError("sweep evaluation outside EVALUATE") action = self.action + self.last_task_quality = None + self.last_training_decision = None if self.diagnostic_capture is None: quality = evaluate_capture_unit(self.profile, action.scan_unit, action.retry+1, records, first_cycle_spans=self.first_cycle_spans) @@ -226,7 +299,45 @@ class SessionExecution: self.last_quality = quality if quality.passed: unit = action.scan_unit - self.completed_units.add((unit.task_key, unit.cycle, unit.direction)) + self.completed_units.add(unit.identity) + if self.training_check_due(): + if training_decision is None: + raise ValueError("adaptive_training_requires_assessment_before_advancing") + from ..core.domain.capture_plan import CapturePlan, evidence_digest + if (training_decision.get("task_name") != unit.task_key + or training_decision.get("input_plan_sha256") != CapturePlan.from_profile(self.profile).sha256 + or training_decision.get("assessed_cycles") != list(range(unit.cycle + 1)) + or training_decision.get("decision_sha256") != evidence_digest({k: v + for k, v in training_decision.items() if k != "decision_sha256"})): + raise ValueError("training_assessment_does_not_match_current_round") + self.last_training_decision = training_decision + self._training_checks.add((unit.task_key, unit.cycle)) + decision = training_decision["decision"] + if decision == "freeze": + self.session.select_training(unit.task_key, training_decision["assessed_cycles"]) + self.profile = self.session.profile + elif decision == "fail": + self.session.fail("training_quality_failed", ",".join(training_decision["failures"])) + return quality + elif decision != "add_training" or unit.cycle != 1: + raise ValueError("training_assessment_cannot_add_more_rounds") + support_phase = ("steady" if self.profile.command_based_release + and self.profile.acquisition.command_capture_mode == "separate" else "sweep") + task_units = {u.identity for u in self.session.engine.scan_units() + if u.task_key == unit.task_key and u.sample_phase == support_phase} + if (self.diagnostic_capture is None and self.profile.artifacts.output_schema_version >= 3 + and unit.sample_phase == support_phase and task_units <= self.completed_units): + from .task_quality import evaluate_task_input_support + task = next(task for task in self.profile.motion.tasks if task.key == unit.task_key) + self.last_task_quality = evaluate_task_input_support(self.profile, task, records) + if not self.last_task_quality.passed: + self.completed_units.difference_update(task_units) + domain = "稳态指令" if self.profile.command_based_release else "反馈" + self.session.pause("task_input_support_failed", + f"独立验证的{domain}超出训练实测曲线范围，需要完整重采当前任务", + {"failures": self.last_task_quality.failures, "metrics": dict(self.last_task_quality.metrics)}, + suggestion="检查端点重复性；不能裁剪输入、外推或只重采验证轮") + return quality self.session.evaluation_complete(quality) return quality @@ -238,11 +349,11 @@ class SessionExecution: completed = set(decision.completed_units) if decision.reuse else set() valid = [] for unit in self.session.engine.scan_units(): - key = (unit.task_key, unit.cycle, unit.direction) - if key not in completed: + key = unit.identity + if key not in completed or next(t for t in self.profile.motion.tasks if t.key == unit.task_key).segments: continue attempts = [int(r.get("attempt", 1)) for r in records - if (r.get("task_name"), r.get("cycle"), r.get("direction")) == key] + if record_scan_identity(r) == key] quality = evaluate_capture_unit(self.profile, unit, max(attempts, default=1), records, first_cycle_spans=self.first_cycle_spans) if quality.passed: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py index 95416d7..1c46c39 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_capture.py @@ -5,24 +5,32 @@ Additional views observe the existing motion; they never request a movement, declare a joint measured, or change the primary stream's acquisition gates. """ +from ..core.domain.motion_path import segment_metadata + IMAGE_OBSERVATION_POLICY = "all_view_rectified_corners_v1" def image_observation_record(profile, frame, motion): - if motion is None or not motion.recording or motion.phase not in {"sweep", "steady", "joint_zero"}: + if motion is None or not (motion.recording or motion.reference_joints) or motion.phase not in { + "zero_approach", "sweep", "steady", "joint_zero"}: return None tags = next(view.tags for view in profile.vision.views if view.name == frame.view) return { "kind": "image_observation_frame", "policy": IMAGE_OBSERVATION_POLICY, "task_name": motion.task_key, "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + **segment_metadata(motion.segment_key), "sample_phase": motion.phase, "steady_index": motion.steady_index, + **({"reference_joints": list(motion.reference_joints), + "reference_path_index": motion.reference_path_index, "is_accepted_joint_sample": False} + if motion.phase == "zero_approach" else {}), "view": frame.view, "image_stamp_ns": frame.stamp_ns, "command_unit": profile.command.unit, "command_vector": None if frame.command is None else list(frame.command), "feedback_vector": None if frame.feedback is None else list(frame.feedback), "command_direction_by_index": list(frame.command_directions), "state_image_sync_error_ns": None if frame.feedback is None else frame.skew_ns, + "command_image_skew_ns": frame.command_skew_ns, "camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]", "input_is_rectified": True, "tags": {tag.role: { diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/image_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_reference.py new file mode 100644 index 0000000..bd13c1a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_reference.py @@ -0,0 +1,125 @@ +"""Own a stationary image reference independently of physical pose branches.""" + +from collections import deque +from dataclasses import asdict +from threading import RLock + +import cv2 + +from ..core.geometry.tag_pose.image_reference import ( + ImageReferenceSnapshot, ReferenceImage, StationaryImageReference, coordinate_origin, +) +from ..core.geometry.tag_pose.ippe import solve_square_tag_ippe +from ..core.geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS +from ..core.geometry.frozen_evidence import freeze_evidence + + +class ImageReferenceTracker: + """One camera's immutable anchor; keeps both IPPE candidates as diagnostics.""" + + def __init__(self, minimum_frames, *, reference=None): + self.minimum_frames = max(10, int(minimum_frames)) + self._lock = RLock() + self.reset() + if reference is not None: + # Restore the immutable coordinate convention, not historical + # observations or tracker authorization. Only a fresh image can + # create a current snapshot for a resumed model. + self._bind(reference) + + def _bind(self, reference): + self.reference = reference + self._reference_payload = freeze_evidence(asdict(reference)) + self._reference_identity = reference.identity + + def reset(self): + with self._lock: + self.reference = None + self._reference_payload = None + self._reference_identity = None + self._images = deque(maxlen=self.minimum_frames) + self._last_stamp = 0 + self._snapshot = None + self._motion_snapshot = None + self._epoch = getattr(self, "_epoch", 0) + 1 + + def estimate(self, role, points, *, size, matrix, stamp_ns, locking): + points = tuple(tuple(float(value) for value in point) for point in points) + matrix = tuple(tuple(float(value) for value in row) for row in matrix) + with self._lock: + epoch = self._epoch + try: + candidates = tuple(p for p in solve_square_tag_ippe(points, + tag_size_m=size, camera_matrix=matrix) + if p.reprojection_error_px <= DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px) + except (ValueError, cv2.error): + candidates = () + with self._lock: + if epoch != self._epoch or stamp_ns <= self._last_stamp: + return None, "pose_observation_not_new" + self._last_stamp = stamp_ns + if locking and candidates and self.reference is None: + self._images.append(ReferenceImage(stamp_ns, tuple(map(tuple, points)))) + if len(self._images) >= self.minimum_frames: + try: + images = tuple(self._images) + self._bind(StationaryImageReference(role, size, tuple(map(tuple, matrix)), + images, coordinate_origin(images, size, matrix))) + except (ValueError, cv2.error): + pass # Sliding window must first prove stationarity. + pose, reason = None, "image_reference_initializing" + diagnostics = {"observation_stamp_ns": stamp_ns, + "reference_frame_is_tag_pose": False, + "branch_status": "initializing", "branch_revision": 0, + "branch_frozen": False, "motion_managed": False, + "motion_evidence_verified": False, "reprojection_candidate_count": len(candidates)} + if self.reference is not None: + ref = self.reference + diagnostics.update(reference_frame=self._reference_payload, + reference_frame_sha256=self._reference_identity, + branch_revision=1, branch_frozen=True) + try: + if not candidates: + raise ValueError("image_reference_current_projection_invalid") + diagnostics["image_reference_corner_error_px"] = ref.check_image( + role, size, matrix, points, stamp_ns) + pose, reason = ref.pose, "" + diagnostics["branch_status"] = "tracking" + except ValueError as error: + reason = str(error) + diagnostics["branch_status"] = "unavailable" + diagnostics["branch_reason"] = reason + self._snapshot = (role, stamp_ns, candidates, diagnostics) + self._motion_snapshot = None + if pose is not None: + from ..core.geometry.tag_pose.tracking import TrackerImageObservation + observation = TrackerImageObservation(stamp_ns, tuple(map(tuple, points)), + tuple(map(tuple, matrix)), size) + self._motion_snapshot = ImageReferenceSnapshot(self, role, epoch, + stamp_ns, self.reference, observation) + return pose, reason + + def observation_snapshot(self, role, stamp_ns): + with self._lock: + value = self._snapshot + if value is None or value[:2] != (role, stamp_ns): + return None + return value[2], dict(value[3]), 0 + + def motion_snapshot(self, role, stamp_ns): + with self._lock: + snapshot = self._motion_snapshot + if snapshot is None or (snapshot.role, snapshot.stamp_ns) != (role, stamp_ns): + return None + return snapshot + + def snapshot_current(self, snapshot): + with self._lock: + return (snapshot is self._motion_snapshot and snapshot.epoch == self._epoch + and snapshot.reference is self.reference and snapshot.stamp_ns == self._last_stamp) + + def reference_confirmed(self, role, revision): + with self._lock: + snapshot = self._motion_snapshot + return (type(revision) is int and revision == 1 and snapshot is not None + and snapshot.role == role and self.snapshot_current(snapshot)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/image_replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_replay.py new file mode 100644 index 0000000..3010e1f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/image_replay.py @@ -0,0 +1,59 @@ +"""Bounded offline image replay; workers never own ROS nodes or command output.""" + +from collections import deque +from concurrent.futures import ProcessPoolExecutor +from itertools import islice +import multiprocessing +import os + +from ..core.geometry.image_motion_replay import replay_image_models, validate_image_sample + + +def checkpoint_replay_workers(): + """Reserve host capacity for cameras and feedback while preparing a resume.""" + return min(4, max(1, (os.cpu_count() or 1) // 2)) + + +def _validate_image_group(group): + frame, samples = group + replayed = replay_image_models(frame) + for row in samples: + validate_image_sample(row, frame, replayed) + + +def _validate_batch(batch): + for group in batch: + _validate_image_group(group) + + +def _batches(groups, size=32): + iterator = iter(groups) + while batch := tuple(islice(iterator, size)): + yield batch + + +def validate_image_groups(groups, *, workers=1): + """Recompute every frame once and check all associated samples. + + Only checkpoint preparation requests parallelism. Spawn avoids inheriting + ROS threads; small inputs and daemon finalizers use the same serial checks. + Batches share immutable model payloads during transport. No replay result + is persisted or reused across captures. + """ + if workers <= 1 or len(groups) < 256 or multiprocessing.current_process().daemon: + for group in groups: + _validate_image_group(group) + return + executor = ProcessPoolExecutor(max_workers=workers, + mp_context=multiprocessing.get_context("spawn")) + try: + batches = _batches(groups) + pending = deque(executor.submit(_validate_batch, batch) + for batch in islice(batches, 2 * workers)) + while pending: + pending.popleft().result() # Propagate errors in capture order. + batch = next(batches, None) + if batch is not None: + pending.append(executor.submit(_validate_batch, batch)) + finally: + executor.shutdown(wait=True, cancel_futures=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py index a005aae..9144dad 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py @@ -1,28 +1,28 @@ -"""Stage old data until its individual zero poses have been checked again.""" +"""Stage checkpoint data under the explicitly selected reuse policy.""" from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field import numpy as np from scipy.spatial.transform import Rotation from ..core.domain.reference import read_joint_zero_references +from ..core.domain.motion_path import record_scan_identity from .scan_quality import evaluate_capture_unit from .resume import ResumeVerifier, TagPoseFingerprint @dataclass -class TaskResumeImport: - """Bounded journal transfer while live feedback and safety keep running.""" +class ResumeJournalImport: + """An ordered transfer that yields between bounded journal writes.""" - task_key: str - units: tuple rows: tuple - offset: int = 0 + offset: int = field(default=0, init=False) def take_batch(self): - batch = self.rows[self.offset:self.offset + 64] + from .resume_storage import materialize_record + batch = tuple(materialize_record(row) for row in self.rows[self.offset:self.offset + 64]) self.offset += len(batch) return batch @@ -31,12 +31,27 @@ class TaskResumeImport: return self.offset == len(self.rows) +@dataclass +class TaskResumeImport(ResumeJournalImport): + task_key: str + units: tuple + + +@dataclass +class ReferenceResumeImport(ResumeJournalImport): + """A verified zero cannot authorize motion until all its evidence is durable.""" + + references: dict + motion_version: int + + @dataclass(frozen=True) class PreparedResume: """Static checkpoint checks completed before live callbacks or motion. - This grants no reuse: the coordinator must still compare the current - fixed reference and verify each joint's physical zero installation. + The default policy replays evidence and requires current physical zeros. + Passed-task mode reads persisted passes under the declared unchanged + installation assumption; imports still precede skipped motion. """ header: dict @@ -44,13 +59,17 @@ class PreparedResume: rows: tuple completed_units: frozenset joints: JointResume + passed_import: ResumeJournalImport | None = None @classmethod - def load(cls, profile, engine, path, serial_number): + def load(cls, profile, engine, path, serial_number, *, image_replay_workers=1, resume_mode="verify"): from .acquisition import load_capture + from .resume_storage import load_passed_capture from .diagnostic_capture import reject_diagnostic_capture - rows = tuple(load_capture(path)) + if resume_mode not in {"verify", "passed"}: + raise ValueError("unknown resume mode") + rows = tuple(load_passed_capture(path) if resume_mode == "passed" else load_capture(path)) reject_diagnostic_capture(rows) headers = [r for r in rows if r.get("kind") == "session_start"] references = [r for r in rows if r.get("kind") == "fixed_base_reference_locked"] @@ -59,12 +78,24 @@ class PreparedResume: header = headers[0] if header.get("serial_number") != serial_number or not engine.resume_compatible(header): raise ValueError("checkpoint identity/policy changed") - complete = frozenset((r["task_name"], r["cycle"], r["direction"]) for r in rows + from .training import resolve_capture_plan + from .engine import CalibrationEngine + profile = resolve_capture_plan(profile, rows) + engine = CalibrationEngine(profile) + complete = frozenset(record_scan_identity(r) for r in rows if r.get("kind") == "scan_unit_complete" and r.get("passed") is True) - joints = JointResume(profile) + joints = JointResume(profile, image_replay_workers=image_replay_workers) + passed_import = None if profile.artifacts.output_schema_version >= 3: - joints.stage(engine, rows, complete) - return cls(header, references[0], rows, complete, joints) + if resume_mode == "passed": + from .passed_resume import reference_import, stage_passed_tasks + stage_passed_tasks(joints, engine, rows) + else: + joints.stage(engine, rows, complete) + joints.restrict_adaptive_prefix(engine) + if resume_mode == "passed": + passed_import = reference_import(joints) + return cls(header, references[0], rows, complete, joints, passed_import) def _pose(samples, field): @@ -75,14 +106,20 @@ def _pose(samples, field): def _index_motion_provenance(rows): from .motion_provenance import source_frame_sha256 + from .resume_storage import materialize_record zero_images = {(r.get("view"), r.get("image_stamp_ns")) for r in rows if r.get("kind") == "joint_zero_sample"} kinds = {"motion_branch_observation", "motion_branch_initialization", - "joint_zero_motion_reference_verified", "joint_zero_sample"} - return {source_frame_sha256(row): row for row in rows - if row.get("kind") in kinds or (row.get("kind") == "pnp_candidate_frame" - and (row.get("view"), row.get("image_stamp_ns")) in zero_images)} + "capture_scope_extension", "capture_observation_revision", + "passed_tasks_reused", "passed_pose_models_restored", + "parent_reference_verified", "joint_zero_recovery", + "joint_zero_motion_reference_verified", "joint_zero_sample", + "visual_motion_observation", "visual_motion_settled"} + selected = (materialize_record(row) for row in rows + if row.get("kind") in kinds or (row.get("kind") == "pnp_candidate_frame" + and (row.get("view"), row.get("image_stamp_ns")) in zero_images)) + return {source_frame_sha256(row): row for row in selected} def reference_pose_failures(previous, current): @@ -104,8 +141,9 @@ def reference_pose_failures(previous, current): class JointResume: - def __init__(self, profile): + def __init__(self, profile, *, image_replay_workers=1): self.profile = profile + self.image_replay_workers = image_replay_workers self.references = {} self.verified = set() self.units = set() @@ -115,27 +153,66 @@ class JointResume: self._motion_provenance_source = None self._motion_provenance_emitted = set() + def restrict_adaptive_prefix(self, engine): + """A restarted task invalidates later decisions' causal training inputs. + + Reuse complete tasks up to that point. New preparation and decisions + restart the incomplete suffix, without rewriting the source journal. + """ + from ..core.domain.capture_plan import ADAPTIVE, configure_training, select_task_training + if self.profile.quality.training_policy != ADAPTIVE: + return + profile = configure_training(self.profile, ADAPTIVE) + kept, joints = set(), set() + for task in self.profile.motion.tasks: + expected = {u.identity for u in engine.scan_units() if u.task_key == task.key} + cycles = self.profile.quality.task_training_cycles.get(task.key) + if not expected <= self.units or cycles is None: + break + kept.update(expected) + joints.update(task.joints) + profile = select_task_training(profile, task.key, cycles) + self.profile = profile + self.units = kept + self.references = {name: ref for name, ref in self.references.items() if name in joints} + self.verified.intersection_update(joints) + def stage(self, engine, rows, units): headers = [row for row in rows if row.get("kind") == "session_start"] if len(headers) != 1 or not engine.resume_compatible(headers[0]): raise ValueError("joint_resume_checkpoint_policy_incompatible") references = read_joint_zero_references(self.profile, rows) from .motion_provenance import validate_motion_provenance - validate_motion_provenance(self.profile, rows, references=references) + validate_motion_provenance(self.profile, rows, references=references, + image_replay_workers=self.image_replay_workers) from ..core.fitting.command_sampling import validate_sampling_plans validate_sampling_plans(self.profile, rows) spans, valid = {}, set() for unit in engine.scan_units(): - key = (unit.task_key, unit.cycle, unit.direction) + key = unit.identity task = next(t for t in self.profile.motion.tasks if t.key == unit.task_key) + if task.segments: + continue if key not in units or not set(task.joints) <= references.keys(): continue attempts = [int(r.get("attempt", 1)) for r in rows - if (r.get("task_name"), r.get("cycle"), r.get("direction")) == key] + if record_scan_identity(r) == key] if evaluate_capture_unit(self.profile, unit, max(attempts, default=1), rows, first_cycle_spans=spans).passed: valid.add(key) - self.references, self.units, self.rows = references, valid, tuple(rows) + # Restart a segmented task with fresh references as well as fresh rows. + restarted = {name for task in self.profile.motion.tasks if task.segments for name in task.joints} + from .task_quality import evaluate_task_input_support + support_phase = ("steady" if self.profile.command_based_release + and self.profile.acquisition.command_capture_mode == "separate" else "sweep") + for task in self.profile.motion.tasks: + motion_units = {unit.identity for unit in engine.scan_units() + if unit.task_key == task.key and unit.sample_phase == support_phase} + if motion_units <= valid and not evaluate_task_input_support(self.profile, task, rows).passed: + valid = {key for key in valid if key[0] != task.key} + restarted.update(task.joints) + self.references = {name: reference for name, reference in references.items() if name not in restarted} + self.units, self.rows = valid, tuple(rows) self.first_cycle_spans = spans self._motion_provenance_source = _index_motion_provenance(self.rows) self._motion_provenance_pending.clear() @@ -164,9 +241,16 @@ class JointResume: zero_images = {(row.get("view"), row.get("image_stamp_ns")) for row in self._motion_provenance_source.values() if row.get("kind") == "joint_zero_sample" and row.get("joint") == reference.joint} + parent_images = {(row["view"], int(stamp)) for row in self._motion_provenance_source.values() + if row.get("kind") == "parent_reference_verified" for stamp in row["source_frame_hashes"]} + parent_images.update((sample["view"], sample["image_stamp_ns"]) + for row in self._motion_provenance_source.values() if row.get("kind") == "parent_reference_verified" + for sample in row["source_reference"]["samples"]) + zero_images.update(parent_images) for identity, row in self._motion_provenance_source.items(): if (row["kind"] in {"joint_zero_sample", "joint_zero_motion_reference_verified"} - and row.get("joint") != reference.joint): + and row.get("joint") != reference.joint + and (row.get("view"), row.get("image_stamp_ns")) not in parent_images): continue # An interrupted resume must not import unverified joints' references. if (row["kind"] == "pnp_candidate_frame" and (row.get("view"), row.get("image_stamp_ns")) not in zero_images): @@ -196,10 +280,13 @@ class JointResume: if not set(task.joints) <= self.verified: return (), () units = {key for key in self.units if key[0] == task_key} - rows = tuple(r for r in self.rows if (r.get("task_name"), r.get("cycle"), r.get("direction")) in units + rows = tuple(r for r in self.rows if (record_scan_identity(r) in units and r.get("kind") not in {"joint_zero_reference", "joint_zero_sample"}) + or (units and r.get("kind") == "training_decision" and r.get("task_name") == task_key)) if units: - rows = tuple(r for r in self.rows if r.get("kind") == "command_sampling_plan" - and r.get("task_name") == task_key) + rows + plans = tuple(r for r in self.rows if r.get("kind") == "command_sampling_plan" + and r.get("task_name") == task_key) + rows = plans + rows + tuple(r for r in self.rows if r.get("kind") == "task_input_support_checked" + and r.get("task_name") == task_key) self.units.difference_update(units) return tuple(sorted(units)), rows diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py index 067fd98..dfcd40a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -1,7 +1,7 @@ """Clock-driven trajectory execution, independent of ROS and hand identity. The session owns business transitions. This module executes one whole segment -and supplies its stable feedback-space safety goals; it never retries or fits. +and supplies feedback-space motion evidence; it never retries or fits. """ from __future__ import annotations @@ -14,6 +14,21 @@ from .safety import MotionGoal from .trajectory import cosine_position_trajectory_u8, cosine_ramp_velocity_trajectory +@dataclass(frozen=True) +class FeedbackTolerances: + resolution: float + stability: float + + @property + def minimum_travel(self): + return max(3*self.resolution, self.stability+self.resolution) + + +def feedback_tolerances(unit): + """Shared native-unit thresholds for live movement and captured travel.""" + return FeedbackTolerances(.5, 2.) if unit == "u8" else FeedbackTolerances(.001, .02) + + @dataclass(frozen=True) class MotionCommand: phase: str @@ -30,6 +45,17 @@ class MotionCommand: minimum_hold_seconds: float = 0.0 steady_training_nodes: tuple[float, ...] = () sampling_plan_sha256: str = "" + segment_key: str = "" + observed_joints: tuple[str, ...] = () + measured_channels: tuple[int, ...] = () + reference_only: bool = False + reference_reuse: bool = False + reference_command: tuple[float, ...] = () + reference_approach: tuple[tuple[float, ...], ...] = () + reference_path_index: int | None = None + feedback_targets: tuple[tuple[int, float], ...] = () + visual_return_joints: tuple[str, ...] = () + defer_settling_to_hold: bool = False @property def recording(self): @@ -38,10 +64,15 @@ class MotionCommand: class MotionExecution: def __init__(self, profile, command: MotionCommand, *, initial_command, - initial_feedback, now: float, identity: str): + initial_feedback, now: float, identity: str, visual_observer=None): self.profile, self.command, self.identity = profile, command, str(identity) + self.visual = visual_observer + if profile.vision_motion and visual_observer is None: + raise ValueError("visual motion requires its independent image observer") if not math.isfinite(command.minimum_hold_seconds) or command.minimum_hold_seconds < 0: raise ValueError("minimum hold must be finite and nonnegative") + if command.defer_settling_to_hold and command.phase != "sweep": + raise ValueError("only a sweep followed by a steady hold can defer settling") self.start = tuple(float(v) for v in initial_command) self.start_feedback = tuple(float(v) for v in initial_feedback) self.started_at = float(now) @@ -57,10 +88,10 @@ class MotionExecution: if len(vector) != layout.command_count or any(not math.isfinite(v) or not lo <= v <= hi for v, lo, hi in zip(vector, layout.minimum_values, layout.maximum_values)): raise ValueError("motion command outside declared command domain") - if len(self.start_feedback) != layout.command_count: + if not profile.vision_motion and len(self.start_feedback) != layout.command_count: raise ValueError("motion starts without complete feedback") - self.resolution = 0.5 if layout.unit == "u8" else 0.001 - self.stability = 2.0 if layout.unit == "u8" else 0.02 + tolerances = feedback_tolerances(layout.unit) + self.resolution, self.stability = tolerances.resolution, tolerances.stability probe = command.phase in {"mapping_probe", "mapping_return"} # A mapping jog proves direction and basic electrical movement, not # servo accuracy. Baseline corrections no larger than the mapping @@ -69,20 +100,40 @@ class MotionExecution: self.minimum_request = (2.0 if layout.unit == "u8" else self.resolution if probe else max(profile.acquisition.mapping_probe_maximum_rad, math.radians(3))) self.required_fraction = 0.10 if probe else 0.80 + self.feedback_travel_matches_command = profile.acquisition.feedback_travel_matches_command self.deltas = tuple(end-start for start, end in zip(self.start, command.target)) + # Without a measured command/encoder mapping, substantial requests + # still need an electrical movement witness. Small endpoint corrections + # can remain inside a dead zone; do not fabricate proportional travel. + self.minimum_requests = tuple(self.minimum_request if self.feedback_travel_matches_command + else max(self.minimum_request, .05*(hi-lo)) + for lo, hi in zip(layout.minimum_values, layout.maximum_values)) + self.minimum_feedback_travel = tolerances.minimum_travel self.moving = tuple(i for i, delta in enumerate(self.deltas) - if abs(delta) > self.minimum_request and i not in layout.disabled_indices) - self.goals = tuple(MotionGoal(i, self.start_feedback[i], - self.start_feedback[i] + self.required_fraction*self.deltas[i], self.resolution, self.resolution) - for i in self.moving) + if abs(delta) > self.minimum_requests[i] and i not in layout.disabled_indices) + self.required_feedback_travel = tuple(self.required_fraction*abs(delta) + if self.feedback_travel_matches_command else self.minimum_feedback_travel for delta in self.deltas) + self._travel_origins = list(self.start_feedback) + # A return to a measured zero has an encoder reference, even when the + # last sent command already equals zero. Never infer that return from + # command equality or from a small amount of unrelated motion. + self.reference_targets = dict(command.feedback_targets) + if (len(self.reference_targets) != len(command.feedback_targets) + or any(not 0 <= i < layout.command_count or not math.isfinite(value) + or not layout.minimum_feedback_values[i] <= value <= layout.maximum_feedback_values[i] + for i, value in self.reference_targets.items())): + raise ValueError("invalid measured feedback target") + self.reference_tolerance = 2*self.stability def sample(self, now: float) -> tuple[float, ...]: distance = max((abs(v) for v in self.deltas), default=0.0) elapsed = max(0.0, float(now)-self.started_at) params = self.profile.motion.speed_parameters if self.profile.command.unit == "u8": + duration_key = ("scan_trajectory_full_range_seconds" + if self.command.phase in {"sweep", "steady"} else "command_trajectory_full_range_seconds") travelled, phase, duration = cosine_position_trajectory_u8(0, distance, - elapsed, float(params.get("command_trajectory_full_range_seconds", 6.0))) + elapsed, float(params.get(duration_key, params.get("command_trajectory_full_range_seconds", 6.0)))) else: speed = self.command.speed caps = self.profile.command.maximum_velocity @@ -98,38 +149,101 @@ class MotionExecution: return tuple(float(round(v)) for v in values) if self.profile.command.unit == "u8" else values def observe(self, feedback, *, stamp, now): + if self.visual is not None: + if self.phase >= 1.0 and self.finished_at is None: + self.finished_at = float(now) + return # A fast timer is not ten independent feedback samples. if stamp == self.last_feedback_stamp: return self.last_feedback_stamp = stamp + if not self.feedback_travel_matches_command: + # At a reversal, a delayed encoder can first finish reporting the + # preceding movement. Count subsequent travel in the requested + # direction from that observed turning point, not an invented + # endpoint beyond the segment's initial feedback. An opposite-only + # excursion is not movement evidence, and cannot restart the + # watchdog: the segment identity and requested direction stay fixed. + for i in self.moving: + if i not in self.reference_targets: + direction = math.copysign(1., self.deltas[i]) + if direction*(feedback[i]-self._travel_origins[i]) < 0: + self._travel_origins[i] = float(feedback[i]) self.history.append(tuple(feedback)) self.stamped_history.append((float(now), tuple(feedback))) if self.phase >= 1.0 and self.finished_at is None: self.finished_at = float(now) + @property + def goals(self): + goals = {} if self.profile.vision_motion 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) + for i in self.moving} + # Measured zero/return targets remain absolute. Travel, including a + # reversal, never substitutes for reaching the measured reference. + goals.update({i: MotionGoal(i, self.start_feedback[i], value, + self.reference_tolerance, self.resolution) for i, value in self.reference_targets.items()}) + if self.feedback_travel_matches_command: + return tuple(goals.values()) + # Coupled trajectories reach meaningful demand at different times. + # A fast channel cannot start a slow channel's no-motion clock. + return tuple(goal for goal in goals.values() + if abs(self.deltas[goal.channel])*self.fraction > self.minimum_requests[goal.channel] + or (goal.channel in self.reference_targets + and abs(self.deltas[goal.channel]) <= self.minimum_requests[goal.channel])) + @property def motion_expected(self): # A cosine ramp initially requests less than encoder resolution. Start # the watchdog only after meaningful demand, then keep the segment ID. - return bool(self.moving and self.phase > 0 and - any(abs(self.deltas[i])*self.fraction > self.minimum_request for i in self.moving)) + return bool(self.goals and self.phase > 0 and (self.reference_targets or + any(abs(self.deltas[i])*self.fraction > self.minimum_requests[i] for i in self.moving))) + + def feedback_motion_complete(self): + """Physical completion is shared by capture and transition decisions.""" + if self.visual is not None: + return self.visual.motion_complete() + if not self.history: + return False + latest = self.history[-1] + if any(abs(latest[i]-target) > self.reference_tolerance + for i, target in self.reference_targets.items()): + return False + return all(i in self.reference_targets or + math.copysign(1, self.deltas[i])*(latest[i]-self._travel_origins[i])+self.resolution + >= self.required_feedback_travel[i] for i in self.moving) def arrived(self, now): + if self.visual is not None: + if self.phase < 1 or self.finished_at is None or not self.visual.motion_complete(): + return False + if self.command.phase in {"steady", "joint_zero"}: + return now-self.finished_at >= max(self.profile.acquisition.steady_timeout_seconds, + self.command.minimum_hold_seconds) + if self.command.defer_settling_to_hold: + return True + hold = float(self.profile.motion.speed_parameters.get("endpoint_hold_seconds", .25)) + return now-self.finished_at >= hold and self.visual.stable_window(self.finished_at, now) is not None + 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.command.phase in {"steady", "joint_zero"}: # Data sufficiency is evaluated after the direction; never wait - # indefinitely for a Tag or exact command/feedback equality. - return (self.finished_at is not None and - now-self.finished_at >= max(self.profile.acquisition.steady_timeout_seconds, - self.command.minimum_hold_seconds)) - if self.phase < 1 or self.finished_at is None or len(self.history) < 3: - return False + # indefinitely for a Tag. Missing physical motion is instead + # stopped by the watchdog; a capture timeout cannot waive it. + return now-self.finished_at >= max(self.profile.acquisition.steady_timeout_seconds, + self.command.minimum_hold_seconds) + if self.command.defer_settling_to_hold: + # The immediately following steady effect holds this same target + # and independently gates its image window on stable feedback. + # Do not wait twice at each interior mapping node. + return True hold = float(self.profile.motion.speed_parameters.get("endpoint_hold_seconds", 0.25)) if now-self.finished_at < hold: return False - for i in self.moving: - projected = math.copysign(1, self.deltas[i])*(self.history[-1][i]-self.start_feedback[i]) - if projected+self.resolution < self.required_fraction*abs(self.deltas[i]): - return False + for i in set(self.moving) | self.reference_targets.keys(): if max(row[i] for row in self.history)-min(row[i] for row in self.history) > self.stability: return False return True @@ -138,7 +252,11 @@ class MotionExecution: return self.finished_at is not None and now-self.finished_at >= self.command.minimum_hold_seconds def steady_ready(self, now): - if self.command.phase not in {"steady", "joint_zero"} or self.finished_at is None: + if self.visual is not None: + return (self.command.phase in {"steady", "joint_zero"} + and self.visual.stable_window(self.finished_at, now) is not None) + if (self.command.phase not in {"steady", "joint_zero"} or self.finished_at is None + or not self.feedback_motion_complete()): return False window = self.profile.acquisition.steady_window_seconds rows = [(stamp, value) for stamp, value in self.stamped_history @@ -158,7 +276,8 @@ class MotionExecution: rows = rows[boundary:] if len(rows) < 3: return False - indices = range(self.profile.command.command_count) if self.command.phase == "joint_zero" else (self.command.command_index,) + indices = (range(self.profile.command.command_count) if self.command.phase == "joint_zero" + else self.command.measured_channels or (self.command.command_index,)) return all(max(v[i] for _, v in rows)-min(v[i] for _, v in rows) <= max(2*self.resolution, 0.005*(self.profile.command.maximum_feedback_values[i]-self.profile.command.minimum_feedback_values[i])) for i in indices) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py index bf9783c..3444210 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_provenance.py @@ -11,10 +11,13 @@ from collections.abc import Mapping from ..core.domain.reference import joint_zero_moving_roles, read_joint_zero_references from ..core.geometry.image_motion_replay import ( - IMAGE_POSE_SOURCE, frozen_image_model, has_image_model, replay_image_models, - validate_image_sample, + IMAGE_POSE_SOURCE, frozen_image_model, has_image_model, ) from ..core.geometry.tag_pose.parameters import POSE_TRACKING_POLICY_VERSION +from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence +from ..core.geometry.tag_pose.pose_bridge import ( + SHARED_TAG_CONDITIONING_POLICIES, +) MOTION_BRANCH_POLICY_VERSION = POSE_TRACKING_POLICY_VERSION @@ -60,11 +63,24 @@ def _check_source_hashes(payload, observations, hashes): or any(not isinstance(value, str) or re.fullmatch(r"[0-9a-f]{64}", value) is None for value in declared.values())): _fail("source_hash_coverage_invalid") + versions = payload.get("source_motion_versions") + path = payload.get("approach_evidence_scope") == "path" + if path or versions is not None: + if (not path or not isinstance(versions, Mapping) + or set(versions) != {str(stamp) for stamp in stamps} + or any(type(version) is not int or not 1 <= version <= payload["motion_version"] + for version in versions.values()) + or [versions[str(stamp)] for stamp in stamps] != sorted(versions.values())): + _fail("source_motion_versions_invalid") for stamp in stamps: key = (payload["view"], stamp) source = observations.get(key) - if source is None or any(source.get(field) != payload[field] for field in ( - "task_name", "zero_joints", "session_epoch", "motion_version")): + if (source is None or any(source.get(field) != payload[field] for field in ( + "task_name", "zero_joints", "session_epoch")) + or source.get("motion_version") != (versions[str(stamp)] if path else payload["motion_version"]) + or path and (type(source.get("reference_path_index")) is not int + or source["reference_path_index"] < 1 + or source.get("sample_phase") != "zero_approach")): _fail("source_image_missing_or_changed") if key not in hashes: hashes[key] = source_frame_sha256(source) @@ -81,6 +97,14 @@ def _initializations(records): _fail("source_image_changed") observations[key] = row if row.get("kind") == "motion_branch_initialization" and row.get("status") == "resolved": + _check_measured_transfer_report(row) + if ((row.get('image_model_selection_policy') in SHARED_TAG_CONDITIONING_POLICIES) + != bool(row.get('shared_tag_pose_bridges'))): + _fail('shared_pose_selection_policy_changed') + conditioning = SHARED_TAG_CONDITIONING_POLICIES.get(row.get('image_model_selection_policy')) + if any(bridge.get('conditioning') != conditioning + for bridge in row.get('shared_tag_pose_bridges', ())): + _fail('shared_pose_conditioning_policy_changed') resolved.append(row) for role, payload in row.get("evidence_payloads", {}).items(): identity = row.get("evidence_ids", {}).get(role) @@ -90,7 +114,9 @@ def _initializations(records): if payload.get("tag_role") != role or any(payload.get(key) != row.get(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")): + "frozen_image_model", "image_model_sha256", + "geometry_uncertainty", "parent_reference_sha256", + "approach_evidence_scope", "source_motion_versions", "shared_tag_pose_bridges")): _fail("initialization_binding_changed") stamps = payload.get("source_image_stamps", ()) if (not stamps or any(type(stamp) is not int or stamp < 1 for stamp in stamps) @@ -119,12 +145,40 @@ def _initializations(records): headers = [row for row in records if row.get("kind") == "session_start"] if len(headers) != 1 or headers[0].get("source_urdf_sha256") != source_hash: _fail("image_model_source_urdf_changed") + from .parent_reference_evidence import validate_source_hinges + validate_source_hinges(evidence, records) return evidence +def _check_measured_transfer_report(row): + """Bind the fitted prior domain and uncertainty to the accepted hypothesis.""" + from dataclasses import asdict + from ..core.geometry.tag_pose.measured_transfer import MEASURED_TRANSFER_POLICY, transfer_geometry_checks + + if row.get("frozen_image_model", {}).get("policy") != MEASURED_TRANSFER_POLICY: + return + model = frozen_image_model(row) + candidates = [h for h in row.get("hypotheses", ()) + if _canonical(h.get("branches")) == _canonical(model.branches)] + if len(candidates) != 1: + _fail("measured_transfer_hypothesis_missing") + winner = candidates[0] + checks, inherited = transfer_geometry_checks(model.geometry, model.constraints, + model.source_hinges, model.current_geometry_uncertainty) + axes = [(name, axis+inherited[name][0]) for name, axis, _ in model.current_geometry_uncertainty] + points = [(name, point+inherited[name][1]) for name, _, point in model.current_geometry_uncertainty] + if (winner.get("training_accepted") is not True or winner.get("converged") is not True + or winner.get("reason") or _canonical(winner.get("source_geometry_checks")) != _canonical([asdict(c) for c in checks]) + or _canonical(winner.get("axis_uncertainty_95_rad")) != _canonical(axes) + or _canonical(winner.get("pivot_uncertainty_95_m")) != _canonical(points) + or _canonical(winner.get("child_frame_uncertainty")) != _canonical(row.get("geometry_uncertainty"))): + _fail("measured_transfer_uncertainty_binding_changed") + + def validate_source_geometry(profile, source_urdf, records): """Re-derive source constraints before finalization; never trust labels alone.""" - from ..core.geometry.tag_pose.cad_image_model import CAD_IMAGE_MOTION_MODEL_POLICY + from ..core.geometry.tag_pose.cad_image_model import CAD_IMAGE_MOTION_MODEL_POLICY, TRANSFERRED_IMAGE_MOTION_MODEL_POLICY + from ..core.geometry.tag_pose.measured_transfer import MEASURED_TRANSFER_POLICY from ..core.urdf.kinematics import UrdfKinematicModel from ..profiles.observations import compile_parallel_axis_geometry @@ -133,15 +187,24 @@ def validate_source_geometry(profile, source_urdf, records): if row.get("kind") != "motion_branch_initialization" or row.get("status") != "resolved": continue payload = row.get("frozen_image_model", {}) - if payload.get("policy") != CAD_IMAGE_MOTION_MODEL_POLICY: + if payload.get("policy") not in {CAD_IMAGE_MOTION_MODEL_POLICY, TRANSFERRED_IMAGE_MOTION_MODEL_POLICY, MEASURED_TRANSFER_POLICY}: continue model = frozen_image_model(row) if source_model is None: source_model = UrdfKinematicModel(source_urdf) source_hash = hashlib.sha256(source_model.source.read_bytes()).hexdigest() if (model.source_urdf_sha256 != source_hash - or model.constraints != compile_parallel_axis_geometry(profile, source_model, model.relations)): + or model.constraints != compile_parallel_axis_geometry(profile, source_model, + tuple(item.geometry.relation for item in getattr(model, "source_hinges", ())) + model.relations)): _fail("image_model_source_geometry_changed") + if source_model is not None and any(task.parent_reference is not None for task in profile.motion.tasks): + from .parent_reference_evidence import validate_parent_references + validate_parent_references(profile, records, _initializations(records), source_model=source_model) + if any(row.get('shared_tag_pose_bridges') for row in records): + from .shared_tag_evidence import validate_shared_tag_references + if source_model is None: + source_model = UrdfKinematicModel(source_urdf) + validate_shared_tag_references(profile, records, _initializations(records), source_model=source_model) def _check_image_initialization(payload, observations): @@ -159,8 +222,35 @@ def _check_image_initialization(payload, observations): or _canonical(source.get("camera_matrix")) != _canonical(model.camera_matrix)): _fail("image_model_support_not_pre_zero") for role, size in model.tag_sizes: - if source.get("tags", {}).get(role, {}).get("tag_size_m") != size: + item = source.get("tags", {}).get(role, {}) + if item.get("tag_size_m") != size: _fail("image_model_tag_size_changed") + references = {ref.role: ref for ref in model.reference_frames} + if role in references or item.get("pose_source") == IMAGE_REFERENCE_SOURCE: + reference = reference_from_evidence(item, stamp_ns=stamp, camera_matrix=model.camera_matrix) + if reference != references.get(role): + _fail("image_model_reference_changed") + + +def _check_reference_modes(profile, records, evidence): + """Only declared fixed Tags can supply camera-aligned coordinates.""" + mode = profile.acquisition.fixed_reference_mode + headers = [row for row in records if row.get("kind") == "session_start"] + if mode == "stationary_image": + from .engine import capture_schedule_version + if (len(headers) != 1 or headers[0].get("fixed_reference_mode") != mode + or headers[0].get("capture_schedule_version") != capture_schedule_version(profile)): + _fail("image_reference_header_changed") + fixed = {view.name: {tag.role for tag in view.tags if tag.fixed_reference} + for view in profile.vision.views} + for payload in evidence.values(): + if not has_image_model(payload): + continue + model = frozen_image_model(payload) + expected = (fixed.get(payload["view"], set()) & set(dict(model.tag_sizes)) + if mode == "stationary_image" else set()) + if {ref.role for ref in model.reference_frames} != expected: + _fail("image_reference_profile_binding_changed") def _image_frames(records): @@ -191,7 +281,8 @@ def _reference_authorizations(profile, reference, evidence): return result -def validate_motion_provenance(profile, records, *, required=False, require_complete=False, references=None): +def validate_motion_provenance(profile, records, *, required=False, require_complete=False, + references=None, image_replay_workers=1): """Check capture authorization, never certify physical accuracy. The explicit ``required`` flag is set by the production owner. Independent @@ -199,12 +290,20 @@ def validate_motion_provenance(profile, records, *, required=False, require_comp """ if not required and not uses_motion_evidence(records): return {"status": "legacy_image_confirmation", "motion_evidence_verified": False} + from .visual_motion import validate_visual_motion_evidence + validate_visual_motion_evidence(profile, records) references = read_joint_zero_references(profile, records) if references is None else references expected = {name for task in profile.motion.tasks for name in task.joints} if not references or (require_complete and not expected <= references.keys()): _fail("joint_zero_references_missing") evidence = _initializations(records) - image_frames, replayed = _image_frames(records), {} + _check_reference_modes(profile, records, evidence) + from .parent_reference_evidence import validate_parent_references + validate_parent_references(profile, records, evidence) + from .shared_tag_evidence import validate_shared_tag_references + if any(payload.get('shared_tag_pose_bridges') for payload in evidence.values()): + validate_shared_tag_references(profile, records, evidence) + image_frames, image_samples = _image_frames(records), {} authorizations = {name: _reference_authorizations(profile, reference, evidence) for name, reference in references.items()} zero_images = {(name, sample.view, sample.image_stamp_ns) @@ -239,6 +338,7 @@ def validate_motion_provenance(profile, records, *, required=False, require_comp by_role = {item.get("tag_role"): item for item in observed.values()} if not roles or not roles <= by_role.keys(): _fail(f"sample_role_missing:{name}") + image_group = None for role in roles: item = by_role[role] diagnostics = item.get("candidate_diagnostics", {}) @@ -261,12 +361,16 @@ def validate_motion_provenance(profile, records, *, required=False, require_comp frame = image_frames.get(frame_key) if frame is None: _fail(f"sample_image_frame_missing:{name}:{view}:{role}") - if frame_key not in replayed: - replayed[frame_key] = replay_image_models(frame) - validate_image_sample(row, frame, replayed[frame_key]) + image_group = image_samples.setdefault(frame_key, (frame, [])) + if image_group is not None: + image_group[1].append(row) if not is_zero: cycles.setdefault(name, set()).add(row.get("cycle")) - if require_complete and any(not {0, 1, 2, 3} <= cycles.get(name, set()) for name in expected): + from .image_replay import validate_image_groups + validate_image_groups(image_samples.values(), workers=image_replay_workers) + from ..core.domain.capture_plan import training_cycles + if require_complete and any(not {*training_cycles(profile, joint=name), profile.quality.holdout_cycle} + <= cycles.get(name, set()) for name in expected): _fail("independent_cycles_missing") return {"status": "motion_evidence_verified", "motion_evidence_verified": True, "joint_count": len(references), "evidence_count": len(evidence), "is_accuracy_certificate": False} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py index 2957973..55d6abd 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/observation_timing.py @@ -4,6 +4,7 @@ from collections.abc import Mapping, Sequence import math from numbers import Real +from ..core.domain.motion_path import record_scan_identity, task_segment from ..core.domain.profile import CalibrationProfile from .engine import ScanUnit from .scan_quality import observation_streams @@ -60,14 +61,15 @@ def summarize_observation_timing( inferred. These facts neither apply thresholds nor imply accuracy. """ task = next(task for task in profile.motion.tasks if task.key == unit.task_key) - streams = tuple(observation_streams(profile, task)) + segment = task_segment(task, unit.segment_key) if unit.segment_key else None + streams = tuple(item for item in observation_streams(profile, task) + if segment is None or item[1] in segment.joints) required_views = {spec.view for _, _, spec in streams} frames: dict[str, dict[int, Mapping]] = {view: {} for view in required_views} for row in records: if (row.get("kind") != "diagnostic_observation_frame" or row.get("sample_phase") != "sweep" - or (row.get("task_name"), row.get("cycle"), row.get("direction"), row.get("attempt")) - != (unit.task_key, unit.cycle, unit.direction, attempt)): + or record_scan_identity(row) != unit.identity or row.get("attempt") != attempt): continue view, stamp = row.get("view"), row.get("image_stamp_ns") if (view not in frames or not isinstance(stamp, int) @@ -96,7 +98,7 @@ def summarize_observation_timing( for role, tag_id in required.items())) if visible: visible_stamps.append(stamp) - if command and feedback: + if command and (feedback or profile.vision_motion): synchronized_stamps.append(stamp) result[f"{field}:{name}:{spec.view}:{unit.direction}"] = { "raw_frames": len(frames[spec.view]), diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py index 98a523c..95b410e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py @@ -40,6 +40,9 @@ class RuntimeParameters: detection: DetectionPolicy tracking: PoseTrackingParameters diagnostic_capture: str = "" + initial_command: tuple[float, ...] = () + initial_device_uid: str = "" + resume_mode: str = "verify" def new_trackers(self, views): return {view: SquareTagPoseTracker(**asdict(self.tracking)) for view in views} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py new file mode 100644 index 0000000..0c56e27 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference.py @@ -0,0 +1,147 @@ +"""Reuse a measured Tag installation only after checking its current held pose. + +Pose authorization and local hinge geometry are deliberately separate: an +upstream pose change invalidates the former, but not a rigid Tag's mounting. +No curve, joint zero or candidate selection is copied between joints. +""" + +import math + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..core.domain.reference import JointZeroSample +from ..core.geometry.tag_pose.source_hinge import SourceHinge +from .motion_execution import feedback_tolerances +from .resume import ResumeVerifier + + +def check_parent_pose(profile, task, reference, rows, channels): + """Same pure check for live execution and persisted evidence read-back.""" + name = task.parent_reference.pose_joint + spec = profile.measurement.measurements[name] + target = profile.motion.joint_zero_references[task.joints[0]].command + selected = [JointZeroSample.from_row(row, profile.command.unit) for row in rows] + old = [sample for sample in reference.samples if sample.view == spec.view] + if (reference.joint != name or not old + or len(selected) < profile.acquisition.fixed_reference_minimum_frames + or len({sample.sample_id for sample in selected}) != len(selected) + or any(sample.view != spec.view or sample.command != target for sample in selected) + or any(sample.command[index] != target[index] for sample in old for index in channels)): + raise ValueError("parent_reference_pose_contract_changed") + if min(sample.image_stamp_ns for sample in selected) <= max(sample.image_stamp_ns for sample in old): + raise ValueError("parent_reference_check_not_current") + if not profile.vision_motion: + before = np.asarray([sample.feedback for sample in old]) + after = np.asarray([sample.feedback for sample in selected]) + if (before.shape != (len(old), profile.command.command_count) + or after.shape != (len(selected), profile.command.command_count) + or not np.all(np.isfinite(before)) or not np.all(np.isfinite(after))): + raise ValueError("parent_reference_feedback_missing") + tolerance = feedback_tolerances(profile.command.unit).stability + indices = sorted(channels) + if (np.any(np.ptp(after[:, indices], axis=0) > tolerance) + or np.any(np.abs(after[:, indices] - np.median(before[:, indices], axis=0)) > 2*tolerance)): + raise ValueError("parent_reference_held_feedback_changed") + policy = ResumeVerifier() + for field in ("parent_pose_common", "child_pose_common"): + before = [getattr(sample, field) for sample in old] + after = [getattr(sample, field) for sample in selected] + rotation = Rotation.from_quat([pose.quaternion_xyzw for pose in before]).mean() + rotations = Rotation.from_quat([pose.quaternion_xyzw for pose in after]) + translations = np.asarray([pose.translation_xyz_m for pose in after]) + if (np.max((rotation.inv()*rotations).magnitude()) > policy.maximum_rotation_rad + or np.max(np.linalg.norm(translations - np.median( + [pose.translation_xyz_m for pose in before], axis=0), axis=1)) > policy.maximum_translation_m): + raise ValueError(f"parent_reference_pose_changed:{field}") + if (np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1) + or np.max(np.linalg.norm(translations-np.median(translations, axis=0), axis=1)) > .001): + raise ValueError(f"parent_reference_not_stable:{field}") + + +class ParentReferenceRegistry: + """Accepted models are immutable and isolated by observation epoch.""" + + def __init__(self, profile, tag_feedback_channels): + self.profile = profile + self.channels = tag_feedback_channels + self.clear() + + def clear(self): + self.models = {} + self.verified = {} + self.zero_references = {} + + def remember_zero(self, reference, epoch): + for view in {sample.view for sample in reference.samples}: + self.zero_references[(epoch, view, reference.joint)] = reference + + def remember(self, model, report, *, epoch=None): + # An explicit passed-task restore binds the original installation to + # the new observation epoch while retaining its original source report. + epoch = report["session_epoch"] if epoch is None else epoch + if model.image_model is not None: + for geometry in model.image_model.geometry: + self.models[(epoch, model.view, geometry.relation.joint)] = (model, report) + + def advance_recovery_epoch(self, previous, current): + """An internal image retry invalidates callbacks, not rigid installations. + + Called only with the journalled recovery transition, never on pause, + restart or a physical-reference fault. Evidence keeps its original epoch. + """ + if current != previous+1: + raise ValueError("parent_reference_recovery_epoch_discontinuous") + self.models.update({(current, view, joint): value + for (epoch, view, joint), value in tuple(self.models.items()) if epoch == previous}) + self.verified.update({(current, task): value + for (epoch, task), value in tuple(self.verified.items()) if epoch == previous}) + self.zero_references.update({(current, view, joint): value + for (epoch, view, joint), value in tuple(self.zero_references.items()) if epoch == previous}) + + def sources(self, task, epoch): + reference = task.parent_reference + try: + pose = self.models[(epoch, task.view, reference.pose_joint)] + geometry = self.models[(epoch, task.view, reference.geometry_joint)] + except KeyError as error: + raise ValueError("parent_reference_current_session_source_missing") from error + return pose, geometry + + def activate(self, task, epoch, capture): + (model, _), _ = self.sources(task, epoch) + self.verified.pop((epoch, task.key), None) + capture.activate_reference_model(model) + + def confirm(self, task, epoch, version, reference, rows): + from .motion_provenance import source_frame_sha256 + (model, report), _ = self.sources(task, epoch) + role = self.profile.measurement.measurements[task.parent_reference.pose_joint].child_role + check_parent_pose(self.profile, task, reference, rows, self.channels[role]) + identity = dict(model.evidence_ids)[role] + for row in rows: + items = [item for item in row.get("pnp_observation_evidence", {}).values() if item.get("tag_role") == role] + if (len(items) != 1 or items[0].get("candidate_diagnostics", {}).get("motion_evidence_id") != identity + or items[0].get("image_model_sha256") != report["image_model_sha256"]): + raise ValueError("parent_reference_current_image_source_changed") + record = {"kind": "parent_reference_verified", "schema_version": 1, + "task_name": task.key, "view": task.view, "session_epoch": epoch, "motion_version": version, + "pose_joint": reference.joint, "pose_evidence_id": identity, + "source_reference": reference.as_record(), "held_channels": sorted(self.channels[role]), + "source_frame_hashes": {str(row["image_stamp_ns"]): source_frame_sha256(row) for row in rows}} + self.verified[(epoch, task.key)] = record + return record + + def geometry(self, task, epoch): + from .motion_provenance import source_frame_sha256 + if (epoch, task.key) not in self.verified: + raise ValueError("parent_reference_current_pose_unverified") + _, (model, report) = self.sources(task, epoch) + joint = task.parent_reference.geometry_joint + geometry = next(item for item in model.image_model.geometry if item.relation.joint == joint) + bounds = {item[0]: item[1:] for item in report.get("geometry_uncertainty", ())} + if joint not in bounds: + raise ValueError("parent_reference_source_uncertainty_missing") + source = SourceHinge(geometry, dict(model.evidence_ids)[geometry.relation.child_role], + report["image_model_sha256"], *bounds[joint]) + return (source,), source_frame_sha256(self.verified[(epoch, task.key)]) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py new file mode 100644 index 0000000..2d1fb6d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parent_reference_evidence.py @@ -0,0 +1,143 @@ +"""Validate transferred installation geometry against its independent sources.""" + +from ..core.domain.reference import JointZeroSample, read_joint_zero_references +from ..core.geometry.image_motion_replay import frozen_image_model, replay_image_models, validate_image_sample +from .parent_reference import check_parent_pose + + +def same_capture_epoch(records, source, target, source_stamp, target_stamp): + """Only journalled internal zero retries may carry existing installations.""" + if source == target: + return True + links = [row for row in records + if row.get("kind") == "joint_zero_recovery" + and row.get("action") in {"repeat_approach", "retry_zero_images"} + and source_stamp < row.get("stamp_ns", 0) < target_stamp] + if not (type(source) is int and type(target) is int and 0 < source < target): + return False + for epoch in range(source, target): + candidates = [row for row in links if row.get("previous_session_epoch") == epoch] + if len(candidates) != 1: + return False + link = candidates[0] + if link.get("session_epoch") != epoch+1 or source_stamp >= link["stamp_ns"]: + return False + source_stamp = link["stamp_ns"] + return True + + +def source_epoch_matches(records, source, target_epoch, identity, target_stamp): + """Bind reused model identity through an explicit passed-task restoration.""" + source_stamp = max(source["source_image_stamps"]) + if same_capture_epoch(records, source["session_epoch"], target_epoch, source_stamp, target_stamp): + return True + for row in records: + if (row.get("kind") != "passed_pose_models_restored" or row.get("resume_mode") != "passed" + or row.get("unchanged_installation_declared") is not True + or not source_stamp < row.get("stamp_ns", 0) < target_stamp + or not same_capture_epoch(records, row.get("session_epoch"), target_epoch, + row["stamp_ns"], target_stamp)): + continue + if any(item.get("source_epoch") == source["session_epoch"] + and item.get("image_model_sha256") == source["image_model_sha256"] + and item.get("task_name") == source["task_name"] + and item.get("evidence_ids", {}).get(source["tag_role"]) == identity + for item in row.get("models", ())): + return True + return False + + +def validate_source_hinges(evidence, records=()): + for payload in evidence.values(): + if not payload.get("frozen_image_model"): + continue + model = frozen_image_model(payload) + for source in getattr(model, "source_hinges", ()): + original = evidence.get(source.evidence_id) + if (original is None or original.get("image_model_sha256") != source.model_sha256 + or original["view"] != payload["view"] + or not source_epoch_matches(records, original, payload["session_epoch"], + source.evidence_id, min(payload["source_image_stamps"])) + or max(original["source_image_stamps"]) >= min(payload["source_image_stamps"])): + raise ValueError("motion_branch_provenance_source_hinge_unbound") + geometry = {item.relation.joint: item for item in frozen_image_model(original).geometry} + bounds = {item[0]: tuple(item[1:]) for item in original.get("geometry_uncertainty", ())} + joint = source.geometry.relation.joint + if (geometry.get(joint) != source.geometry + or original["tag_role"] != source.geometry.relation.child_role + or bounds.get(joint) != (source.child_axis_uncertainty_rad, source.child_point_uncertainty_m)): + raise ValueError("motion_branch_provenance_source_hinge_changed") + + +def validate_parent_references(profile, records, evidence, *, source_model=None): + from .motion_provenance import source_frame_sha256 + from ..profiles.observations import compile_tag_feedback_channels + + tasks = {task.key: task for task in profile.motion.tasks} + checks = {source_frame_sha256(row): row for row in records if row.get("kind") == "parent_reference_verified"} + samples = {(row.get("view"), str(row.get("image_stamp_ns"))): row for row in records + if row.get("kind") == "joint_zero_sample"} + frames = {(row.get("view"), row.get("image_stamp_ns")): row for row in records + if row.get("kind") == "pnp_candidate_frame"} + checked = set() + channels = None if source_model is None else compile_tag_feedback_channels(profile, source_model) + for payload in evidence.values(): + model = frozen_image_model(payload) if payload.get("frozen_image_model") else None + if not getattr(model, "source_hinges", ()): + if payload.get("parent_reference_sha256"): + raise ValueError("parent_reference_unexpected_binding") + continue + identity = payload.get("parent_reference_sha256") + check = checks.get(identity) + task = tasks.get(payload["task_name"]) + if (check is None or task is None or task.parent_reference is None + or any(check.get(key) != payload.get(key) for key in ("task_name", "view")) + or not same_capture_epoch(records, check.get("session_epoch"), payload["session_epoch"], + max(map(int, check.get("source_frame_hashes", {"0": ""}))), min(payload["source_image_stamps"])) + or check.get("pose_joint") != task.parent_reference.pose_joint + or {s.geometry.relation.joint for s in model.source_hinges} != {task.parent_reference.geometry_joint} + or check.get("motion_version", 0) >= payload["motion_version"]): + raise ValueError("parent_reference_source_check_missing_or_changed") + if identity in checked: + continue + checked.add(identity) + name = task.parent_reference.pose_joint + spec = profile.measurement.measurements[name] + pose_source = evidence.get(check.get("pose_evidence_id")) + if (pose_source is None or pose_source["view"] != spec.view + or pose_source["tag_role"] != spec.child_role + or not source_epoch_matches(records, pose_source, check["session_epoch"], + check["pose_evidence_id"], min(map(int, check.get("source_frame_hashes", {"0": ""}))))): + raise ValueError("parent_reference_pose_source_unbound") + rows = [] + for stamp, digest in check.get("source_frame_hashes", {}).items(): + row = samples.get((spec.view, stamp)) + if (row is None or source_frame_sha256(row) != digest or row.get("joint") != name + or row.get("task_name") != task.key or int(stamp) >= min(payload["source_image_stamps"]) + or int(stamp) <= max(pose_source["source_image_stamps"])): + raise ValueError("parent_reference_check_image_changed") + item = next((item for item in row.get("pnp_observation_evidence", {}).values() + if item.get("tag_role") == spec.child_role), {}) + if (item.get("candidate_diagnostics", {}).get("motion_evidence_id") != check["pose_evidence_id"] + or item.get("image_model_sha256") != pose_source.get("image_model_sha256")): + raise ValueError("parent_reference_check_model_changed") + frame = frames.get((spec.view, int(stamp))) + if frame is None: + raise ValueError("parent_reference_check_pixels_missing") + validate_image_sample(row, frame, replay_image_models(frame)) + rows.append(row) + original = read_joint_zero_references(profile, [check["source_reference"]]).get(name) + if original is None: + raise ValueError("parent_reference_zero_source_changed") + for sample in original.samples: + row = samples.get((sample.view, str(sample.image_stamp_ns))) + frame = frames.get((sample.view, sample.image_stamp_ns)) + if (row is None or frame is None or JointZeroSample.from_row(row, profile.command.unit) != sample): + raise ValueError("parent_reference_zero_pixels_missing_or_changed") + validate_image_sample(row, frame, replay_image_models(frame)) + held = check.get("held_channels") + if (not isinstance(held, list) or not held or held != sorted(set(held)) + or any(type(index) is not int or not 0 <= index < profile.command.command_count for index in held) + or channels is not None and set(held) != channels[spec.child_role]): + raise ValueError("parent_reference_held_channels_changed") + check_parent_pose(profile, task, original, rows, held) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/passed_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/passed_resume.py new file mode 100644 index 0000000..04832c5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/passed_resume.py @@ -0,0 +1,98 @@ +"""Reuse durable task passes under an explicit unchanged-installation policy. + +No historical image solver, sampling-quality calculation or physical zero +reverification runs here. New acquisitions and final artifact validation keep +their normal gates. An incomplete task is restarted as a whole. +""" + +from dataclasses import dataclass + +from ..core.domain.motion_path import record_scan_identity +from ..core.domain.reference import read_joint_zero_references +from .joint_resume import ResumeJournalImport, _index_motion_provenance + + +@dataclass +class PassedReferenceImport(ResumeJournalImport): + references: dict + model_records: tuple + + +def stage_passed_tasks(resume, engine, rows): + references = read_joint_zero_references(resume.profile, rows) + latest, support = {}, {} + for index, row in enumerate(rows): + if row.get("kind") == "scan_unit_complete": + key = record_scan_identity(row) + previous = latest.get(key) + if previous is None or int(row.get("attempt", 1)) >= int(previous[1].get("attempt", 1)): + latest[key] = index, row + elif row.get("kind") == "task_input_support_checked": + support[row.get("task_name")] = index, row.get("passed") is True + units, joints = set(), set() + for task in resume.profile.motion.tasks: + expected = {unit.identity for unit in engine.scan_units() if unit.task_key == task.key} + if (not expected or not set(task.joints) <= references.keys() + or any(latest.get(key, (-1, {}))[1].get("passed") is not True for key in expected) + or support.get(task.key, (-1, False))[1] is not True + or support[task.key][0] < max(latest[key][0] for key in expected)): + continue + units.update(expected) + joints.update(task.joints) + resume.references = {name: references[name] for name in joints} + resume.verified = set(joints) + resume.units, resume.rows = units, tuple(rows) + + +def reference_import(resume): + """Import prior evidence durably before skipping any completed task.""" + provenance = _index_motion_provenance(resume.rows) + references = resume.references + parent_images = {(row["view"], int(stamp)) for row in provenance.values() + if row.get("kind") == "parent_reference_verified" for stamp in row["source_frame_hashes"]} + parent_images.update((sample["view"], sample["image_stamp_ns"]) + for row in provenance.values() if row.get("kind") == "parent_reference_verified" + for sample in row["source_reference"]["samples"]) + zero_images = parent_images | {(row.get("view"), row.get("image_stamp_ns")) + for row in provenance.values() if row.get("kind") == "joint_zero_sample" and row.get("joint") in references} + selected = [] + for row in provenance.values(): + image = row.get("view"), row.get("image_stamp_ns") + if (row.get("kind") in {"joint_zero_sample", "joint_zero_motion_reference_verified"} + and row.get("joint") not in references and image not in parent_images): + continue + if row.get("kind") == "pnp_candidate_frame" and image not in zero_images: + continue + selected.append(row) + tasks = {reference.task_key for reference in references.values()} + models = tuple(row for row in selected if row.get("kind") == "motion_branch_initialization" + and row.get("status") == "resolved" and row.get("task_name") in tasks) + audit = {"kind": "passed_tasks_reused", "resume_mode": "passed", + "unchanged_installation_declared": True, "tasks": sorted(tasks), + "joints": sorted(references), "completed_units": [list(key) for key in sorted(resume.units)], + "historical_replay_performed": False, "physical_zero_reverification_performed": False} + return PassedReferenceImport(rows=(*selected, + *(reference.as_record() for reference in references.values()), audit), + references=references, model_records=models) + + +def restore_parent_models(registry, records, epoch): + """Keep source identities while making passed installations available to new tasks.""" + from ..core.geometry.image_motion_replay import frozen_image_model + from .branch_initialization import MotionBranchModel + + for row in records: + if "frozen_image_model" not in row: + continue + model = MotionBranchModel(row["task_name"], row["view"], (), + tuple(sorted(row["evidence_ids"].items())), frozen_image_model(row)) + registry.remember(model, row, epoch=epoch) + + +def restored_model_record(records, epoch, stamp_ns): + """Bind imported evidence to this session's declared unchanged installation.""" + return dict(kind='passed_pose_models_restored', session_epoch=epoch, stamp_ns=stamp_ns, + resume_mode='passed', unchanged_installation_declared=True, + models=[dict(task_name=row['task_name'], source_epoch=row['session_epoch'], + image_model_sha256=row['image_model_sha256'], evidence_ids=row['evidence_ids']) + for row in records if 'frozen_image_model' in row]) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py index 726bdf5..56dfc14 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/pose_evidence.py @@ -54,6 +54,9 @@ def snapshot_pose_evidence( # A shared hinge model produces a current-image constrained pose; # it must never be described as one of the raw IPPE candidates. source = "image_constrained_current_image" + if diagnostics.get("reference_frame") is not None: + from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE + source = IMAGE_REFERENCE_SOURCE evidence[role] = { "tag_role": role, "tag_id": tag.tag_id, @@ -70,6 +73,10 @@ def snapshot_pose_evidence( if source == "image_constrained_current_image": evidence[role].update(frozen_image_model=diagnostics["frozen_image_model"], image_model_sha256=diagnostics.get("image_model_sha256")) + if diagnostics.get("reference_frame") is not None: + evidence[role].update(reference_frame=diagnostics["reference_frame"], + reference_frame_sha256=diagnostics["reference_frame_sha256"], + reference_branch_revision=diagnostics["branch_revision"]) if role in cached_roles: evidence[role]["reference_branch_revision"] = (reference_revisions or {})[role] elif source == "verified_fixed_reference": @@ -96,6 +103,12 @@ def reference_branch_revisions(rows: Sequence[Mapping], *, profile, if item.get("tag_role") != role: raise ValueError("joint_zero_branch_tag_binding_changed") source = item["pose_source"] + from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence + if source == IMAGE_REFERENCE_SOURCE: + if not any(tag.role == role and tag.fixed_reference for view in profile.vision.views + if view.name == row["view"] for tag in view.tags): + raise ValueError("joint_zero_image_reference_is_not_fixed") + reference_from_evidence(item, stamp_ns=row["image_stamp_ns"]) if source in {"cached_fixed_reference", "verified_fixed_reference"}: if not any(tag.role == role and tag.fixed_reference for view in profile.vision.views if view.name == row["view"] for tag in view.tags): 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 a9c5dff..6b1383f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -11,9 +11,24 @@ 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 reason.startswith("training_quality_failed"): + return ("FIT-TRAINING-511", "第三轮训练后质量仍不合格，本任务已结束，未进入独立验证或发布。", + "查看 training_decision 的重复性、几何依赖和零位置信区间；保留原始日志，先分析原因。") + if reason.startswith("training_assessment_failed"): + return ("FIT-TRAINING-WORKER-512", "训练预检未能在规定时间内完成，会话已暂停。", + "查看原始异常和阶段耗时；已采数据保留，不会继续增加训练轮次或更换冻结模型。") + if "shared_pose" in reason: + return ("OBS-SHARED-POSE-116", f"共享标签在当前基准姿态下尚不能可靠继承已确认的姿态约束：{reason}", + "查看共享标签检查中的静止图像、相关通道反馈及各候选姿态差；已通过的关节数据保留。证据仍不充分时需要补充有效观测，原样重复扫描不保证消除歧义。") + if "parent_reference_" in reason: + return ("OBS-PARENT-REFERENCE-115", f"父标签的当前姿态或原安装证据未通过复核：{reason}", + "保留诊断，核对保持指令、反馈稳定性、当前图像与模型来源；不会额外转动父关节或替换原零位。若标签安装发生变化，需要重新采集相关关节。") if "tag_installation_not_rigid:" in reason: return ("FIT-RIGID-CHAIN-510", "采集已完成，但实测 Tag 轨迹与整条 URDF 运动链未通过刚性一致性验证。", "查看 tag_installation_diagnostics.json 中全部 Tag 的角度和位置残差，核对坐标转换、视觉重建和模型几何；这不等于已确认硬件松动，不应直接重复整场采集。") + if "image_motion_families_not_distinguishable" in reason: + return ("OBS-POSE-AMBIGUOUS-117", "现有图像仍可由多个不同的三维姿态解释，无法可靠选择唯一结果。", + "补充当前关节的独立观测，或改善尚未通过关节标签的视角；已通过的数据保留。原样重复同一段运动不保证消除歧义，不能只取像素误差略小的解。") if reason.startswith("joint_zero_motion_unresolved:"): return ("OBS-JOINT-BRANCH-111", f"归零准备运动未能可靠确认当前关节的姿态分支：{reason}", "查看所列关节、机位及 motion_branch_initialization 记录；检查 Tag 刚性、完整可见性和运动几何证据。保持当前位置，不把分数较低的候选直接当作真值，不覆盖已冻结零位。") @@ -42,6 +57,10 @@ def reason_zh( return ("FIT-COMMAND-MAPPING-506", "稳态指令与独立视觉观测不足或不一致，已禁止发布指令映射。", "查看具体关节、方向、稳态点及诊断；反馈曲线不能替代缺失的指令标定。") live_failures = { + "waiting_for_visual_motion_tags": ("OBS-MOTION-112", "运动前缺少所需关节的新鲜 Tag 图像。", "保持当前指令，检查所列关节的运动 Tag 和基准 Tag 是否完整可见。"), + "visual_motion_unavailable": ("OBS-MOTION-112", "运动观测所需的 Tag 图像持续缺失或过期。", "已暂停；检查所列关节对应的 Tag 和相机。"), + "visual_motion_not_observed": ("OBS-MOTION-113", "已下发明显运动，但图像未观察到对应关节运动。", "检查实际机构、指令响应及 Tag；该判断不使用位置反馈数值，也不能单独确定硬件原因。"), + "visual_motion_target_unconfirmed": ("OBS-MOTION-114", "图像尚未确认稳定到位或回到已冻结零位。", "已暂停；检查图像抖动、遮挡与实际姿态，不跳过确认继续释放避让。"), "duplicate_controller": ("DEVICE-COMMAND-CONFLICT-204", "检测到另一个命令控制节点。", "退出 GUI 或其他控制节点，只保留本次标定。"), "fixed_reference_moved": ("OBS-BASE-DRIFT-105", "已锁定的固定基准连续 10 帧漂移超过 5 px。", "检查手掌、相机、支架和固定 Tag；重新固定后开始新采集。"), "sdk_disconnected": ("DEVICE-COMMUNICATION-202", "SDK 通信或健康报告持续失效。", "检查供电、连接及 SDK 日志；不要直接重复强推动作。"), @@ -138,9 +157,11 @@ def reason_zh( "停止 GUI、手动控制节点或其他标定进程，只保留当前标定命令。", ) if reason.startswith("mechanical_stall:"): + timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason) + duration = f" {timeout.group(1)} 秒" if timeout else "两秒" return ( "MOTION-STALL-303", - "目标电机连续两秒没有向目标推进，程序已保持当前位置。", + f"目标电机连续{duration}没有向目标推进，程序已保持当前位置。", "检查碰撞、摩擦和机械端点；不要连续重启强推。", ) if reason.startswith("motion_timeout:"): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py index a300be8..1e3ceda 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py @@ -12,18 +12,44 @@ from typing import Any, Mapping, Sequence import numpy as np from ..core.geometry.pnp import POSE_TRACKING_POLICY_VERSION -from .engine import ACQUISITION_POLICY_VERSION, CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION +from .engine import ACQUISITION_POLICY_VERSION, SUPPORTED_CAPTURE_SCHEDULE_VERSIONS from .diagnostic_capture import reject_diagnostic_capture +from ..core.domain.motion_path import record_scan_identity + + +def _compatible_header(header, profile_id, serial_number, serial_fallback, protected_hashes): + model, side, layout, _revision = profile_id.split("/") + recorded_id = header.get("profile_id") + if recorded_id is None: + # Read-only legacy journal spelling; the profile hash remains + # mandatory and identifies the exact YAML contract. + if (header.get("model"), header.get("hand_type"), header.get("tag_layout")) != (model, side, layout): + return False + elif recorded_id != profile_id: + return False + if header.get("serial_number", serial_fallback) != serial_number: + return False + if header.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION: + return False + if header.get("capture_schedule_version") not in SUPPORTED_CAPTURE_SCHEDULE_VERSIONS: + return False + if header.get("pose_tracking_policy_version") != POSE_TRACKING_POLICY_VERSION: + return False + recorded = header.get("protected_hashes", header) + if not isinstance(recorded, Mapping) or any(recorded.get(key) != value for key, value in protected_hashes.items()): + return False + return True def discover_resume_candidate( session_root: Path, *, profile_id: str, serial_number: str, protected_hashes: Mapping[str, str], + training_policy: str = "fixed", ) -> Path | None: """Locate evidence, never authorize its reuse or publish from a checkpoint. - The online owner must still verify the current physical reference and every - reused Tag installation. A newer empty/truncated attempt cannot shadow an + The online owner applies the product's declared reuse policy. A newer + empty/truncated attempt cannot shadow an older complete capture. Old source files are never modified. """ if not protected_hashes or any(re.fullmatch(r"[0-9a-f]{64}", value) is None @@ -36,7 +62,7 @@ def discover_resume_candidate( pointer.resolve() for pointer in root.iterdir() if pointer.name.startswith("latest_") and "passed" in pointer.name and pointer.is_symlink() } - model, side, layout, _revision = profile_id.split("/") + best, best_completed = None, -1 for candidate in sorted(root.iterdir(), key=lambda path: path.name, reverse=True): if candidate.is_symlink() or not candidate.is_dir() or candidate.name.startswith("latest_"): continue @@ -45,6 +71,7 @@ def discover_resume_candidate( start = None reference_found = False completed_found = False + completed = {} try: summary_path = candidate / "calibration_summary_zh.json" if summary_path.exists(): @@ -67,43 +94,42 @@ def discover_resume_candidate( if start is not None: raise ValueError("multiple checkpoint session headers") start = row + plan = start.get("capture_plan", {}) + if not isinstance(plan, Mapping) or plan.get("policy", "fixed") != training_policy: + raise ValueError("checkpoint training policy differs") + if not _compatible_header(start, profile_id, serial_number, root.name, protected_hashes): + raise ValueError("checkpoint header incompatible") if kind == "fixed_base_reference_locked": reference_found = True if kind == "scan_unit_complete" or kind.endswith("sweep_observation_quality"): # Old direction records used an explicit empty failure # list instead of a boolean. This merely finds a source; - # the node still validates every imported unit/view. + # the node applies the selected checkpoint reuse policy. passed = row.get("passed") is True or ( "passed" not in row and row.get("failures") == [] and int(row.get("valid_frames", 0)) >= 40 and int(row.get("feedback_bins", 0)) >= 32 ) completed_found = completed_found or passed + identity = record_scan_identity(row) + attempt = int(row.get("attempt", 1)) + previous = completed.get(identity) + if previous is None or attempt >= previous[0]: + completed[identity] = (attempt, passed) except (OSError, ValueError): continue if start is None or not reference_found or not completed_found: continue - recorded_id = start.get("profile_id") - if recorded_id is None: - # Read-only legacy journal spelling; the profile hash remains - # mandatory and identifies the exact YAML contract. - if (start.get("model"), start.get("hand_type"), start.get("tag_layout")) != (model, side, layout): - continue - elif recorded_id != profile_id: - continue - if start.get("serial_number", root.name) != serial_number: - continue - if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION: - continue - if start.get("capture_schedule_version") not in {CAPTURE_SCHEDULE_VERSION, SEPARATE_CAPTURE_SCHEDULE_VERSION}: - continue - if start.get("pose_tracking_policy_version") != POSE_TRACKING_POLICY_VERSION: - continue - recorded = start.get("protected_hashes", start) - if not isinstance(recorded, Mapping) or any(recorded.get(key) != value for key, value in protected_hashes.items()): - continue - return candidate - return None + count = sum(passed for _attempt, passed in completed.values()) + if count > best_completed: + best, best_completed = candidate, count + # A resumed process may stop after importing only its first task. + # Prefer the most complete checkpoint in the same restart lineage; + # equal coverage keeps the newest record. Never reach across an + # explicit fresh capture to resurrect superseded physical evidence. + if start.get("resume_checkpoint_requested") is False: + break + return best @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/resume_storage.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume_storage.py new file mode 100644 index 0000000..fb2398a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume_storage.py @@ -0,0 +1,84 @@ +"""Bounded memory for passed-task imports; original image bytes stay on disk. + +Startup needs scan identities, persisted passes and reference provenance. Large +per-image payloads are only needed when a bounded import batch is written. Each +deferred row is bound to the exact bytes read during indexing, so replacing or +editing a checkpoint cannot silently change an import after it was prepared. +""" + +from collections.abc import Mapping +import hashlib +import json +from pathlib import Path + +from ..core.geometry.frozen_evidence import EvidenceDecoder + + +class DeferredCaptureRecord(Mapping): + __slots__ = ("path", "offset", "length", "digest", "metadata", "field_names", "decoder") + + def __init__(self, path, offset, line, row, decoder): + self.path, self.offset, self.length = path, offset, len(line) + self.digest = hashlib.sha256(line).digest() + self.metadata = {k: v for k, v in row.items() + if v is None or isinstance(v, (str, int, float, bool))} + self.field_names = tuple(row) + self.decoder = decoder + + def materialize(self): + with self.path.open("rb") as stream: + stream.seek(self.offset) + line = stream.read(self.length) + if hashlib.sha256(line).digest() != self.digest: + raise ValueError("resume_source_record_changed") + return self.decoder.loads(line) + + def __getitem__(self, key): + if key in self.metadata: + return self.metadata[key] + if key not in self.field_names: + raise KeyError(key) + return self.materialize()[key] + + def __iter__(self): + return iter(self.field_names) + + def __len__(self): + return len(self.field_names) + + +def materialize_record(row): + return row.materialize() if isinstance(row, DeferredCaptureRecord) else row + + +def load_passed_capture(path): + """Read once without retaining every scan's expanded image dictionaries. + + No historical image fitting or hash verification is introduced. Provenance + remains eager, while per-image records retain their full field interface. + Final artifact replay still reads and verifies the complete journal. + """ + path = Path(path).resolve() + decoder = EvidenceDecoder(verify_hashes=False) + rows = [] + deferred_kinds = {"joint_sample", "secondary_joint_sample", "pnp_candidate_frame"} + with path.open("rb") as stream: + number = 0 + while True: + offset = stream.tell() + line = stream.readline() + if not line: + break + number += 1 + if not line.strip(): + continue + try: + row = decoder.loads(line) + except json.JSONDecodeError as error: + raise ValueError(f"invalid calibration JSONL at line {number}") from error + if not isinstance(row, dict): + raise ValueError(f"calibration JSONL record is not an object at line {number}") + if row.get("kind") in deferred_kinds: + row = DeferredCaptureRecord(path, offset, line, row, decoder) + rows.append(row) + return rows diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py index 595b2cc..196849e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py @@ -1,5 +1,7 @@ """ROS identity, parameters and callback wiring for the shared coordinator.""" +from contextlib import contextmanager +import gc import time from rclpy.executors import SingleThreadedExecutor @@ -23,15 +25,21 @@ class UnifiedCalibrationNode(Node): super().__init__(f"{profile.key.model.lower()}_calibration") for name, default in parameter_defaults(profile).items(): self.declare_parameter(name, default) + from ...core.domain.capture_plan import configure_training + profile = configure_training(profile, self.get_parameter("training_policy").value) parameters = load_runtime_parameters(profile, lambda name: self.get_parameter(name).value) self.io = RosCalibrationIO(self, profile, parameters) + from ..timing_diagnostics import RuntimeTimingDiagnostics + self.timing_diagnostics = RuntimeTimingDiagnostics(parameters.session_dir / "runtime_timing_diagnostic.jsonl") ports = RuntimePorts(lambda: int(self.get_clock().now().nanoseconds), time.monotonic, lambda: self.count_publishers(parameters.command_topic), self.io.publish_position, self.io.publish_setting) def subscribe_health(receive): + from ..adapters.ros_topics import sdk_topics + topic = sdk_topics(profile).health or parameters.command_topic.rsplit("/", 1)[0]+"/calibration_health" self.create_subscription(String, - parameters.command_topic.rsplit("/", 1)[0]+"/calibration_health", + topic, lambda message: receive(message.data), 10) def adapter_factory(publish, set_speed, feedback_fresh): @@ -105,19 +113,43 @@ class UnifiedCalibrationNode(Node): def _tick(self): self.detection_dispatch.raise_if_failed() - self.coordinator.tick() + started = time.perf_counter() + try: + self.coordinator.tick() + finally: + self.timing_diagnostics.record("control_tick", started) def _publish_status(self): self.io.publish_status(self.coordinator.snapshot()) + self.timing_diagnostics.flush() def destroy_node(self): self.detection_dispatch.close() self.branch_solver.close() self.projection_worker.close() self.coordinator.close() + self.timing_diagnostics.close() return super().destroy_node() +@contextmanager +def _frozen_startup_graph(): + """Exclude the resident checkpoint from cyclic scans during live callbacks. + + This dedicated process owns its collector lifecycle. Preparation can load + hundreds of MB of validated, long-lived records; revisiting that graph in + generation 2 can block feedback for seconds. New live objects still receive + normal cyclic collection, and ordinary reference counting stays enabled. + Restore the startup graph only after the executor and node have stopped. + """ + gc.collect() + gc.freeze() + try: + yield + finally: + gc.unfreeze() + + def run_profile_node(profile, args=None): import rclpy rclpy.init(args=args) @@ -127,9 +159,14 @@ def run_profile_node(profile, args=None): # compete for the same session lock and delay feedback and command ticks. executor = SingleThreadedExecutor() executor.add_node(node) - try: - executor.spin() - finally: - executor.shutdown() - node.destroy_node() - rclpy.shutdown() + with _frozen_startup_graph(): + try: + executor.spin() + finally: + try: + executor.shutdown() + finally: + try: + node.destroy_node() + finally: + rclpy.shutdown() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py index c9ee945..ed5fc82 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py @@ -28,6 +28,9 @@ def parameter_defaults(profile) -> dict[str, Any]: "sdk_package_expected_sha256": "", "profile_config_expected_sha256": "", "resume_raw_samples_path": "", + "resume_mode": "verify", + "training_policy": profile.quality.training_policy, + "initial_command_file": "", "command_topic": f"/{model}/cb_right_hand_control_cmd", "state_topic": f"/{model}/cb_right_hand_state", "setting_topic": f"/{model}/cb_hand_setting_cmd", @@ -60,6 +63,10 @@ def parameter_defaults(profile) -> dict[str, Any]: defaults[f"{view}_detections_topic"] = ( f"{namespace}/{view}/apriltag/detections" ) + if profile.sdk_adapter == "o30_ros": + from ..adapters.ros_topics import sdk_topics + topics = sdk_topics(profile) + defaults.update(command_topic=topics.command, state_topic=topics.feedback, setting_topic=topics.setting) return defaults @@ -145,6 +152,11 @@ def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimePara raise ValueError("command_rate_hz must be in (0, 200]") if not 0 <= settle <= 5: raise ValueError("speed_settle_seconds must be in [0, 5]") + from ..adapters.command_seed import load_command_seed + initial_command, device_uid = load_command_seed(str(value("initial_command_file")), profile) + resume_mode = str(value("resume_mode")) + if resume_mode not in {"verify", "passed"}: + raise ValueError("resume_mode must be verify or passed") return RuntimeParameters( serial_number=serial_number, session_dir=session_dir, source_urdf=source_urdf, camera_extrinsics_file=camera_extrinsics_file, extrinsics=extrinsics, @@ -158,6 +170,8 @@ def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimePara float(value("minimum_decision_margin")), float(value("minimum_edge_pixels"))), tracking=tracking_parameters(value), diagnostic_capture=diagnostic_capture, + initial_command=initial_command, initial_device_uid=device_uid, + resume_mode=resume_mode, ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 6d73682..8d512f6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -8,9 +8,11 @@ from __future__ import annotations import argparse import json +import math import os from pathlib import Path import subprocess +import sys import time from typing import Any, Callable, Mapping @@ -103,9 +105,10 @@ def _write_failure(config: ProductConfig, session: Path, status: Mapping[str, An def run_online( config: ProductConfig, *, record_bag: bool = False, commands_enabled: bool = True, allow_resume: bool = True, - diagnostic_capture: str = "", + diagnostic_capture: str = "", startup_timeout: float = 120.0, ) -> int: import rclpy + from rclpy.signals import SignalHandlerOptions from std_srvs.srv import Trigger from .artifacts.completion import finish_online_artifacts @@ -131,6 +134,7 @@ def run_online( resume = discover_resume_candidate( config.session_root, profile_id=profile.key.profile_id, serial_number=config.serial_number, protected_hashes=protected_inputs(config), + training_policy=profile.quality.training_policy, ) if allow_resume and commands_enabled and diagnostic is None else None if contract_report["cad_constraint_conflicts"]: print("原始 CAD 存在限位/mimic 冲突，未修改原文件；详见 measurement_contract.json。最终范围验证不通过时禁止发布。", flush=True) @@ -145,7 +149,10 @@ def run_online( environment = overlay_environment(config.sdk_setup) if config.sdk_setup else dict(os.environ) environment.setdefault("ROS_LOG_DIR", str(session / "ros_logs")) if owns_context: - rclpy.init() + # Keep Python's SIGINT handler: Ctrl+C must request a hold + # while ROS is still alive, before tearing down the stack. + # SIGTERM still uses rclpy's orderly context shutdown. + rclpy.init(signal_handler_options=SignalHandlerOptions.SIGTERM) progress = ProgressConsole(lambda status, estimator: render_status_zh( normalize_status(status, profile=profile, serial_number=config.serial_number), estimator, @@ -166,7 +173,10 @@ def run_online( raise RuntimeError(f"existing_calibration_publishers:{occupied}") print(f"{config.serial_number} 标定环境正在启动；日志：{log_path}", flush=True) if resume is not None: - print(f"发现候选断点：{resume.name}；尚未复用，启动后验证基准和 Tag 安装关系。", flush=True) + if getattr(config, "resume_mode", "verify") == "passed": + print(f"发现断点：{resume.name}；按已通过任务续采，跳过历史回放和已完成关节零位复核。", flush=True) + else: + print(f"发现候选断点：{resume.name}；尚未复用，启动后验证基准和 Tag 安装关系。", flush=True) if not commands_enabled: print("仅预览：不自动开始、不发送标定运动；Ctrl+C 退出。", flush=True) if diagnostic is not None: @@ -176,8 +186,17 @@ def run_online( f"每个方向扫描后回到该方向起点，按方向依次到达 {values}，各保持 {diagnostic.hold_seconds:g} 秒；" "不复用断点、不拟合或发布标定产物。", flush=True) else: - print("拇指短程诊断：仅执行拇指任务各一次往返，保留准备和零位采集；不复用断点、不拟合或发布标定产物。", flush=True) + print(f"{diagnostic_capture_label(diagnostic.mode)}：仅执行声明任务各一次往返，保留准备和零位采集；不复用断点、不拟合或发布标定产物。", flush=True) launch_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} + if commands_enabled and profile.vision_motion: + if profile.sdk_adapter != "o30_ros": + raise ValueError("visual_motion_requires_native_command_seed_adapter") + seed = session / "initial_command.json" + subprocess.run([sys.executable, "-m", "linkerhand_calibration.runtime.adapters.command_seed", + "--side", profile.key.side, "--output", str(seed)], + cwd=config.workspace, env=environment, stdout=log_stream, + stderr=subprocess.STDOUT, check=True, timeout=20) + launch_options["initial_command_file"] = seed process = subprocess.Popen( launch_command(config, session, record_bag=record_bag, commands_enabled=commands_enabled, resume_from=resume, @@ -189,6 +208,7 @@ def run_online( monitor, process, spin=lambda: rclpy.spin_once(monitor, timeout_sec=0.1), ok=rclpy.ok, request_start=lambda: monitor.start_client.call_async(Trigger.Request()), auto_start=commands_enabled, + startup_timeout=startup_timeout, ) atomic_write_json(session / "node_status.json", status) if diagnostic is not None: @@ -231,9 +251,12 @@ def run_online( print(f"{name}：{path}", flush=True) return 0 except KeyboardInterrupt: - if not capture_complete and process is not None and monitor is not None and monitor.abort_client.service_is_ready(): - monitor.abort_client.call_async(Trigger.Request()) - rclpy.spin_once(monitor, timeout_sec=0.2) + if (not capture_complete and process is not None and monitor is not None + and rclpy.ok() and monitor.abort_client.service_is_ready()): + aborted = monitor.abort_client.call_async(Trigger.Request()) + 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 "已请求中止" print(f"{message}；本次数据保留在 {session}。", flush=True) return 130 @@ -264,17 +287,29 @@ def main(args: list[str] | None = None) -> None: parser.add_argument("--record-bag", action="store_true") parser.add_argument("--commands-disabled", action="store_true") parser.add_argument("--no-resume", action="store_true") + parser.add_argument("--training-policy", choices=("fixed", "adaptive_2_to_3"), default=None, + help="试验采集策略：两轮训练，必要时补第三轮；保留独立验证，默认沿用配置") + parser.add_argument("--startup-timeout-seconds", type=float, default=None, + help="等待节点初始化及设备就绪的秒数（默认 120）；大断点逐帧校验可显式延长，不改变运动保护") diagnostic_options = parser.add_mutually_exclusive_group() diagnostic_options.add_argument("--diagnostic-thumb", action="store_true", help="仅 O6 右手：拇指任务各采一次往返，只保存诊断数据") diagnostic_options.add_argument("--diagnostic-yaw-hold", action="store_true", help="仅 O6 右手：yaw 两方向扫描后各在 64/128/192 保持 10 秒，只保存诊断数据") + from ..profiles.diagnostics import DIAGNOSTIC_CAPTURE_PRESETS + diagnostic_options.add_argument("--diagnostic-capture", choices=sorted(DIAGNOSTIC_CAPTURE_PRESETS), + default="", help="执行已声明的诊断任务，只保存诊断数据，不发布标定产物") parser.add_argument("--offline-raw", default="") parser.add_argument("--offline-output", default="") parser.add_argument("--publish-offline", action="store_true") selected = parser.parse_args(args) + if selected.startup_timeout_seconds is not None and ( + not math.isfinite(selected.startup_timeout_seconds) or selected.startup_timeout_seconds <= 0 + ): + parser.error("--startup-timeout-seconds must be finite and positive") diagnostic_flag = ("--diagnostic-thumb" if selected.diagnostic_thumb else - "--diagnostic-yaw-hold" if selected.diagnostic_yaw_hold else "") + "--diagnostic-yaw-hold" if selected.diagnostic_yaw_hold else + "--diagnostic-capture" if selected.diagnostic_capture else "") if diagnostic_flag and (selected.offline_raw or selected.offline_output or selected.publish_offline or selected.commands_disabled): parser.error(f"{diagnostic_flag} cannot be combined with --offline-raw, --offline-output, --publish-offline or --commands-disabled") @@ -282,10 +317,21 @@ def main(args: list[str] | None = None) -> None: 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), ) + if selected.training_policy is not None: + from dataclasses import replace + from ..product import ProductCalibrationContract + from ..core.domain.capture_plan import configure_training + try: + profile = configure_training(config.calibration_contract.typed_profile, selected.training_policy) + config = replace(config, calibration_contract=ProductCalibrationContract(declarative=profile)) + except ValueError as error: + parser.error(str(error)) diagnostic_capture = ("o6_thumb" if selected.diagnostic_thumb else - "o6_yaw_hold" if selected.diagnostic_yaw_hold else "") + "o6_yaw_hold" if selected.diagnostic_yaw_hold else selected.diagnostic_capture) diagnostic = None if diagnostic_capture: + if config.calibration_contract.typed_profile.quality.training_policy != "fixed": + parser.error("adaptive training cannot be combined with diagnostic capture") from .diagnostic_capture import resolve_diagnostic_capture try: diagnostic = resolve_diagnostic_capture(config.calibration_contract.typed_profile, diagnostic_capture) @@ -294,6 +340,8 @@ def main(args: list[str] | None = None) -> None: if selected.validate_only: from ..profiles.validator import validate_executable_profile 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 diagnostic is not None: report["diagnostic_capture"] = {**diagnostic.as_dict(), "publication_allowed": False} print(json.dumps(report, ensure_ascii=False, indent=2)) @@ -308,6 +356,8 @@ def main(args: list[str] | None = None) -> None: if selected.offline_output or selected.publish_offline: parser.error("--offline-output/--publish-offline require --offline-raw") online_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} + if selected.startup_timeout_seconds is not None: + online_options["startup_timeout"] = selected.startup_timeout_seconds raise SystemExit(run_online(config, record_bag=selected.record_bag, commands_enabled=not selected.commands_disabled, allow_resume=not selected.no_resume and diagnostic is None, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py index 801674f..c5ce682 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py @@ -96,6 +96,7 @@ def launch_command( sdk_startup_speed_u8: int | None = None, resume_from: Path | None = None, diagnostic_capture: str = "", + initial_command_file: Path | None = None, ) -> list[str]: """Build the single launch invocation entirely from the product contract.""" arguments = { @@ -124,6 +125,8 @@ def launch_command( "sdk_package_expected_sha256": getattr(config, "sdk_package_sha256", ""), "record_bag": str(record_bag).lower(), "diagnostic_capture": diagnostic_capture, + "resume_mode": getattr(config, "resume_mode", "verify"), + "training_policy": config.calibration_contract.typed_profile.quality.training_policy, } startup_speed = sdk_startup_speed_u8 if startup_speed is None: @@ -134,6 +137,8 @@ def launch_command( arguments["resume_raw_samples_path"] = str( resume_from / "raw_samples.jsonl" ) + if initial_command_file is not None: + arguments["initial_command_file"] = str(initial_command_file) if config.can_interface: arguments["can_interface"] = config.can_interface for view, camera in config.cameras.items(): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py index 27143ac..3a2fc15 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py @@ -60,6 +60,8 @@ class SafetyPolicy: self._axis_progress: dict[int, tuple[float, float, float]] = {} def evaluate(self, sample: SafetySample) -> SafetyDecision: + if not math.isfinite(float(sample.now_seconds)): + return self._stop("invalid_control_clock", "控制时钟无效") if sample.operator_abort: return self._stop("operator_abort", "操作者已中止标定") if sample.competing_controller: @@ -82,7 +84,7 @@ class SafetyPolicy: "SDK 报告活动硬件故障", ",".join(health.active_faults), ) - if (sample.feedback is None or not health.feedback_fresh + if not self.profile.vision_motion and (sample.feedback is None or not health.feedback_fresh or sample.feedback_timestamp_seconds is None or not math.isfinite(float(sample.now_seconds)) or not math.isfinite(float(sample.feedback_timestamp_seconds)) @@ -92,11 +94,11 @@ class SafetyPolicy: return self._stop("feedback_stale", "反馈连续超过允许时间未更新") try: self._validate_vector(sample.command, feedback=False) - if sample.feedback is not None: + if sample.feedback is not None and not self.profile.vision_motion: self._validate_vector(sample.feedback, feedback=True) except ValueError as error: return self._stop("physical_range_exceeded", str(error)) - stall = self._evaluate_progress(sample) + stall = None if self.profile.vision_motion else self._evaluate_progress(sample) if stall is not None: return stall return SafetyDecision(True) @@ -152,10 +154,13 @@ class SafetyPolicy: if direction * (goal.end_feedback - feedback) <= goal.completion_tolerance or projected >= best + goal.progress_resolution: self._axis_progress[index] = (projected, now, direction) continue - if now - last_advance >= self.profile.acquisition.stall_timeout_seconds: + timeout = self.profile.acquisition.stall_timeout_seconds + if now - last_advance >= timeout: return self._stop("mechanical_stall", - "已要求明显运动，但目标反馈连续两秒没有向目标推进", - f"segment={motion_id},channel={index},feedback={feedback:.9g},goal={goal.end_feedback:.9g}") + f"已要求明显运动，但目标反馈连续 {timeout:g} 秒没有向目标推进", + f"segment={motion_id},channel={index},command={sample.command[index]:.9g}," + f"feedback={feedback:.9g},feedback_progress_goal={goal.end_feedback:.9g}," + f"timeout_seconds={timeout:g},no_progress_seconds={now-last_advance:.9g}") return None def _reset_progress(self) -> None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py index 41d354b..2a2373c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py @@ -5,7 +5,10 @@ from __future__ import annotations import math from typing import Mapping, Sequence -from .engine import CalibrationEngine, ScanUnit, SweepQuality +from .engine import CalibrationEngine, ScanUnit, SweepQuality, evaluate_observability +from .motion_execution import feedback_tolerances +from ..core.domain.motion_path import task_segment, record_scan_identity +from ..core.fitting.motion_fit import channel_for_joint from ..core.domain.profile import observation_streams as observation_streams @@ -23,7 +26,11 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, """ engine = CalibrationEngine(profile) task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + segment = task_segment(task, unit.segment_key) if unit.segment_key else None + streams = [s for s in observation_streams(profile, task) if segment is None or s[1] in segment.joints] native = profile.command.unit + command_coverage = profile.command_based_release and ( + profile.vision_motion or not profile.acquisition.feedback_travel_matches_command) lower, upper = sorted((task.start_value, task.end_value)) span = upper - lower if span <= 0: @@ -33,34 +40,59 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, from .steady import steady_failures, steady_targets failures = steady_failures(profile, unit, attempt, records) targets = steady_targets(profile, unit, records=records) - for field, name, spec in observation_streams(profile, task): + for field, name, spec in streams: rows = [r for r in records if r.get(field) == name and r.get("view") == spec.view and r.get("sample_phase") == "steady" and r.get("task_name") == unit.task_key and r.get("cycle") == unit.cycle and r.get("direction") == unit.direction - and int(r.get("attempt", 1)) == attempt] + and record_scan_identity(r) == unit.identity and int(r.get("attempt", 1)) == attempt] covered = {r["steady_index"] for r in rows} metrics[f"{field}:{name}:{spec.view}:{unit.direction}"] = { "quality_scope": "steady_command_nodes", "valid_frames": len({r["image_stamp_ns"] for r in rows}), "feedback_span": len(covered) / len(targets), "feedback_bins": len(covered), "maximum_bin_gap": 0, "required_nodes": len(targets)} + if command_coverage: + metrics = {key: _command_coverage(value, native) for key, value in metrics.items()} return SweepQuality(not failures, tuple(failures), (), metrics) - for field, name, spec in observation_streams(profile, task): + for field, name, spec in streams: + if segment: + channel = channel_for_joint(profile, name) + lower, upper = sorted((dict(segment.start_commands)[channel], dict(segment.end_commands)[channel])) + span = upper-lower identity = f"{field}:{name}:{spec.view}:{unit.direction}" - samples = {} + if segment: + identity += f":{segment.key}" + samples, command_samples, feedback_samples = {}, {}, {} + channel = channel_for_joint(profile, name) for row in records: if row.get("sample_phase", "sweep") != "sweep": continue if (row.get(field) != name or row.get("view") != spec.view or row.get("task_name") != unit.task_key or row.get("cycle") != unit.cycle or row.get("direction") != unit.direction - or int(row.get("attempt", 1)) != attempt): + or record_scan_identity(row) != unit.identity or int(row.get("attempt", 1)) != attempt): continue stamp = row.get("image_stamp_ns") if stamp is None or not isinstance(stamp, int) or stamp <= 0: continue - value = row.get(f"feedback_{native}") + feedback = row.get(f"feedback_{native}") + if not profile.vision_motion: + if (feedback is None or not math.isfinite(float(feedback)) or not + profile.command.minimum_feedback_values[channel] <= float(feedback) + <= profile.command.maximum_feedback_values[channel]): + continue + 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 + limits = ((profile.command.minimum_values, profile.command.maximum_values) if command_coverage + else (profile.command.minimum_feedback_values, profile.command.maximum_feedback_values)) + if not (limits[0][channel] <= float(value) <= limits[1][channel]): + continue + commanded = row.get(f"command_{native}") + if commanded is not None and math.isfinite(float(commanded)): + command_progress = (float(commanded) - lower) / span + if 0 <= command_progress <= 1: + command_samples[(spec.view, stamp)] = command_progress progress = (float(value) - lower) / span # Physical encoder bounds are distinct from the requested sweep. # Samples outside it are not clipped into fake endpoint bins. @@ -74,10 +106,18 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, failures.append(f"{identity}:missing_training_travel") decision = engine.evaluate_sweep(tuple(samples.values()), minimum_span=minimum, total_frames=len(samples), joint_frame_rate=1.0, feedback_hz=0.0, - detection_rate=1.0, bin_count=256) - failures.extend(f"{identity}:{reason}" for reason in decision.failures) + detection_rate=1.0, bin_count=256, + observed_command_progress_01=tuple(command_samples.values())) + failures.extend(f"{identity}:{'command_span' if command_coverage and reason == 'feedback_span' else reason}" + for reason in decision.failures) warnings.extend(f"{identity}:{reason}" for reason in decision.warnings) metrics[identity] = dict(decision.metrics) + 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()) + 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"]) if include_steady and profile.acquisition.command_capture_mode == "interleaved": from .steady import steady_failures @@ -85,3 +125,40 @@ def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, if not failures and unit.cycle == 0: first_cycle_spans.update(references) return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics) + + +def _feedback_motion_coverage(profile, values): + """Check actual encoder travel without interpreting commands as encoders. + + Runtime checks direction, progress, and stability. The recorded direction + must also contain enough independent, distributed feedback observations. + Its observed interval is explicit: it is not a claim that an encoder has + reached command 0/255 or a grouped segment's command boundary. + """ + values = tuple(values) + lower, upper = (min(values), max(values)) if values else (None, None) + span = upper-lower if values else 0. + normalized = tuple((value-lower)/span if span > 0 else 0. for value in values) + policy = profile.acquisition + 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, + 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", + valid_frames=quality.metrics["valid_frames"], bins=quality.metrics["feedback_bins"], + maximum_bin_gap=quality.metrics["maximum_bin_gap"]) + + +def _command_coverage(metrics, native): + """Do not label image-associated command coverage as measured encoders.""" + result = dict(metrics) + result["input_span"] = result.pop("feedback_span") + result["input_bins"] = result.pop("feedback_bins") + result["coverage_input_domain"] = f"command_{native}" + if result.get("gap_scope") == "observed_feedback_span": + result["gap_scope"] = "observed_command_span" + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py index bdfeecf..c80f2ce 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py @@ -13,6 +13,7 @@ from typing import Mapping from ..core import CalibrationProfile, CalibrationStatus, PauseStatus, TaskStatus from .diagnostic_capture import DiagnosticCapturePlan from .engine import CalibrationEngine, ScanUnit, SweepQuality +from ..core.domain.motion_path import task_segment from .observation_scope import required_observation_views from .resume import ResumeDecision @@ -78,6 +79,16 @@ class CalibrationSession: return None return self._units[self._unit_index] + def select_training(self, task_key, cycles): + """Replace only future optional units; evidence IDs never change.""" + from ..core.domain.capture_plan import select_task_training + current = self.current_unit + self.profile = select_task_training(self.profile, task_key, cycles) + self.engine.profile = self.profile + self._units = self.engine.scan_units() + if current is not None: + self._unit_index = next(i for i, unit in enumerate(self._units) if unit.identity == current.identity) + def device_ready(self) -> None: self._require(CalibrationPhase.WAIT_DEVICE) self.phase = CalibrationPhase.READY @@ -130,12 +141,11 @@ class CalibrationSession: self._reused_units.clear() else: valid = { - (unit.task_key, unit.cycle, unit.direction) + unit.identity for unit in self._units } requested = { - (str(task), int(cycle), str(direction)) - for task, cycle, direction in decision.completed_units + tuple(key) for key in decision.completed_units } if not requested.issubset(valid): raise ValueError("resume contains an unknown scan unit") @@ -160,11 +170,11 @@ class CalibrationSession: self._rescan_pending = False def authorize_task_reuse(self, units): - """Only the coordinator can call this after checking frozen joint zeros.""" + """The coordinator must apply the reuse policy and durably import evidence.""" self._require_formal_resume() self._require_any(CalibrationPhase.PREPARE, CalibrationPhase.MAPPING_PROBE) current = self.current_unit - valid = {(u.task_key, u.cycle, u.direction) for u in self._units} + valid = {u.identity for u in self._units} requested = set(units) if current is None or not requested <= valid or any(key[0] != current.task_key for key in requested): raise ValueError("deferred resume must belong to the current task") @@ -190,7 +200,7 @@ class CalibrationSession: if unit is None: self.fail("missing_scan_unit", "扫描单元状态丢失") return - key = (unit.task_key, unit.cycle, unit.direction) + key = unit.identity retries = self._retry_by_unit.get(key, 0) if not quality.passed: if self.engine.permits_retry("sweep_acquisition", retries): @@ -230,7 +240,7 @@ class CalibrationSession: def holdout_complete(self, *, passed: bool, reason: str = "") -> None: self._require(CalibrationPhase.HOLDOUT_VALIDATE) if not passed: - self.fail("holdout_failed", reason or "独立第四轮验证未通过") + self.fail("holdout_failed", reason or "独立验证未通过") return self.phase = CalibrationPhase.BUILD_ARTIFACTS @@ -270,7 +280,7 @@ class CalibrationSession: retries = 0 if unit is not None: retries = self._retry_by_unit.get( - (unit.task_key, unit.cycle, unit.direction), 0 + unit.identity, 0 ) return SessionAction(self.phase, unit, retries) @@ -299,10 +309,10 @@ 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.joints) + measurement_names = set(task_segment(task, unit.segment_key).joints if unit.segment_key else task.joints) measurement_names.update( self.profile.measurement.cross_view_sources[name] - for name in task.joints + for name in tuple(measurement_names) if name in self.profile.measurement.cross_view_sources ) measurements = [ @@ -330,15 +340,18 @@ class CalibrationSession: task=TaskStatus( key="" if task is None else task.key, label="" if task is None else " / ".join(task.joints), - cycle=None if unit is None or unit.sample_phase == "steady" else unit.cycle + 1, + cycle=(None if unit is None or unit.sample_phase == "steady" else + list(dict.fromkeys(u.cycle for u in self._units if u.task_key == unit.task_key + and u.sample_phase == "sweep")).index(unit.cycle) + 1), cycle_count=( len(self.diagnostic_capture.cycles) if self.diagnostic_capture is not None - else 4 + else len({u.cycle for u in self._units if unit is not None and u.task_key == unit.task_key + and u.sample_phase == "sweep"}) or len(self.profile.quality.training_cycles) + 1 ), direction="" if unit is None else unit.direction, capture_stage=("command_validation" if unit is not None and unit.sample_phase == "steady" - and unit.cycle == 5 else "command_training" if unit is not None + and unit.cycle == self.profile.quality.holdout_cycle + 2 else "command_training" if unit is not None and unit.sample_phase == "steady" else "motion"), required_tag_ids_by_view=required, ), @@ -370,7 +383,7 @@ class CalibrationSession: def _advance_reused_units(self) -> None: while self._unit_index < len(self._units): unit = self._units[self._unit_index] - if (unit.task_key, unit.cycle, unit.direction) not in self._reused_units: + if unit.identity not in self._reused_units: break self._unit_index += 1 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py new file mode 100644 index 0000000..9895318 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_evidence.py @@ -0,0 +1,106 @@ +"""Read back shared-Tag posture constraints without re-fitting old sweeps.""" + +from ..core.domain.reference import JointZeroReference, JointZeroSample +from ..core.geometry.image_motion_replay import frozen_image_model, replay_image_models, validate_image_sample +from dataclasses import replace + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..core.geometry.tag_pose.pose_bridge import ( + check_pose_bridge, check_source_pose, SHARED_ZERO_CONDITIONING, SHARED_POSE_CONDITIONING, +) +from .parent_reference_evidence import source_epoch_matches +from .shared_tag_reference import build_pose_bridge, shared_zero_source + + +def validate_shared_tag_references(profile, records, evidence, *, source_model=None): + from .motion_provenance import source_frame_sha256 + from ..profiles.observations import compile_tag_feedback_channels + + source_references = [r for r in records if r.get('kind')=='joint_zero_reference'] + source_references.extend(r['current_reference'] for r in records + if r.get('kind')=='joint_zero_motion_reference_verified') + sources = {source_frame_sha256(r): r for r in source_references} + observations = {(r['view'], str(r['image_stamp_ns'])): r for r in records + if r.get('kind')=='motion_branch_observation'} + samples = {(r['view'], r['image_stamp_ns']): r for r in records if r.get('kind')=='joint_zero_sample'} + pixels = {(r['view'], r['image_stamp_ns']): r for r in records if r.get('kind')=='pnp_candidate_frame'} + channels = None if source_model is None else compile_tag_feedback_channels(profile, source_model) + checked = set() + for payload in evidence.values(): + for record in payload.get('shared_tag_pose_bridges', ()): + key = (payload.get('image_model_sha256'), source_frame_sha256(record)) + if key in checked: + continue + checked.add(key) + joint = record.get('joint') + source_record = record.get('source_reference', {}) + if source_frame_sha256(source_record) not in sources: + raise ValueError('shared_pose_original_zero_missing_or_changed') + reference = JointZeroReference.from_record(source_record) + source = evidence.get(record.get('source_evidence_id')) + spec = profile.measurement.measurements.get(joint) + if (spec is None or joint not in payload['zero_joints'] or source is None + or payload['task_name'] != profile.motion.joint_zero_references[joint].task_key + or source['view'] != spec.view or payload['view'] != spec.view + or source['tag_role'] != spec.child_role + or source['image_model_sha256'] != record.get('source_model_sha256') + or reference.joint not in source['zero_joints'] + or not any(b.view==spec.view and b.tag_role==spec.child_role + and b.motion_evidence_id==record['source_evidence_id'] for b in reference.branch_references)): + raise ValueError('shared_pose_source_model_unbound') + held = record.get('held_channels') + if (not isinstance(held, (list, tuple)) or not held or list(held)!=sorted(set(held)) + or any(type(i) is not int or not 0<=imax(payload['source_image_stamps'])): + raise ValueError('shared_pose_current_image_missing_or_changed') + rows.append(row) + rows.sort(key=lambda r:r['image_stamp_ns']) + if (not rows or not source_epoch_matches(records, source, payload['session_epoch'], + record['source_evidence_id'], rows[0]['image_stamp_ns'])): + raise ValueError('shared_pose_source_epoch_changed') + for sample in reference.samples: + row = samples.get((sample.view, sample.image_stamp_ns)) + frame = pixels.get((sample.view, sample.image_stamp_ns)) + if (row is None or frame is None or row.get('joint')!=reference.joint + or JointZeroSample.from_row(row, profile.command.unit)!=sample): + raise ValueError('shared_pose_zero_pixels_missing_or_changed') + validate_image_sample(row, frame, replay_image_models(frame)) + bridge = build_pose_bridge(profile, joint, reference, rows, tuple(held)) + current_model, source_image_model = frozen_image_model(payload), frozen_image_model(source) + conditioning = record.get('conditioning') + if conditioning is not None: + if conditioning not in {SHARED_ZERO_CONDITIONING, SHARED_POSE_CONDITIONING}: + raise ValueError('shared_pose_conditioning_policy_changed') + bridge = replace(bridge, source_model=source_image_model, conditioning=conditioning) + if check_source_pose(bridge).status != 'consistent': + raise ValueError('shared_pose_source_current_image_changed') + geometry = next(g for g in current_model.geometry if g.relation.joint == joint) + if (Rotation.from_quat(geometry.reference_quaternion_xyzw).inv() + * Rotation.from_quat(bridge.quaternion_xyzw)).magnitude() > 1e-10: + raise ValueError('shared_pose_conditioned_orientation_changed') + if conditioning == SHARED_POSE_CONDITIONING: + zero_point = (np.asarray(geometry.pivot_parent_xyz_m) + + Rotation.from_quat(geometry.reference_quaternion_xyzw).apply(geometry.mount_child_xyz_m)) + if np.linalg.norm(zero_point-np.asarray(bridge.translation_xyz_m)) > 1e-10: + raise ValueError('shared_pose_conditioned_translation_changed') + if (current_model.camera_matrix!=source_image_model.camera_matrix + or any(f.camera_matrix!=current_model.camera_matrix for f in bridge.frames) + or check_pose_bridge(current_model, bridge).status!='consistent'): + raise ValueError('shared_pose_current_model_inconsistent') diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py new file mode 100644 index 0000000..252a1bc --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/shared_tag_reference.py @@ -0,0 +1,146 @@ +"""Build a bounded posture bridge from passed zeros and new approach images. + +This consumes immutable references already available to the session. It never +replays an old sweep, moves hardware, or changes a frozen joint reference. +""" + +import math +from dataclasses import replace + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..core.geometry.tag_pose.motion_evidence import MotionEvidenceFrame, MotionRelation, RoleCandidates +from ..core.geometry.tag_pose.motion_image_types import ImageMotionFrame, ImageRoleObservation +from ..core.geometry.tag_pose.pose_bridge import SharedTagPoseBridge +from ..core.geometry.tag_pose.types import SquareTagPose +from ..core.geometry.tag_pose.image_reference import IMAGE_REFERENCE_SOURCE, reference_from_evidence +from .motion_execution import feedback_tolerances + + +def shared_zero_source(profile, joint, view, references, channels): + """Nearest earlier task observing the same rigid roles at the same posture.""" + spec = profile.measurement.measurements[joint] + if spec.view != view or channels is None or profile.vision_motion: + return None + # A changing parent needs its own authorization path; this bridge is for + # two single hinges measured against the same stationary camera reference. + if not any(tag.role == spec.parent_role and tag.fixed_reference + for v in profile.vision.views if v.name == view for tag in v.tags): + return None + target = profile.motion.joint_zero_references[joint].command + relevant = sorted(channels[spec.child_role] | channels[spec.parent_role]) + previous = [] + for task in profile.motion.tasks: + if joint in task.joints: + break + for name in task.joints: + old = profile.measurement.measurements[name] + reference = references.get(name) + if (reference is not None and (old.view, old.parent_role, old.child_role) == + (view, spec.parent_role, spec.child_role)): + samples = [s for s in reference.samples if s.view == view] + if samples and all(s.command[i] == target[i] for s in samples for i in relevant): + previous.append(reference) + return (previous[-1], tuple(relevant)) if previous else None + + +def bridge_frames(rows, relation): + """Use actual same-image pixels; never obtain a moving pose from continuity.""" + frames = [] + for row in rows: + roles, observations, references = [], [], [] + for role in (relation.parent_role, relation.child_role): + item = row['tags'][role] + if item['candidate_diagnostics']['observation_stamp_ns'] != row['image_stamp_ns']: + raise ValueError('shared_pose_image_identity_changed') + root = role == relation.parent_role + payloads = ([item['selected_pose']] if root else item['reprojection_valid_candidates']) + if not payloads or any(p is None for p in payloads): + raise ValueError('shared_pose_candidates_missing') + if root and item['candidate_diagnostics'].get('branch_frozen') is not True: + raise ValueError('shared_pose_parent_not_frozen') + poses = tuple(SquareTagPose(tuple(p['quaternion_xyzw']), tuple(p['translation_xyz_m']), + float(p['reprojection_error_px'])) for p in payloads) + roles.append(RoleCandidates(role, poses, root)) + observations.append(ImageRoleObservation(role, item['tag_size_m'], tuple(map(tuple, item['corners_xy'])))) + if item.get('pose_source') == IMAGE_REFERENCE_SOURCE: + references.append(reference_from_evidence(item, stamp_ns=row['image_stamp_ns'], camera_matrix=row['camera_matrix'])) + frames.append(ImageMotionFrame(MotionEvidenceFrame(row['image_stamp_ns'], tuple(roles)), + tuple(map(tuple, row['camera_matrix'])), tuple(observations), tuple(references))) + return tuple(frames) + + +def build_pose_bridge(profile, joint, reference, rows, channels): + """The same physical checks serve live construction and journal read-back.""" + spec = profile.measurement.measurements[joint] + old_spec = profile.measurement.measurements[reference.joint] + if (spec.view, spec.parent_role, spec.child_role) != (old_spec.view, old_spec.parent_role, old_spec.child_role): + raise ValueError('shared_pose_physical_roles_changed') + samples = [s for s in reference.samples if s.view == spec.view] + minimum = max(10, profile.acquisition.fixed_reference_minimum_frames) + if len(samples) < minimum or len(rows) < minimum: + raise ValueError('shared_pose_stationary_images_missing') + stamps = [row['image_stamp_ns'] for row in rows] + if (stamps != sorted(set(stamps)) or min(stamps) <= max(s.image_stamp_ns for s in samples) + or stamps[-1]-stamps[0] < 200_000_000): + raise ValueError('shared_pose_stationary_window_invalid') + target = profile.motion.joint_zero_references[joint].command + if (any(s.command[i] != target[i] for s in samples for i in channels) + or any(row.get('command_vector') is None or row.get('feedback_vector') is None + or row['view'] != spec.view or row['sample_phase'] != 'zero_approach' + or any(row['command_vector'][i] != target[i] for i in channels) for row in rows)): + raise ValueError('shared_pose_posture_changed') + old_feedback = np.asarray([s.feedback for s in samples])[:, channels] + current_feedback = np.asarray([r['feedback_vector'] for r in rows])[:, channels] + tolerance = feedback_tolerances(profile.command.unit).stability + if (not np.all(np.isfinite(current_feedback)) or not np.all(np.isfinite(old_feedback)) + or np.any(np.ptp(current_feedback, axis=0) > tolerance) + or np.any(np.abs(current_feedback-np.median(old_feedback, axis=0)) > 2*tolerance)): + raise ValueError('shared_pose_feedback_changed') + rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in samples]) + points = np.asarray([s.relative_translation_xyz_m for s in samples]) + if (np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1.) + or np.max(np.linalg.norm(points-np.median(points, axis=0), axis=1)) > .001): + raise ValueError('shared_pose_source_not_stable') + relation = MotionRelation(joint, spec.parent_role, spec.child_role) + return SharedTagPoseBridge(relation, tuple(rotations.mean().as_quat()), + tuple(np.median(points, axis=0)), bridge_frames(rows, relation)) + + +def prepare_pose_bridges(owner, view, references, sources, arc_stamps, epoch): + from .motion_provenance import source_frame_sha256 + + bridges, records = [], [] + for joint in owner.zero_joints: + # Verify-mode resume preserves historical curve zeros, but also has a + # freshly verified pose attached to the current image model. Use that + # pose for new geometry; never relabel the historical zero's identity. + pose_references = {name: owner.parent_references.zero_references.get((epoch, view, name), ref) + for name, ref in references.items()} + selected = shared_zero_source(owner.profile, joint, view, pose_references, owner.tag_feedback_channels) + if selected is None: + continue + reference, channels = selected + source = owner.parent_references.models.get((epoch, view, reference.joint)) + if source is None: + raise ValueError('shared_pose_source_model_missing') + model, report = source + spec = owner.profile.measurement.measurements[joint] + identity = dict(model.evidence_ids).get(spec.child_role) + if not any(b.view == view and b.tag_role == spec.child_role and b.motion_evidence_id == identity + for b in reference.branch_references): + raise ValueError('shared_pose_source_model_changed') + minimum = max(10, owner.profile.acquisition.fixed_reference_minimum_frames) + rows = tuple(row for row in sources if row['image_stamp_ns'] not in arc_stamps)[-minimum:] + bridge = replace(build_pose_bridge(owner.profile, joint, reference, rows, channels), + source_model=model.image_model) + if any(f.camera_matrix != model.image_model.camera_matrix for f in bridge.frames): + raise ValueError('shared_pose_camera_changed') + bridges.append(bridge) + records.append(dict(joint=joint, source_reference=reference.as_record(), + conditioning=bridge.conditioning, + source_evidence_id=identity, source_model_sha256=report['image_model_sha256'], + held_channels=list(channels), + source_frame_hashes={str(row['image_stamp_ns']): source_frame_sha256(row) for row in rows})) + return tuple(bridges), tuple(records) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py index d22e74f..5343cee 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py @@ -132,8 +132,8 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot: metrics = tuple(quality.metrics.values()) acquisition = replace(acquisition, valid_samples=min(m["valid_frames"] for m in metrics), - coverage_01=min(m["feedback_span"] for m in metrics), - bins=min(m["feedback_bins"] for m in metrics), + coverage_01=min(m.get("input_span", m.get("feedback_span")) for m in metrics), + bins=min(m.get("input_bins", m.get("feedback_bins")) for m in metrics), maximum_gap=max(m["maximum_bin_gap"] for m in metrics)) status = replace(status, task=task, acquisition=acquisition, phase=motion.phase if not interrupted and motion is not None and motion.phase in diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py index 65094f6..124c0ef 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py @@ -18,7 +18,7 @@ _PHASES = { "PREPARE": "扫描起点准备", "MAPPING_PROBE": "固定通道映射点动", "SWEEP": "正式扫描", "EVALUATE": "方向数据检查", "RESCAN": "同速重扫", "RETURN_BASELINE": "按避让顺序安全收尾", - "FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "第四轮独立验证", + "FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "独立验证", "BUILD_ARTIFACTS": "生成产物", "VALIDATE_URDF": "校验导出 URDF", "PUBLISH": "发布", "COMPLETE": "会话完成", "PAUSED": "已暂停", "DIAGNOSTIC_COMPLETE": "诊断采集完成（未进行精度验收）", @@ -159,7 +159,7 @@ def render_status_zh(value: Mapping[str, Any], estimator=None) -> str: metrics = tuple(accepted.get("metrics", {}).values()) if metrics: lines.append(f"可信姿态样本 {min(m['valid_frames'] for m in metrics)}；" - f"覆盖 {min(m['feedback_span'] for m in metrics):.1%}（未进行精度验收）") + f"覆盖 {min(m.get('input_span', m.get('feedback_span')) for m in metrics):.1%}（未进行精度验收）") unresolved = diagnostic_info.get("unresolved_joint_zero_references", ()) if unresolved: lines.append("零位未确认，仅保留原始观测：" + ", ".join(unresolved)) @@ -193,6 +193,7 @@ def render_status_zh(value: Mapping[str, Any], estimator=None) -> str: if resume: message = resume.get('message', resume.get('reason', '')) message = {"not_requested": "本次完整重采，未请求恢复", + "waiting_for_reference_verification": "已请求断点恢复，等待基准和逐关节零位复核", "diagnostic_capture_no_resume": "诊断采集，不复用断点"}.get(message, message) lines.append(f"断点：{message}；复用 {resume.get('completed_unit_count', 0)} 单元；来源 {resume.get('source_session') or '-'}") hardware = value.get("hardware", {}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py index 1af09c2..cc64e77 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py @@ -1,6 +1,7 @@ """Bounded, direction-specific command observations, separate from sweeps.""" import math +from ..core.domain.motion_path import task_segment, record_scan_identity def steady_targets(profile, unit, *, records=(), training_nodes=None): @@ -9,6 +10,15 @@ def steady_targets(profile, unit, *, records=(), training_nodes=None): from ..core.fitting.command_sampling import declared_training_nodes values = (command_nodes(profile, task, holdout=True) if unit.cycle == command_holdout_cycle(profile) else training_nodes if training_nodes is not None else declared_training_nodes(profile, task, records)) + if unit.segment_key: + segment = task_segment(task, unit.segment_key) + starts, ends = dict(segment.start_commands), dict(segment.end_commands) + fractions = {0.0, 1.0} + for channel in segment.moving_channels: + lo, hi = sorted((starts[channel], ends[channel])) + fractions.update((value-starts[channel])/(ends[channel]-starts[channel]) + for value in values if lo <= value <= hi) + return tuple(segment.start+(segment.end-segment.start)*f for f in sorted(fractions)) return tuple(values if unit.direction == "increasing" else reversed(values)) @@ -18,15 +28,23 @@ def steady_failures(profile, unit, attempt, records): failures = [] selected = [r for r in records if r.get("sample_phase") == "steady" and r.get("task_name") == unit.task_key and r.get("cycle") == unit.cycle - and r.get("direction") == unit.direction and int(r.get("attempt", 1)) == attempt] + and record_scan_identity(r) == unit.identity and int(r.get("attempt", 1)) == attempt] + segment = task_segment(task, unit.segment_key) if unit.segment_key else None for field, name, spec in observation_streams(profile, task): + if segment and name not in segment.joints: + continue for index, target in enumerate(steady_targets(profile, unit, records=records)): + if segment: + from ..core.fitting.motion_fit import channel_for_joint + expected = segment.commands_at(target, quantize=profile.command.unit == "u8")[channel_for_joint(profile, name)] + else: + expected = target images = {r.get("image_stamp_ns") for r in selected if r.get("sample_phase") == "steady" and r.get(field) == name and r.get("view") == spec.view and r.get("task_name") == unit.task_key and r.get("cycle") == unit.cycle and r.get("direction") == unit.direction and int(r.get("attempt", 1)) == attempt and r.get("steady_index") == index - and math.isclose(float(r.get("steady_target", float("nan"))), target, abs_tol=1e-9) + and math.isclose(float(r.get("steady_target", float("nan"))), expected, abs_tol=1e-9) and r.get("image_stamp_ns", 0) > 0} if len(images) < profile.acquisition.steady_minimum_samples: failures.append(f"steady:{field}:{name}:{spec.view}:node={index}:samples={len(images)}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py new file mode 100644 index 0000000..7950088 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/task_quality.py @@ -0,0 +1,56 @@ +"""Reject unsupported independent inputs after their capture phase completes. + +The protected release policy selects steady command support or native feedback +support. This never extends a curve domain or changes a recorded endpoint. +Final numerical and artifact validation still run. +""" + +import math + +from ..core.domain.motion_path import record_scan_identity +from ..core.domain.profile import observation_streams +from ..core.domain.capture_plan import CapturePlan +from ..core.fitting.rotation_curve import common_directional_input_support +from .engine import SweepQuality + + +def evaluate_task_input_support(profile, task, records): + rows = [row for row in records if row.get("task_name") == task.key] + latest = {} + for row in rows: + if all(key in row for key in ("cycle", "direction")): + key = record_scan_identity(row) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + phase = "steady" if profile.command_based_release else "sweep" + partition = CapturePlan.from_profile(profile).task(task.key) + training_cycles = partition.command_training if phase == "steady" else partition.training + holdout_cycle = partition.command_holdout if phase == "steady" else partition.holdout + selected = [row for row in rows if row.get("sample_phase", "sweep") == phase + and row.get("cycle") in (*training_cycles, holdout_cycle) + and row.get("direction") in {"increasing", "decreasing"} + and int(row.get("attempt", 1)) == latest.get(record_scan_identity(row))] + field = f"command_{profile.command.unit}" if profile.command_based_release else profile.curve_input_domain + failures, metrics = [], {} + for kind, joint, spec in observation_streams(profile, task): + stream = [row for row in selected if row.get(kind) == joint and row.get("view") == spec.view] + identity = f"{kind}:{joint}:{spec.view}" + training = [row for row in stream if row["cycle"] in training_cycles] + holdout = [row for row in stream if row["cycle"] == holdout_cycle] + if (not training or not holdout or any(row.get(field) is None + or not math.isfinite(float(row[field])) for row in stream)): + failures.append(f"{identity}:missing_native_input_evidence") + continue + try: + lower, upper = common_directional_input_support( + [row[field] for row in training], [row["direction"] for row in training]) + except ValueError as error: + failures.append(f"{identity}:{error}") + continue + outside = [row for row in holdout if not lower <= float(row[field]) <= upper] + metrics[identity] = {"input_domain": field, "training_support": [lower, upper], + "holdout_span": [min(float(row[field]) for row in holdout), max(float(row[field]) for row in holdout)], + "holdout_samples": len(holdout), "outside_training_support": len(outside), + "outside_image_ids": [f"{row['view']}:{row['image_stamp_ns']}" for row in outside]} + if outside: + failures.append(f"{identity}:holdout_outside_training_support") + return SweepQuality(not failures, tuple(failures), (), metrics) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py new file mode 100644 index 0000000..0971742 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/timing_diagnostics.py @@ -0,0 +1,92 @@ +"""Observe control-loop and garbage-collector pauses without changing guards.""" +from collections import deque +import gc +from pathlib import Path +import time + +from ..core.artifacts.storage import append_jsonl_many + + +class StageTiming: + """Exclusive wall-time buckets; repeated polls do not write to disk.""" + + def __init__(self, now): + self.started = self.previous = float(now) + self.stage = "startup" + self.seconds = {} + self.transitions = {} + + def enter(self, stage, now): + now = float(now) + self.seconds[self.stage] = self.seconds.get(self.stage, 0.) + max(0., now - self.previous) + self.previous = now + if stage != self.stage: + self.transitions[stage] = self.transitions.get(stage, 0) + 1 + self.stage = stage + + def as_dict(self, now): + self.enter(self.stage, now) + return {"schema_version": 1, "elapsed_seconds": max(0., float(now) - self.started), + "stage_seconds": dict(self.seconds), "stage_entries": dict(self.transitions), + "clock": "monotonic", "resolution": "control_tick_boundaries"} + + +def acquisition_stage(host): + if host.state in {"PASSED", "FAILED", "ABORTED", "PAUSED", "DIAGNOSTIC_COMPLETE"}: + return "terminal" + if host.finalization.started: + return ("final_acceptance" if host.execution.session.phase.value in { + "HOLDOUT_VALIDATE", "BUILD_ARTIFACTS", "VALIDATE_URDF", "PUBLISH"} else "finalization_fit") + if host._training_request is not None: + return "training_assessment" + if host.branch_initialization.pending(): + return "geometry_solving" + if host._resume_import is not None or host._passed_reference_import is not None or host._reference_import is not None: + return "resume_import" + if host.execution.action.retry: + return "recovery" + motion = host._motion + if motion is None: + return "initialization" if not host.started else "coordination" + if motion.phase == "sweep": + return "scan_motion" + if motion.phase in {"steady", "joint_zero"}: + now = host.ports.monotonic() + if host.segment is not None and host.segment.steady_ready(now): + return "zero_sampling" if motion.phase == "joint_zero" else "steady_sampling" + return "stable_wait" + return "preparation_motion" + + +class RuntimeTimingDiagnostics: + def __init__(self, path, threshold_seconds=.05): + self.path = Path(path) + self.threshold_seconds = threshold_seconds + self.pending = deque(maxlen=256) + self._gc_started = None + gc.callbacks.append(self._gc_event) + + def record(self, operation, started, **details): + elapsed = time.perf_counter()-started + if elapsed >= self.threshold_seconds: + self.pending.append(dict(kind='runtime_processing_delay', operation=operation, + elapsed_seconds=elapsed, stamp_ns=time.time_ns(), **details)) + + def _gc_event(self, phase, info): + if phase == 'start': + self._gc_started = time.perf_counter() + elif self._gc_started is not None: + self.record('garbage_collection', self._gc_started, generation=info['generation'], + collected=info.get('collected',0)) + self._gc_started = None + + def flush(self): + rows = [] + while self.pending: + rows.append(self.pending.popleft()) + if rows: + append_jsonl_many(self.path, rows, durable=False) + + def close(self): + gc.callbacks.remove(self._gc_event) + self.flush() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py new file mode 100644 index 0000000..d49ade0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/training.py @@ -0,0 +1,136 @@ +"""Causal training-plan replay and isolated live assessment.""" + +from concurrent.futures import ThreadPoolExecutor +import multiprocessing +import threading +import time +import traceback + +from ..core.domain.capture_plan import (ADAPTIVE, CapturePlan, configure_training, + evidence_digest, select_task_training) +from ..core.domain.reference import JointZeroReference +from ..core.fitting.training_quality import (TRAINING_DECISION_VERSION, assess_training, + training_snapshot) + + +def resolve_capture_plan(profile, records, *, source_urdf=None, verify_decisions=False, require_complete=False): + """Replay decisions in journal order, never from future or held-out rows.""" + base = configure_training(profile, profile.quality.training_policy) + headers = [row for row in records if row.get("kind") == "session_start"] + plan = CapturePlan.from_profile(base) + if base.quality.training_policy != ADAPTIVE: + if any(row.get("kind") == "training_decision" for row in records): + raise ValueError("adaptive_decisions_in_fixed_capture") + if headers and "capture_plan" in headers[0] and headers[0]["capture_plan"] != plan.as_dict(): + raise ValueError("capture_plan_header_changed") + return base + if len(headers) != 1 or headers[0].get("capture_plan") != plan.as_dict(): + raise ValueError("adaptive_capture_requires_matching_plan_header") + current, previous, frozen, prefix, references = base, {}, set(), [], {} + for row in records: + kind = row.get("kind") + if kind == "joint_zero_reference": + reference = JointZeroReference.from_record(row) + references[reference.joint] = reference + if kind == "training_decision": + key = row.get("task_name") + cycles = tuple(row.get("assessed_cycles", ())) + decision = row.get("decision") + expected_cycles = (0, 1, 2) if previous.get(key) == "add_training" else (0, 1) + if (key not in {task.key for task in profile.motion.tasks} or key in frozen + or previous.get(key) == "fail" + or cycles != expected_cycles or row.get("version") != TRAINING_DECISION_VERSION + or decision not in {"add_training", "freeze", "fail"} + or (decision == "add_training" and len(cycles) != 2) + or row.get("input_plan_sha256") != CapturePlan.from_profile(current).sha256 + or row.get("decision_sha256") != evidence_digest({k: v for k, v in row.items() + if k != "decision_sha256"})): + raise ValueError(f"training_decision_invalid:{key}") + if verify_decisions: + expected = assess_training(current, key, cycles, + training_snapshot(current, prefix, key, cycles), references, source_urdf) + if row != expected: + raise ValueError(f"training_decision_evidence_changed:{key}") + previous[key] = decision + if decision == "freeze": + current = select_task_training(current, key, cycles) + frozen.add(key) + elif decision == "fail" and require_complete: + raise ValueError(f"training_failed:{key}") + elif kind in {"joint_sample", "secondary_joint_sample"}: + key = row.get("task_name") + if row.get("cycle") == 2 and previous.get(key) != "add_training": + raise ValueError(f"third_training_without_decision:{key}") + if row.get("cycle") == profile.quality.holdout_cycle and key not in frozen: + raise ValueError(f"holdout_before_training_freeze:{key}") + if key in frozen and row.get("cycle") in profile.quality.training_cycles: + raise ValueError(f"training_observation_after_freeze:{key}") + prefix.append(row) + if require_complete and frozen != {task.key for task in profile.motion.tasks}: + raise ValueError("training_plan_incomplete") + return current + + +def _assess_child(send, kwargs): + try: + send.send((assess_training(**kwargs), None)) + except Exception: + send.send((None, traceback.format_exc())) + finally: + send.close() + + +class TrainingWorker: + """One bounded job at a time; spawning and fitting stay off the control loop.""" + + def __init__(self, timeout_seconds=120.): + self.timeout_seconds = timeout_seconds + self._closed = threading.Event() + self._executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix="training-check") + self._future = None + + def _run(self, kwargs): + context = multiprocessing.get_context("spawn") + receive, send = context.Pipe(duplex=False) + process = context.Process(target=_assess_child, args=(send, kwargs), daemon=True) + started = time.monotonic() + try: + process.start() + send.close() + while not self._closed.is_set(): + remaining = self.timeout_seconds - (time.monotonic() - started) + if remaining <= 0: + raise TimeoutError("training_assessment_timeout") + if receive.poll(min(.05, remaining)): + result, error = receive.recv() + if error is not None: + raise RuntimeError(error) + return result + if not process.is_alive(): + raise RuntimeError(f"training_assessment_process_exited:{process.exitcode}") + raise RuntimeError("training_assessment_cancelled") + finally: + receive.close() + send.close() + if process.pid is not None: + if process.is_alive(): + process.terminate() + process.join(timeout=1.) + if process.is_alive(): + process.kill() + process.join(timeout=1.) + process.close() + + def poll(self, **kwargs): + if self._closed.is_set(): + raise RuntimeError("training_assessment_cancelled") + if self._future is None: + self._future = self._executor.submit(self._run, kwargs) + if not self._future.done(): + return None + future, self._future = self._future, None + return future.result() + + def close(self): + self._closed.set() + self._executor.shutdown(wait=False, cancel_futures=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/training_replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/training_replay.py new file mode 100644 index 0000000..c8a96ec --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/training_replay.py @@ -0,0 +1,120 @@ +"""Read-only two/three-cycle comparison against the unchanged holdout. + +This diagnostic uses the live compact index. It does not publish, change the +recorded policy, or replace motion/image provenance and final-file acceptance. +""" + +import argparse +import hashlib +import json +from pathlib import Path +import time + +from ..core.domain.capture_plan import ADAPTIVE, configure_training, select_task_training +from ..core.domain.reference import read_joint_zero_references +from ..core.domain.motion_path import record_scan_identity +from ..core.fitting.command_motion import fit_command_motion +from ..core.fitting.training_quality import assess_training, training_snapshot +from ..core.geometry.frozen_evidence import EvidenceDecoder +from ..core.urdf.kinematics import UrdfKinematicModel +from ..profiles import load_bundled_hand_profile +from .acquisition import accepted_joint_records +from .capture_index import CaptureRecordIndex +from .engine import CalibrationEngine + + +def load_training_index(path): + started, digest = time.perf_counter(), hashlib.sha256() + index = CaptureRecordIndex(retain_complete=False, retain_training_geometry=True) + # Final release replays and verifies full evidence. This comparison never + # claims a provenance pass and only needs the recorded poses/scalars. + decoder = EvidenceDecoder(verify_hashes=False) + with Path(path).open("rb") as stream: + for line in stream: + digest.update(line) + if line.strip(): + index.append(decoder.loads(line)) + return index, {"load_seconds": time.perf_counter()-started, + "raw_bytes": Path(path).stat().st_size, "raw_sha256": digest.hexdigest(), "indexed_records": len(index)} + + +def compare_training(profile, source, records, *, task_keys=()): + selected = set(task_keys) or {t.key for t in profile.motion.tasks} + if not selected <= {t.key for t in profile.motion.tasks}: + raise ValueError("unknown comparison task") + adaptive = configure_training(profile, ADAPTIVE) + model = UrdfKinematicModel(source) + references = read_joint_zero_references(profile, records) + sweep = accepted_joint_records(profile, records) + steady = accepted_joint_records(profile, records, sample_phase="steady") + completions, attempts = {}, {} + for row in records: + if all(key in row for key in ("task_name", "cycle", "direction")): + key = record_scan_identity(row) + attempts[key] = max(attempts.get(key, 0), int(row.get("attempt", 1))) + if row.get("kind") == "scan_unit_complete": + completions[(key, int(row.get("attempt", 1)))] = row.get("passed") is True + units = CalibrationEngine(profile).scan_units() + tasks = {} + for task in profile.motion.tasks: + if task.key not in selected: + continue + variants = {} + if any(completions.get((u.identity, attempts.get(u.identity))) is not True + for u in units if u.task_key == task.key): + tasks[task.key] = {"status": "task_capture_incomplete"} + continue + holdout_rows = [r for name in task.joints for r in steady[name] if r["cycle"] == profile.quality.holdout_cycle] + if not holdout_rows: + tasks[task.key] = {"status": "no_independent_holdout"} + continue + cutoff = next(i for i, r in enumerate(records) if r.get("task_name") == task.key + and r.get("cycle") == profile.quality.holdout_cycle and r.get("kind") == "joint_sample") + prefix = records[:cutoff] + for cycles in ((0, 1), (0, 1, 2)): + began = time.perf_counter() + current = select_task_training(adaptive, task.key, cycles) + decision = assess_training(adaptive, task.key, cycles, + training_snapshot(adaptive, prefix, task.key, cycles), references, source) + result = {"training_decision": decision, "holdout_cycle": profile.quality.holdout_cycle} + try: + part = {name: [r for r in sweep[name] if r["cycle"] in (*cycles, profile.quality.holdout_cycle)] + for name in task.joints} + fixed = {name: [r for r in steady[name] if r["cycle"] in (*cycles, profile.quality.holdout_cycle)] + for name in task.joints} + fit = fit_command_motion(current, model, part, fixed, {name: references[name] for name in task.joints}) + result.update(holdout_passed=True, holdout_metrics={name: {key: metric[key] + for key in ("count", "mae_deg", "p95_deg", "maximum_deg", "by_direction")} + for name, metric in fit.command_metrics.items()}) + except (ValueError, KeyError) as error: + result.update(holdout_passed=False, reason=str(error)) + result["assessment_seconds"] = time.perf_counter()-began + variants[str(len(cycles))] = result + tasks[task.key] = {"status": "compared", "holdout_image_ids": sorted({r["sample_id"] for r in holdout_rows}), + "variants": variants} + return {"schema_version": 1, "publication_allowed": False, + "scope": "software_training_comparison_only; motion/image provenance and full-hand release not certified", + "profile_id": profile.key.profile_id, "tasks": tasks} + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--profile", default="o30_right_18") + parser.add_argument("--source-urdf", required=True, type=Path) + parser.add_argument("--raw-samples", required=True, type=Path) + parser.add_argument("--task", action="append", default=[]) + parser.add_argument("--output", required=True, type=Path) + args = parser.parse_args() + if args.output.resolve() == args.raw_samples.resolve(): + parser.error("comparison output must not replace the source journal") + records, timing = load_training_index(args.raw_samples) + report = compare_training(load_bundled_hand_profile(args.profile), args.source_urdf, records, task_keys=args.task) + report["input"] = {"raw_samples": str(args.raw_samples.resolve()), **timing} + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(json.dumps(report, ensure_ascii=False, indent=2, allow_nan=False)+"\n") + print(json.dumps({"output": str(args.output), **timing, + "compared_tasks": sum(t["status"] == "compared" for t in report["tasks"].values())}, ensure_ascii=False)) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py index da503f9..3c34898 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py @@ -9,6 +9,7 @@ from typing import Sequence import numpy as np from ..core import CalibrationProfile, TaskSpec +from ..core.domain.motion_path import task_segment, task_channels @dataclass(frozen=True) @@ -136,12 +137,18 @@ def build_calibration_motion_command( command_value: float, *, profile: CalibrationProfile, + segment_key: str = "", ) -> list[float]: """Build one task command from the profile baseline and avoidance values.""" values = list(profile.command.baseline_values) for index, value in task.auxiliary_commands: values[int(index)] = float(value) - values[int(task.command_index)] = float(command_value) + if segment_key: + for index, value in task_segment(task, segment_key).commands_at(command_value, + quantize=profile.command.unit == "u8").items(): + values[index] = value + else: + values[int(task.command_index)] = float(command_value) if profile.command.unit == "u8": return [int(value) for value in values] return values @@ -153,14 +160,16 @@ def build_calibration_preparation_waypoints( profile: CalibrationProfile, current_command: Sequence[float] | None = None, start_value: float | None = None, + segment_key: str = "", ) -> tuple[tuple[float, ...], ...]: """Execute declarative avoidance groups, then the measured channel.""" # A return sweep or its retry starts at the opposite endpoint. Never # silently substitute the task's first endpoint and reverse/jump the hand. start = task.start_value if start_value is None else float(start_value) - if start not in (task.start_value, task.end_value): + segment = task_segment(task, segment_key) if segment_key else None + if start not in ((segment.start, segment.end) if segment else (task.start_value, task.end_value)): raise ValueError("scan preparation must target a declared sweep endpoint") - final = tuple(build_calibration_motion_command(task, start, profile=profile)) + final = tuple(build_calibration_motion_command(task, start, profile=profile, segment_key=segment_key)) return _grouped_waypoints(current_command, final, task.preparation_groups, profile=profile, measured_channel=task.command_index) @@ -187,6 +196,8 @@ def build_joint_reference_waypoints(task, target, *, profile, current_command): """Visit an explicitly declared reference pose, which may be inside a scan.""" declared = {pose for spec in profile.motion.joint_zero_references.values() if spec.task_key == task.key for pose in (spec.command, *spec.approach_commands)} + declared.update(pose for preparation in task.model_preparations + for pose in (preparation.command, *preparation.approach_commands)) if tuple(target) not in declared: raise ValueError("undeclared joint zero reference pose") return _grouped_waypoints(current_command, target, task.preparation_groups, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/visual_motion.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/visual_motion.py new file mode 100644 index 0000000..ccac374 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/visual_motion.py @@ -0,0 +1,307 @@ +"""Physical motion witnesses from current Tag corners, independent of encoders. + +The feature is the child's four corners in its parent's projective frame. It +does not choose a planar-PnP branch, infer a motor angle, or feed fitted values +back into capture. Angular accuracy still belongs to the independent image +and URDF validation. Every moved channel needs its own visible Tag pair. +""" + +from collections import deque +from dataclasses import dataclass +import hashlib +import json + +import cv2 +import numpy as np + +from ..core.fitting.motion_fit import channel_for_joint + + +VISUAL_MOTION_POLICY = "relative_tag_corners_v1" + + +def relative_corners(parent, child): + parent, child = np.asarray(parent, dtype=float), np.asarray(child, dtype=float) + if (parent.shape != (4, 2) or child.shape != (4, 2) + or not np.all(np.isfinite(parent)) or not np.all(np.isfinite(child))): + raise ValueError("visual_motion_invalid_corners") + canonical = np.array([[0, 0], [1, 0], [1, 1], [0, 1]], dtype=np.float32) + if abs(cv2.contourArea(parent.astype(np.float32))) < 1: + raise ValueError("visual_motion_degenerate_projection") + matrix = cv2.getPerspectiveTransform(parent.astype(np.float32), canonical) + points = np.column_stack((child, np.ones(4))) @ matrix.T + if np.min(np.abs(points[:, 2])) < 1e-8: + raise ValueError("visual_motion_degenerate_projection") + return points[:, :2]/points[:, 2, None] + + +def corner_distance(a, b, scale): + return float(np.sqrt(np.mean(np.sum((np.asarray(a)-np.asarray(b))**2, axis=1)))*scale) + + +def witness_hash(record): + return hashlib.sha256(json.dumps(record, sort_keys=True, + separators=(",", ":"), allow_nan=False).encode()).hexdigest() + + +@dataclass(frozen=True) +class VisualSample: + stamp_ns: int + received_at: float + points: tuple + scale_px: float + record: dict + + +class VisualMotionObserver: + def __init__(self, profile): + self.profile = profile + self.specs = {name: spec for name, spec in profile.measurement.measurements.items() + if name in profile.command.command_index_by_joint and spec.view is not None} + self.channels = {name: channel_for_joint(profile, name) for name in self.specs} + self.latest = {} + self.zeros = {} + self.pending_records = [] + self.motion = None + self.required = () + self.initial = {} + self.history = {} + self.requested_at = {} + self.window_id = "" + + def observe(self, view, stamp_ns, corners, *, now, epoch, version, command=None): + for name, spec in self.specs.items(): + if spec.view != view or spec.parent_role not in corners or spec.child_role not in corners: + continue + previous = self.latest.get(name) + if previous is not None and stamp_ns <= previous.stamp_ns: + continue + parent, child = corners[spec.parent_role], corners[spec.child_role] + try: + points = relative_corners(parent, child) + except (ValueError, cv2.error): + continue + scale = float(np.mean(np.linalg.norm(np.asarray(parent)-np.roll(parent, -1, axis=0), axis=1))) + record = dict(kind="visual_motion_observation", policy=VISUAL_MOTION_POLICY, + joint=name, view=view, image_stamp_ns=stamp_ns, + session_epoch=epoch, motion_version=version, + command_vector=None if command is None else list(command), + parent_role=spec.parent_role, child_role=spec.child_role, + parent_corners_xy=np.asarray(parent).tolist(), child_corners_xy=np.asarray(child).tolist()) + sample = VisualSample(stamp_ns, now, tuple(map(tuple, points)), scale, record) + self.latest[name] = sample + if name in self.history: + self.history[name].append(sample) + if self.motion is not None and name in self.required: + self.pending_records.append(record) + + def names_for(self, motion, initial): + moved = {i for i, (a, b) in enumerate(zip(initial, motion.target)) if abs(a-b) > 1e-9} + names = {name for name, channel in self.channels.items() if channel in moved} + names.update(motion.visual_return_joints) + names.update(motion.zero_joints or motion.reference_joints or motion.observed_joints) + if motion.recording and not names and motion.task_key: + task = next(task for task in self.profile.motion.tasks if task.key == motion.task_key) + names.update(task.joints) + if moved - {self.channels[name] for name in names}: + raise ValueError("visual_motion_channel_without_observer") + return tuple(sorted(names)) + + def available(self, names, now): + return all(name in self.latest and 0 <= now-self.latest[name].received_at <= + self.profile.acquisition.visual_stale_seconds for name in names) + + def begin(self, motion, initial, *, now, version): + names = self.names_for(motion, initial) + if not self.available(names, now): + return False + self.motion, self.required = motion, names + self.initial_command = tuple(initial) + self.version = version + self.initial = {name: self.latest[name] for name in names} + self.history = {name: deque((self.latest[name],), maxlen=120) for name in names} + self.deltas = {name: motion.target[self.channels[name]]-initial[self.channels[name]] for name in names} + self.requested_at = {} + self.maximum_travel = {name: 0.0 for name in names} + self.progress_sources = dict(self.initial) + self.window_id = "" + self.pending_records.extend(sample.record for sample in self.initial.values()) + return True + + def check(self, segment, now): + """Return a stop reason, while never deriving a goal from feedback.""" + policy = self.profile.acquisition + if not self.available(self.required, now): + missing = [name for name in self.required if not self.available((name,), now)] + return "visual_motion_unavailable:" + ",".join(missing) + for name in self.required: + start = self.initial[name] + distance = corner_distance(start.points, self.latest[name].points, start.scale_px) + if distance > self.maximum_travel[name]: + self.maximum_travel[name] = distance + self.progress_sources[name] = self.latest[name] + channel = self.channels[name] + span = self.profile.command.maximum_values[channel]-self.profile.command.minimum_values[channel] + if abs(self.deltas[name])*segment.fraction <= .05*span: + continue + self.requested_at.setdefault(name, now) + if (self.maximum_travel[name] < policy.visual_minimum_motion_px + and now-self.requested_at[name] >= policy.stall_timeout_seconds): + return f"visual_motion_not_observed:{name}" + if (segment.finished_at is not None + and now-segment.finished_at > max(policy.steady_timeout_seconds, policy.stall_timeout_seconds) + and (not self.motion_complete() or self.stable_window(segment.finished_at, now) is None)): + return "visual_motion_target_unconfirmed:" + ",".join(self.required) + return "" + + def motion_complete(self): + policy = self.profile.acquisition + for name, delta in self.deltas.items(): + channel = self.channels[name] + span = self.profile.command.maximum_values[channel]-self.profile.command.minimum_values[channel] + if abs(delta) > .05*span and self.maximum_travel[name] < policy.visual_minimum_motion_px: + return False + for name in self.motion.visual_return_joints: + target = self.zeros.get((name, tuple(self.motion.target))) + if target is None or corner_distance(target.points, self.latest[name].points, + target.scale_px) > policy.visual_zero_tolerance_px: + return False + return True + + def stable_window(self, finished_at, now): + if finished_at is None or not self.available(self.required, now) or not self.motion_complete(): + return None + selected = {} + for name in self.required: + rows = [sample for sample in self.history[name] if sample.received_at >= finished_at + and tuple(sample.record.get("command_vector") or ()) == tuple(self.motion.target)] + if len(rows) < 3: + return None + end = rows[-1].stamp_ns + boundary = next((i for i in range(len(rows)-1, -1, -1) + if (end-rows[i].stamp_ns)/1e9 >= self.profile.acquisition.steady_window_seconds), None) + if boundary is None: + return None + rows = rows[boundary:] + if len(rows) < 3: + return None + center = np.median([row.points for row in rows], axis=0) + if max(corner_distance(row.points, center, self.initial[name].scale_px) + for row in rows) > self.profile.acquisition.visual_stability_px: + return None + selected[name] = rows + return selected + + def settle_record(self, finished_at, now, *, epoch): + windows = self.stable_window(finished_at, now) + if windows is None: + self.window_id = "" + return None + record = dict(kind="visual_motion_settled", policy=VISUAL_MOTION_POLICY, + session_epoch=epoch, motion_version=self.version, + required_joints=list(self.required), + sources={name: [witness_hash(row.record) for row in rows] for name, rows in windows.items()}, + initial_sources={name: witness_hash(row.record) for name, row in self.initial.items()}, + initial_command=list(self.initial_command), + progress_sources={name: witness_hash(row.record) for name, row in self.progress_sources.items()}, + return_sources={name: witness_hash(self.zeros[(name, tuple(self.motion.target))].record) + for name in self.motion.visual_return_joints}, + target_command=list(self.motion.target)) + identity = witness_hash(record) + if identity == self.window_id: + return self.window_id + self.window_id = identity + record["evidence_id"] = self.window_id + self.pending_records.append(record) + return self.window_id + + def freeze_zeros(self, references): + for name, reference in references.items(): + if (name not in self.latest or not self.window_id + or tuple(self.latest[name].record.get("command_vector") or ()) != tuple(reference.samples[0].command)): + raise ValueError(f"visual_zero_observation_missing:{name}") + pose = tuple(reference.samples[0].command) + self.zeros[(name, pose)] = self.latest[name] + + def drain(self): + result, self.pending_records = self.pending_records, [] + return result + + +def validate_visual_motion_evidence(profile, records): + """Recompute accepted zero/static windows from original image corners.""" + if not profile.vision_motion: + return + samples, windows = {}, {} + policy = profile.acquisition + for row in records: + if row.get("kind") == "visual_motion_observation": + if row.get("policy") != VISUAL_MOTION_POLICY: + raise ValueError("visual_motion_policy_changed") + spec = profile.measurement.measurements.get(row.get("joint")) + if spec is None or (row.get("view"), row.get("parent_role"), row.get("child_role")) != ( + spec.view, spec.parent_role, spec.child_role): + raise ValueError("visual_motion_observer_changed") + relative_corners(row["parent_corners_xy"], row["child_corners_xy"]) + samples[witness_hash(row)] = row + elif row.get("kind") == "visual_motion_settled": + if row.get("policy") != VISUAL_MOTION_POLICY: + raise ValueError("visual_motion_policy_changed") + identity = row.get("evidence_id") + if identity != witness_hash({key: value for key, value in row.items() if key != "evidence_id"}): + raise ValueError("visual_settled_identity_changed") + if set(row["sources"]) != set(row["required_joints"]): + raise ValueError("visual_settled_observers_missing") + for name, ids in row["sources"].items(): + if len(set(ids)) != len(ids) or len(ids) < 3: + raise ValueError("visual_settled_independent_frames_missing") + try: + sources = [samples[key] for key in ids] + initial = samples[row["initial_sources"][name]] + except KeyError as error: + raise ValueError("visual_settled_source_missing") from error + if initial["joint"] != name: + raise ValueError("visual_settled_motion_identity_changed") + stamps = [r["image_stamp_ns"] for r in sources] + if stamps != sorted(set(stamps)) or (stamps[-1]-stamps[0])/1e9 < policy.steady_window_seconds: + raise ValueError("visual_settled_window_incomplete") + if any(r["joint"] != name or r["session_epoch"] != row["session_epoch"] + or r["motion_version"] != row["motion_version"] + or r.get("command_vector") != row["target_command"] for r in sources): + raise ValueError("visual_settled_motion_identity_changed") + scale = float(np.mean(np.linalg.norm(np.asarray(initial["parent_corners_xy"]) + - np.roll(initial["parent_corners_xy"], -1, axis=0), axis=1))) + points = [relative_corners(r["parent_corners_xy"], r["child_corners_xy"]) for r in sources] + center = np.median(points, axis=0) + if max(corner_distance(p, center, scale) for p in points) > policy.visual_stability_px: + raise ValueError("visual_settled_pose_not_stable") + channel = channel_for_joint(profile, name) + span = profile.command.maximum_values[channel]-profile.command.minimum_values[channel] + if abs(row["target_command"][channel]-row["initial_command"][channel]) > .05*span: + progress = samples.get(row.get("progress_sources", {}).get(name)) + if (progress is None or progress["joint"] != name + or progress["session_epoch"] != row["session_epoch"] + or progress["motion_version"] != row["motion_version"] + or corner_distance(relative_corners(initial["parent_corners_xy"], initial["child_corners_xy"]), + relative_corners(progress["parent_corners_xy"], progress["child_corners_xy"]), scale) + < policy.visual_minimum_motion_px): + raise ValueError("visual_settled_motion_not_observed") + if name in row.get("return_sources", {}): + zero = samples.get(row["return_sources"][name]) + if zero is None or zero["joint"] != name or zero.get("command_vector") != row["target_command"]: + raise ValueError("visual_return_source_missing") + reference = relative_corners(zero["parent_corners_xy"], zero["child_corners_xy"]) + if corner_distance(reference, points[-1], scale) > policy.visual_zero_tolerance_px: + raise ValueError("visual_return_pose_not_confirmed") + windows[identity] = row + elif row.get("kind") in {"joint_zero_sample", "joint_sample", "secondary_joint_sample"} and ( + row.get("kind") == "joint_zero_sample" or row.get("sample_phase") == "steady"): + window = windows.get(row.get("visual_settled_evidence_id")) + name = row.get("joint", row.get("observation_joint")) + if window is None or name not in window["required_joints"]: + raise ValueError("visual_sample_settled_evidence_missing") + if row.get(f"command_vector_{profile.command.unit}") != window["target_command"]: + raise ValueError("visual_sample_command_changed") + sources = [samples[key] for key in window["sources"][name]] + if row.get("image_stamp_ns") != sources[-1]["image_stamp_ns"]: + raise ValueError("visual_sample_image_changed") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py index d6d2164..08e7ec0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/zero_recovery.py @@ -1,4 +1,49 @@ -"""One bounded recovery per unreferenced joint group, without new zero claims.""" +"""Shared failure categories and bounded acquisition recovery.""" + +from enum import Enum + + +class FailureCategory(str, Enum): + MISSING_SAMPLES = "missing_samples" + UNOBSERVABLE = "unobservable" + REFERENCE_CHANGED = "reference_changed" + DEVICE_FAULT = "device_fault" + UNKNOWN = "unknown" + + +RETRYABLE_PREPARATION_FAILURES = frozenset({ + # The frame validator can fail before the model solver is entered. + # Both stages must consume the same bounded local recovery allowance. + "insufficient_image_frames", + "motion_candidates_missing_or_invalid", + "image_motion_insufficient_independent_frames", + "image_motion_no_observable_image_model", + "image_motion_validation_frame_unresolved", + "image_motion_incomplete_candidate_family", + "motion_geometry_unresolved", +}) + + +def failure_category(reason): + code = str(reason).split(":", 1)[0] + if code in {"image_motion_no_observable_image_model", "motion_geometry_unresolved", + "image_motion_families_not_distinguishable", "image_motion_shared_pose_unresolved"}: + return FailureCategory.UNOBSERVABLE + if code in RETRYABLE_PREPARATION_FAILURES or code in {"sweep_acquisition", "joint_zero_samples_missing"}: + return FailureCategory.MISSING_SAMPLES + if code in {"motion_held_feedback_changed", "fixed_reference_moved", "joint_resume_reference_changed", + "parent_reference_pose_changed", "camera_extrinsics_changed"}: + return FailureCategory.REFERENCE_CHANGED + if code in {"mechanical_stall", "sdk_disconnected", "hardware_fault", "feedback_stale", "duplicate_controller"}: + return FailureCategory.DEVICE_FAULT + return FailureCategory.UNKNOWN + + +def permits_recovery(reasons, completed_retries): + reasons = tuple(reasons) + return completed_retries == 0 and bool(reasons) and all( + failure_category(reason) == FailureCategory.MISSING_SAMPLES + or str(reason).split(":", 1)[0] in RETRYABLE_PREPARATION_FAILURES for reason in reasons) class ZeroRecovery: @@ -6,6 +51,8 @@ class ZeroRecovery: self._used = set() def take(self, motion, *, geometry_error, reports, referenced_joints): + if motion.reference_reuse: + return None if not motion.zero_joints or set(motion.zero_joints) & set(referenced_joints): return None if geometry_error: @@ -14,10 +61,7 @@ class ZeroRecovery: if any(report.get("status") == "resolved" for report in reports.values()): return None reasons = {report.get("reason", "") for report in reports.values()} - recoverable = {"image_motion_insufficient_independent_frames", "image_motion_no_observable_image_model", - "image_motion_validation_frame_unresolved", "image_motion_families_not_distinguishable", - "image_motion_incomplete_candidate_family", "motion_geometry_unresolved"} - if not reasons or not reasons <= recoverable: + if not permits_recovery(reasons, 0): return None action = "repeat_approach" else: diff --git a/src/linkerhand_calibration/pyproject.toml b/src/linkerhand_calibration/pyproject.toml index 09977b5..818845e 100644 --- a/src/linkerhand_calibration/pyproject.toml +++ b/src/linkerhand_calibration/pyproject.toml @@ -1,3 +1,11 @@ [build-system] requires = ["setuptools>=61"] build-backend = "setuptools.build_meta" + +[tool.pytest.ini_options] +markers = [ + "quick: focused unit and contract tests without full artifact generation", + "replay: retained camera/hand observations and real pause regressions", + "integration: fitting, serialized artifacts, process or ROS integration", + "legacy: historical formats, profiles and compatibility contracts", +] diff --git a/src/linkerhand_calibration/test/conftest.py b/src/linkerhand_calibration/test/conftest.py new file mode 100644 index 0000000..e1e17ec --- /dev/null +++ b/src/linkerhand_calibration/test/conftest.py @@ -0,0 +1,84 @@ +"""Shared read-only synthetic input; mutating tests must copy their records.""" + +import pytest + + +# Keep compatibility in one catalog, without moving modules imported as fixtures. +TIERS = { + "legacy": set("""acquisition alignment_view calibrated_joint_state_bridge core diagnostics + full_hand g20_right_product golden_regression l6_right_profile o12_full_hand_zero + o12_observation_resolution o12_right_profile o12_thumb_pnp o6_right_profile + offline_replay sample_schema storage trajectory urdf_comparison urdf_zero zero_calibration + resume_v2""".split()), + "replay": set("""recorded_diagnostic_replay recorded_side_preparation o12_recorded_replay + spatial_solve_order shared_tag_pose_bridge shared_zero_conditioning shared_pose_conditioning + measured_transfer transferred_hinge_geometry o30_side_observer resumed_image_reference + final_image_sources feedback_reversal_progress native_position_feedback + preparation_admission""".split()), + "integration": set("""all_model_artifact_pipeline all_view_image_acceptance axis_line_gauge + bounded_mimic_approximation calibration_coordinator cad_image_geometry + directional_command_mapping dual_mapping final_image_holdout frozen_candidate_selection + frozen_tag_replay image_model_selection image_motion_live_flow independent_urdf_export + json_urdf_rebuild measured_finalization measured_transfer_finalization measured_zero_mapping + o30_artifacts o6_tag_installation partial_o30_calibration parallel_image_replay + production_image_motion profile_finalization relative_standard_urdf single_pass_capture + transfer_artifact_contract unified_ros_host""".split()), +} + + +def pytest_addoption(parser): + parser.addoption("--timings-json", help="Write durations and outcomes for this selected run to JSON") + + +def pytest_collection_modifyitems(items): + for item in items: + if any(item.get_closest_marker(name) for name in ("quick", *TIERS)): + continue + name = item.path.stem.removeprefix("test_") + tier = next((tier for tier, names in TIERS.items() if name in names), "quick") + item.add_marker(getattr(pytest.mark, tier)) + + +def pytest_configure(config): + import time + config._calibration_started = time.perf_counter() + config._calibration_timings = {} + config._calibration_source_sha256 = None + if config.getoption("--timings-json"): + import hashlib + from pathlib import Path + root = Path(__file__).resolve().parents[1] + digest = hashlib.sha256() + for path in sorted((*root.rglob("*.py"), *root.rglob("*.yaml"))): + digest.update(str(path.relative_to(root)).encode()) + digest.update(path.read_bytes()) + config._calibration_source_sha256 = digest.hexdigest() + + +@pytest.hookimpl(hookwrapper=True) +def pytest_runtest_makereport(item, call): + report = (yield).get_result() + result = item.config._calibration_timings.setdefault(item.nodeid, { + "tier": next(name for name in ("quick", *TIERS) if item.get_closest_marker(name)), + "phases": {}}) + result["phases"][report.when] = {"seconds": report.duration, "outcome": report.outcome} + + +def pytest_sessionfinish(session): + path = session.config.getoption("--timings-json") + if path: + import json + import time + from pathlib import Path + output = Path(path) + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(json.dumps({"exit_status": int(session.exitstatus), + "elapsed_seconds": time.perf_counter()-session.config._calibration_started, + "source_sha256": session.config._calibration_source_sha256, + "tests": session.config._calibration_timings}, indent=2) + "\n") + + +@pytest.fixture(scope="session") +def o30_capture(): + from o30_capture_fixture import synthetic_capture + return synthetic_capture() diff --git a/src/linkerhand_calibration/test/fixtures/o30_frontal_reference.json b/src/linkerhand_calibration/test/fixtures/o30_frontal_reference.json new file mode 100644 index 0000000..142926c --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o30_frontal_reference.json @@ -0,0 +1,443 @@ +{ + "source_sha256": "80cb767f034849d22fd7d897ad4cb9a1c01de060439016fefa06aa231542b9a4", + "camera_matrix": [ + [ + 3792.732421875, + 0.0, + 606.0140166893543 + ], + [ + 0.0, + 3825.36962890625, + 568.5244182036549 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "tag_size_m": 0.0165, + "images": [ + { + "stamp_ns": 1789811400295661447, + "corners_xy": [ + [ + 630.1279296874998, + 928.3162231445312 + ], + [ + 686.1949462890624, + 927.6565551757814 + ], + [ + 685.3428344726562, + 870.0587158203125 + ], + [ + 629.3317260742188, + 870.7624511718751 + ] + ] + }, + { + "stamp_ns": 1789811400329003207, + "corners_xy": [ + [ + 630.280517578125, + 928.2393798828126 + ], + [ + 686.3184204101562, + 927.5789184570314 + ], + [ + 685.5235595703125, + 869.9670410156248 + ], + [ + 629.4974365234375, + 870.6890258789062 + ] + ] + }, + { + "stamp_ns": 1789811400362344967, + "corners_xy": [ + [ + 630.4296875, + 928.1881713867188 + ], + [ + 686.4510498046875, + 927.5227661132812 + ], + [ + 685.7029418945314, + 869.900146484375 + ], + [ + 629.6718139648438, + 870.6118774414061 + ] + ] + }, + { + "stamp_ns": 1789811400395686727, + "corners_xy": [ + [ + 630.3947143554688, + 928.2260131835938 + ], + [ + 686.4204101562501, + 927.5676879882811 + ], + [ + 685.6613769531249, + 869.9398803710936 + ], + [ + 629.6418457031248, + 870.6625976562499 + ] + ] + }, + { + "stamp_ns": 1789811400429028487, + "corners_xy": [ + [ + 630.2294311523438, + 928.3228149414064 + ], + [ + 686.2821044921874, + 927.6596069335936 + ], + [ + 685.46533203125, + 870.066650390625 + ], + [ + 629.4439697265625, + 870.7839355468751 + ] + ] + }, + { + "stamp_ns": 1789811400495712007, + "corners_xy": [ + [ + 630.1682739257811, + 928.3910522460938 + ], + [ + 686.2355346679689, + 927.7315673828125 + ], + [ + 685.405029296875, + 870.1271362304688 + ], + [ + 629.3829956054685, + 870.8333740234375 + ] + ] + }, + { + "stamp_ns": 1789811400529053767, + "corners_xy": [ + [ + 630.31494140625, + 928.3104248046875 + ], + [ + 686.3540039062498, + 927.6571655273438 + ], + [ + 685.5723876953126, + 870.0433959960936 + ], + [ + 629.5508422851567, + 870.7587890625 + ] + ] + }, + { + "stamp_ns": 1789811400562395527, + "corners_xy": [ + [ + 630.4141235351562, + 928.2030639648438 + ], + [ + 686.4436645507812, + 927.5493774414062 + ], + [ + 685.6757202148438, + 869.9291381835938 + ], + [ + 629.6626586914062, + 870.6368408203125 + ] + ] + }, + { + "stamp_ns": 1789811400795973611, + "corners_xy": [ + [ + 630.3918457031251, + 928.1535034179686 + ], + [ + 686.4117431640624, + 927.4778442382811 + ], + [ + 685.6537475585939, + 869.8693237304689 + ], + [ + 629.6341552734376, + 870.5805664062499 + ] + ] + }, + { + "stamp_ns": 1789811400829315371, + "corners_xy": [ + [ + 630.2717285156249, + 928.1955566406247 + ], + [ + 686.3017578124998, + 927.5195922851562 + ], + [ + 685.5221557617189, + 869.9111938476565 + ], + [ + 629.4907226562503, + 870.62109375 + ] + ] + }, + { + "stamp_ns": 1789811400862657131, + "corners_xy": [ + [ + 630.1775512695311, + 928.2653198242186 + ], + [ + 686.23046875, + 927.5882568359375 + ], + [ + 685.400634765625, + 869.9884643554686 + ], + [ + 629.3800659179688, + 870.7053222656249 + ] + ] + }, + { + "stamp_ns": 1789811400929340651, + "corners_xy": [ + [ + 630.2564697265625, + 928.3482055664064 + ], + [ + 686.3138427734375, + 927.6983642578125 + ], + [ + 685.5140380859375, + 870.0744018554686 + ], + [ + 629.4822387695311, + 870.7982177734375 + ] + ] + }, + { + "stamp_ns": 1789811400962682411, + "corners_xy": [ + [ + 630.3780517578125, + 928.2880859375002 + ], + [ + 686.40478515625, + 927.6194458007812 + ], + [ + 685.6351928710938, + 869.99658203125 + ], + [ + 629.6276245117188, + 870.7251586914062 + ] + ] + }, + { + "stamp_ns": 1789811401029365931, + "corners_xy": [ + [ + 630.2855224609376, + 928.2488403320312 + ], + [ + 686.3333740234377, + 927.5859375 + ], + [ + 685.5617065429688, + 869.9732055664064 + ], + [ + 629.5308837890624, + 870.688232421875 + ] + ] + }, + { + "stamp_ns": 1789811401062707691, + "corners_xy": [ + [ + 630.1878662109376, + 928.2838134765625 + ], + [ + 686.244873046875, + 927.609619140625 + ], + [ + 685.4309692382812, + 870.0062255859376 + ], + [ + 629.3936767578126, + 870.7116699218751 + ] + ] + }, + { + "stamp_ns": 1789811401096049451, + "corners_xy": [ + [ + 630.189208984375, + 928.2930297851562 + ], + [ + 686.2364501953124, + 927.6361083984375 + ], + [ + 685.4137573242188, + 870.0328369140625 + ], + [ + 629.4002685546875, + 870.746826171875 + ] + ] + }, + { + "stamp_ns": 1789811401129391211, + "corners_xy": [ + [ + 630.2636108398438, + 928.2960205078126 + ], + [ + 686.2976684570311, + 927.627197265625 + ], + [ + 685.492919921875, + 870.0244140625001 + ], + [ + 629.466064453125, + 870.7437133789062 + ] + ] + }, + { + "stamp_ns": 1789811401162732971, + "corners_xy": [ + [ + 630.3629760742185, + 928.2514648437498 + ], + [ + 686.3906249999999, + 927.6051025390625 + ], + [ + 685.624755859375, + 869.9824829101564 + ], + [ + 629.606689453125, + 870.6973876953125 + ] + ] + }, + { + "stamp_ns": 1789811401196074731, + "corners_xy": [ + [ + 630.3822021484375, + 928.1871337890625 + ], + [ + 686.4000854492188, + 927.5312499999999 + ], + [ + 685.6293334960939, + 869.9202880859376 + ], + [ + 629.6141357421875, + 870.6192626953126 + ] + ] + }, + { + "stamp_ns": 1789811401229199593, + "corners_xy": [ + [ + 630.2957763671876, + 928.1724853515625 + ], + [ + 686.32470703125, + 927.5040283203126 + ], + [ + 685.5545043945312, + 869.896545410156 + ], + [ + 629.523620605469, + 870.6052856445311 + ] + ] + } + ] +} diff --git a/src/linkerhand_calibration/test/fixtures/o30_measured_transfer_middle_dip.json.gz b/src/linkerhand_calibration/test/fixtures/o30_measured_transfer_middle_dip.json.gz new file mode 100644 index 0000000..02339c2 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_measured_transfer_middle_dip.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_resumed_parent_reference.json.gz b/src/linkerhand_calibration/test/fixtures/o30_resumed_parent_reference.json.gz new file mode 100644 index 0000000..a32296a Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_resumed_parent_reference.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_ring_preparation_admission.json.gz b/src/linkerhand_calibration/test/fixtures/o30_ring_preparation_admission.json.gz new file mode 100644 index 0000000..5511130 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_ring_preparation_admission.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_roll_delayed_feedback.json b/src/linkerhand_calibration/test/fixtures/o30_roll_delayed_feedback.json new file mode 100644 index 0000000..3e6b872 --- /dev/null +++ b/src/linkerhand_calibration/test/fixtures/o30_roll_delayed_feedback.json @@ -0,0 +1,850 @@ +{ + "source_session": "calibration_output/O30_RIGHT_001/20260919_165005", + "diagnostic_only": true, + "description": "Image-aligned feedback during command 255 to 223 reversal; feedback first catches up in the preceding direction, then returns. No command/angle calibration claim.", + "initial_command": 255, + "target_command": 223, + "initial_feedback": 221, + "frames": [ + [ + 0.008996, + 255.0, + 221.0 + ], + [ + 0.042338, + 255.0, + 221.0 + ], + [ + 0.075679, + 255.0, + 221.0 + ], + [ + 0.109021, + 255.0, + 221.0 + ], + [ + 0.142363, + 254.0, + 221.0 + ], + [ + 0.175705, + 254.0, + 221.0 + ], + [ + 0.209058, + 254.0, + 221.0 + ], + [ + 0.2424, + 253.0, + 221.0 + ], + [ + 0.275742, + 253.0, + 221.0 + ], + [ + 0.309083, + 252.0, + 221.0 + ], + [ + 0.342425, + 252.0, + 221.0 + ], + [ + 0.375767, + 251.0, + 221.64954668688875 + ], + [ + 0.409109, + 251.0, + 228.398229765594 + ], + [ + 0.44245, + 250.0, + 247.0 + ], + [ + 0.475792, + 249.0, + 247.0 + ], + [ + 0.509134, + 248.0, + 247.0 + ], + [ + 0.542476, + 248.0, + 247.0 + ], + [ + 0.575817, + 247.0, + 247.0 + ], + [ + 0.609159, + 246.0, + 247.0 + ], + [ + 0.642501, + 245.0, + 247.0 + ], + [ + 0.675843, + 244.0, + 247.0 + ], + [ + 0.709184, + 243.0, + 247.0 + ], + [ + 0.742367, + 242.0, + 245.98337350676 + ], + [ + 0.775709, + 241.0, + 241.0 + ], + [ + 0.80905, + 240.0, + 241.0 + ], + [ + 0.842392, + 239.0, + 241.0 + ], + [ + 0.875734, + 238.0, + 241.0 + ], + [ + 0.909076, + 237.0, + 241.0 + ], + [ + 0.942417, + 236.0, + 241.0 + ], + [ + 0.975759, + 235.0, + 240.44438808889143 + ], + [ + 1.009101, + 234.0, + 235.0 + ], + [ + 1.042443, + 233.0, + 235.0 + ], + [ + 1.075785, + 232.0, + 235.0 + ], + [ + 1.109126, + 231.0, + 235.0 + ], + [ + 1.142468, + 230.0, + 234.52744808972707 + ], + [ + 1.17581, + 230.0, + 229.0 + ], + [ + 1.209152, + 229.0, + 229.0 + ], + [ + 1.242493, + 228.0, + 229.0 + ], + [ + 1.275771, + 227.0, + 229.0 + ], + [ + 1.309113, + 227.0, + 229.0 + ], + [ + 1.342455, + 226.0, + 229.0 + ], + [ + 1.375796, + 226.0, + 228.9274971081905 + ], + [ + 1.409138, + 225.0, + 226.93087409328646 + ], + [ + 1.44248, + 225.0, + 224.0 + ], + [ + 1.475822, + 224.0, + 224.0 + ], + [ + 1.509163, + 224.0, + 224.0 + ], + [ + 1.542505, + 224.0, + 224.0 + ], + [ + 1.609189, + 223.0, + 224.0 + ], + [ + 1.64253, + 223.0, + 224.0 + ], + [ + 1.675872, + 223.0, + 224.0 + ], + [ + 1.709214, + 223.0, + 224.0 + ], + [ + 1.742556, + 223.0, + 224.0 + ], + [ + 1.775897, + 223.0, + 224.0 + ], + [ + 1.809242, + 223.0, + 224.0 + ], + [ + 1.842584, + 223.0, + 224.0 + ], + [ + 1.875925, + 223.0, + 224.0 + ], + [ + 1.909267, + 223.0, + 224.0 + ], + [ + 1.942609, + 223.0, + 224.0 + ], + [ + 1.975951, + 223.0, + 221.15303107296222 + ], + [ + 2.009292, + 223.0, + 221.0 + ], + [ + 2.042634, + 223.0, + 221.0 + ], + [ + 2.075976, + 223.0, + 221.0 + ], + [ + 2.109318, + 223.0, + 221.0 + ], + [ + 2.142659, + 223.0, + 221.0 + ], + [ + 2.176001, + 223.0, + 221.0 + ], + [ + 2.209343, + 223.0, + 221.0 + ], + [ + 2.242685, + 223.0, + 221.0 + ], + [ + 2.276027, + 223.0, + 221.0 + ], + [ + 2.309368, + 223.0, + 221.0 + ], + [ + 2.342624, + 223.0, + 221.0 + ], + [ + 2.375966, + 223.0, + 221.0 + ], + [ + 2.409308, + 223.0, + 221.0 + ], + [ + 2.442649, + 223.0, + 221.0 + ], + [ + 2.475991, + 223.0, + 221.0 + ], + [ + 2.509333, + 223.0, + 221.0 + ], + [ + 2.542675, + 223.0, + 221.0 + ], + [ + 2.576016, + 223.0, + 221.0 + ], + [ + 2.609358, + 223.0, + 221.0 + ], + [ + 2.6427, + 223.0, + 221.0 + ], + [ + 2.676042, + 223.0, + 221.0 + ], + [ + 2.709384, + 223.0, + 221.0 + ], + [ + 2.742725, + 223.0, + 221.0 + ], + [ + 2.776067, + 223.0, + 221.0 + ], + [ + 2.809409, + 223.0, + 221.0 + ], + [ + 2.842751, + 223.0, + 221.0 + ], + [ + 2.876021, + 223.0, + 221.0 + ], + [ + 2.909363, + 223.0, + 221.0 + ], + [ + 2.942705, + 223.0, + 221.0 + ], + [ + 3.009388, + 223.0, + 221.0 + ], + [ + 3.04273, + 223.0, + 221.0 + ], + [ + 3.076072, + 223.0, + 221.0 + ], + [ + 3.109414, + 223.0, + 221.0 + ], + [ + 3.142755, + 223.0, + 221.0 + ], + [ + 3.176097, + 223.0, + 221.0 + ], + [ + 3.242781, + 223.0, + 221.0 + ], + [ + 3.276122, + 223.0, + 221.0 + ], + [ + 3.309464, + 223.0, + 221.0 + ], + [ + 3.342806, + 223.0, + 221.0 + ], + [ + 3.376148, + 223.0, + 221.0 + ], + [ + 3.409437, + 223.0, + 221.0 + ], + [ + 3.442779, + 223.0, + 221.0 + ], + [ + 3.47612, + 223.0, + 221.0 + ], + [ + 3.509462, + 223.0, + 221.0 + ], + [ + 3.542804, + 223.0, + 221.0 + ], + [ + 3.609487, + 223.0, + 221.0 + ], + [ + 3.642829, + 223.0, + 221.0 + ], + [ + 3.676171, + 223.0, + 221.0 + ], + [ + 3.709513, + 223.0, + 221.0 + ], + [ + 3.742854, + 223.0, + 221.0 + ], + [ + 3.776196, + 223.0, + 221.0 + ], + [ + 3.809538, + 223.0, + 221.0 + ], + [ + 3.84288, + 223.0, + 221.0 + ], + [ + 3.876221, + 223.0, + 221.0 + ], + [ + 3.909563, + 223.0, + 221.0 + ], + [ + 3.94292, + 223.0, + 221.0 + ], + [ + 3.976262, + 223.0, + 221.0 + ], + [ + 4.009604, + 223.0, + 221.0 + ], + [ + 4.042946, + 223.0, + 221.0 + ], + [ + 4.076288, + 223.0, + 221.0 + ], + [ + 4.109629, + 223.0, + 221.0 + ], + [ + 4.142971, + 223.0, + 221.0 + ], + [ + 4.176313, + 223.0, + 221.0 + ], + [ + 4.209655, + 223.0, + 221.0 + ], + [ + 4.242996, + 223.0, + 221.0 + ], + [ + 4.276338, + 223.0, + 221.0 + ], + [ + 4.30968, + 223.0, + 221.0 + ], + [ + 4.376363, + 223.0, + 221.0 + ], + [ + 4.409705, + 223.0, + 221.0 + ], + [ + 4.443047, + 223.0, + 221.0 + ], + [ + 4.476303, + 223.0, + 221.0 + ], + [ + 4.509644, + 223.0, + 221.0 + ], + [ + 4.542986, + 223.0, + 221.0 + ], + [ + 4.576328, + 223.0, + 221.0 + ], + [ + 4.60967, + 223.0, + 221.0 + ], + [ + 4.643011, + 223.0, + 221.0 + ], + [ + 4.676353, + 223.0, + 221.0 + ], + [ + 4.709695, + 223.0, + 221.0 + ], + [ + 4.743037, + 223.0, + 221.0 + ], + [ + 4.776378, + 223.0, + 221.0 + ], + [ + 4.80972, + 223.0, + 221.0 + ], + [ + 4.843062, + 223.0, + 221.0 + ], + [ + 4.876404, + 223.0, + 221.0 + ], + [ + 4.909745, + 223.0, + 221.0 + ], + [ + 4.943087, + 223.0, + 221.0 + ], + [ + 4.976429, + 223.0, + 221.0 + ], + [ + 5.009743, + 223.0, + 221.0 + ], + [ + 5.043085, + 223.0, + 221.0 + ], + [ + 5.076427, + 223.0, + 221.0 + ], + [ + 5.109769, + 223.0, + 221.0 + ], + [ + 5.14311, + 223.0, + 221.0 + ], + [ + 5.176452, + 223.0, + 221.0 + ], + [ + 5.209794, + 223.0, + 221.0 + ], + [ + 5.243136, + 223.0, + 221.0 + ], + [ + 5.276477, + 223.0, + 221.0 + ], + [ + 5.309819, + 223.0, + 221.0 + ], + [ + 5.343161, + 223.0, + 221.0 + ], + [ + 5.376503, + 223.0, + 221.0 + ], + [ + 5.409844, + 223.0, + 221.0 + ], + [ + 5.443186, + 223.0, + 221.0 + ], + [ + 5.476528, + 223.0, + 221.0 + ], + [ + 5.50987, + 223.0, + 221.0 + ], + [ + 5.543188, + 223.0, + 221.0 + ], + [ + 5.57653, + 223.0, + 221.0 + ], + [ + 5.609872, + 223.0, + 221.0 + ], + [ + 5.643213, + 223.0, + 221.0 + ], + [ + 5.676555, + 223.0, + 221.0 + ], + [ + 5.709897, + 223.0, + 221.0 + ], + [ + 5.77658, + 223.0, + 221.0 + ] + ] +} diff --git a/src/linkerhand_calibration/test/fixtures/o30_shared_tag_middle_pip.json.gz b/src/linkerhand_calibration/test/fixtures/o30_shared_tag_middle_pip.json.gz new file mode 100644 index 0000000..0fade19 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_shared_tag_middle_pip.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_shared_tag_pip.json.gz b/src/linkerhand_calibration/test/fixtures/o30_shared_tag_pip.json.gz new file mode 100644 index 0000000..78305ac Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_shared_tag_pip.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_shared_tag_ring_pip.json.gz b/src/linkerhand_calibration/test/fixtures/o30_shared_tag_ring_pip.json.gz new file mode 100644 index 0000000..f63bc5a Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_shared_tag_ring_pip.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_side_mcp_observers.json.gz b/src/linkerhand_calibration/test/fixtures/o30_side_mcp_observers.json.gz new file mode 100644 index 0000000..14b6695 Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_side_mcp_observers.json.gz differ diff --git a/src/linkerhand_calibration/test/fixtures/o30_transferred_ip_geometry.json.gz b/src/linkerhand_calibration/test/fixtures/o30_transferred_ip_geometry.json.gz new file mode 100644 index 0000000..9dbf10f Binary files /dev/null and b/src/linkerhand_calibration/test/fixtures/o30_transferred_ip_geometry.json.gz differ diff --git a/src/linkerhand_calibration/test/o30_capture_fixture.py b/src/linkerhand_calibration/test/o30_capture_fixture.py new file mode 100644 index 0000000..a85d801 --- /dev/null +++ b/src/linkerhand_calibration/test/o30_capture_fixture.py @@ -0,0 +1,160 @@ +"""Independent elementary FK sensor data; no hardware accuracy claim.""" + +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.domain.reference import build_joint_zero_reference +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.steady import steady_targets +from linkerhand_calibration.core.domain.motion_path import task_segment +from linkerhand_calibration.core.fitting.command_sampling import build_sampling_plan + +PACKAGE = Path(__file__).resolve().parents[1] +SOURCE = PACKAGE / 'urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf' + + +def rigid(rpy, xyz): + t = np.eye(4) + t[:3, :3] = Rotation.from_euler('xyz', rpy).as_matrix() + t[:3, 3] = xyz + return t + + +def payload(t): + return dict(translation_xyz_m=t[:3, 3].tolist(), + quaternion_xyzw=Rotation.from_matrix(t[:3, :3]).as_quat().tolist()) + + +def synthetic_capture(*, scope='full', retained_offset_rad=0.0, camera_aligned_reference=False): + profile = load_bundled_hand_profile('o30_right_18', scope=scope) + joints = {j.get('name'): j for j in ET.parse(SOURCE).findall('joint')} + parent_joints = {j.find('child').get('link'): name for name, j in joints.items()} + # Explicit independent truth, including the five accepted CAD-zero leaves. + offsets = {name: (0.0 if name.endswith(('_dip', '_ip')) else .018 if 'thumb' in name else .025) + for name in joints} + offsets.update({name: retained_offset_rad for name in profile.retained_joints}) + channels = ['thumb_cmc_roll', 'thumb_cmc_yaw', 'index_mcp_roll', 'middle_mcp_roll', + 'ring_mcp_roll', 'pinky_mcp_roll', 'thumb_mcp', 'index_mcp_pitch', 'middle_mcp_pitch', + '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, 113, 52] + [0]*14 + signs = [1, 1, -1, -1, -1, -1] + [1]*14 + 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)) + tag_links = {tag.role: tag.link for view in profile.vision.views for tag in view.tags} + stamp, rows = 1000000, [] + + def sample(task, vector, names, cycle, direction, phase='sweep', segment_key='', steady_index=None): + nonlocal stamp + stamp += 33333333 + angles = {} + for index, name in enumerate(channels): + x, z = vector[index]/255, base[index]/255 + shape = (1.0 + .007*index)*(x-z) + .09*(x*x-z*z) + # Distinct monotonic branches, meeting at the baseline. + branch = (1 if direction == 'increasing' else -1)*.006*(x-z)*(1-x) + angles[name] = offsets[name] + signs[index]*(shape + branch) + + cache = {} + def fk(link): + if link in cache: + return cache[link] + name = parent_joints.get(link) + if name is None: + return np.eye(4) + node = joints[name] + origin = node.find('origin') + t = fk(node.find('parent').get('link')) @ 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()]) + 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[name]).as_matrix() + cache[link] = t @ motion + return cache[link] + + def tag(role): + link = tag_links[role] + if link == 'hand_base_link': + reference = hand @ base_tag + if camera_aligned_reference: + # This pose-only fixture uses an identity common/camera + # transform. The fixed Tag normal is not used as an axis. + reference[:3, :3] = np.eye(3) + return reference + return hand @ fk(link) @ mount + + result = [] + for name in names: + channel = channels.index(name) + spec = profile.measurement.measurements[name] + parent, child = tag(spec.parent_role), tag(spec.child_role) + relative = np.linalg.inv(parent) @ child + row = dict(kind='joint_sample', joint=name, motor_index=channel, task_name=task.key, + view=spec.view, image_stamp_ns=stamp, sample_id=f'{spec.view}:{stamp}', + cycle=cycle, direction=direction, attempt=1, sample_phase=phase, + feedback_u8=vector[channel], command_u8=vector[channel], state_u8=list(vector), + command_vector_u8=list(vector), command_direction_by_index=[direction]*20, + relative_quaternion_xyzw=Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(), + relative_translation_xyz_m=relative[:3, 3].tolist(), parent_pose_common=payload(parent), + child_pose_common=payload(child), view_normal_common_xyz=[.577350269]*3, + camera_center_common_xyz_m=[0., 0., -.5]) + if segment_key: + row['segment_key'] = segment_key + if phase == 'steady': + row.update(steady_target=vector[channel], steady_index=steady_index) + result.append(row) + return result + + for name, reference in profile.motion.joint_zero_references.items(): + task = next(t for t in profile.motion.tasks if t.key == reference.task_key) + channel = channels.index(name) + direction = 'increasing' if reference.command[channel] > reference.approach_commands[-1][channel] else 'decreasing' + samples = [sample(task, reference.command, (name,), 0, direction, 'joint_zero')[0] for _ in range(10)] + zero = build_joint_zero_reference(profile, name, samples, session_epoch=1, motion_version=len(rows)+1) + rows.append(zero.as_record()) + plans = {} + for unit in CalibrationEngine(profile).scan_units(): + 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 + stages = [] + if unit.sample_phase == 'steady': + if task.key not in plans: + plans[task.key] = build_sampling_plan(profile, task, rows) + rows.append(plans[task.key]) + targets = steady_targets(profile, unit, training_nodes=plans[task.key]['training_nodes']) + stages.extend(('steady', index, [value]*3) for index, value in enumerate(targets)) + elif profile.acquisition.command_capture_mode == 'interleaved': + targets = steady_targets(profile, unit) + stages.append(('sweep', None, [targets[0]]*3)) + for index, value in enumerate(targets): + if index: + previous = targets[index-1] + count = max(3, int(np.ceil(128*abs(value-previous)/abs(unit.end-unit.start)))+1) + stages.append(('sweep', None, np.linspace(previous, value, count))) + stages.append(('steady', index, [value]*3)) + else: + stages.append(('sweep', None, np.linspace(unit.start, unit.end, 129))) + for phase, index, values in stages: + for value in values: + vector = list(base) + for i, held in task.auxiliary_commands: + vector[i] = held + if segment: + for i, v in segment.commands_at(value, quantize=True).items(): + vector[i] = v + else: + vector[task.command_index] = round(value) + observed = sample(task, vector, segment.joints if segment else task.joints, + unit.cycle, unit.direction, phase, unit.segment_key, index) + if phase == 'steady' and task.key in plans: + for row in observed: + row.update(steady_training_nodes=plans[task.key]['training_nodes'], + sampling_plan_sha256=plans[task.key]['plan_sha256']) + rows.extend(observed) + return profile, SOURCE, rows, offsets diff --git a/src/linkerhand_calibration/test/o30_formal_fixture.py b/src/linkerhand_calibration/test/o30_formal_fixture.py new file mode 100644 index 0000000..b8ae1f7 --- /dev/null +++ b/src/linkerhand_calibration/test/o30_formal_fixture.py @@ -0,0 +1,163 @@ +"""Synthetic FK journal at the formal release entry, without acceptance mocks. + +This uses supported geometric motion authorizations and projected original +poses. It tests serialization/provenance; it is not a real camera or hardware +accuracy certificate, nor a test of the live image-model solver. +""" + +from collections import defaultdict +from copy import deepcopy +import hashlib +import json + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.domain.capture_plan import (ADAPTIVE, CapturePlan, configure_training, + evidence_digest, select_task_training) +from linkerhand_calibration.core.domain.motion_path import record_scan_identity, segment_metadata +from linkerhand_calibration.core.domain.reference import build_joint_zero_reference +from linkerhand_calibration.core.geometry.extrinsics import camera_info_fingerprint +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.image_capture import IMAGE_OBSERVATION_POLICY +from linkerhand_calibration.runtime.motion_provenance import source_frame_sha256 + + +def formal_capture(capture, directory, *, adaptive=False): + profile, source, original, truth = capture + if adaptive: + profile = configure_training(profile, ADAPTIVE) + records = deepcopy(original) + matrix = np.array([[1200., 0., 640.], [0., 1200., 480.], [0., 0., 1.]]) + projection = np.column_stack((matrix, np.zeros(3))).ravel().tolist() + identity = dict(width=1280, height=960, camera_matrix=matrix.tolist(), + distortion=[0.]*5, rectification=np.eye(3).ravel().tolist(), projection=projection) + cameras = {view: dict(serial_number=f"SYNTHETIC_{view}", width=1280, height=960, + intrinsics_sha256=camera_info_fingerprint(**identity)) for view in profile.vision.view_names} + camera_file = directory / "synthetic_cameras.json" + camera_file.write_text(json.dumps(dict(schema_version=1, reference_view=profile.vision.extrinsic_reference_view, + cameras=cameras, **{f"{profile.vision.extrinsic_reference_view}_from_view": { + view: dict(translation_xyz_m=[0., 0., 0.], quaternion_xyzw=[0., 0., 0., 1.]) for view in cameras}}, + quality={"passed": True, **{key: 0. for key in profile.vision.extrinsics_quality_limits}, + **profile.vision.minimum_capture_counts}))) + hashes = {key: "a"*64 for key in profile.artifacts.protected_input_fields} + hashes.update(source_urdf_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + camera_extrinsics_sha256=hashlib.sha256(camera_file.read_bytes()).hexdigest()) + engine = CalibrationEngine(profile) + header = dict(kind="session_start", profile_id=profile.key.profile_id, serial_number="SYNTHETIC_FORMAL_O30", + acquisition_policy_version=ACQUISITION_POLICY_VERSION, pose_tracking_policy_version=POSE_TRACKING_POLICY_VERSION, + capture_schedule_version=engine.capture_schedule_version, capture_plan=CapturePlan.from_profile(profile).as_dict(), + curve_input_domain=profile.curve_input_domain, fixed_reference_mode=profile.acquisition.fixed_reference_mode, **hashes) + models = [dict(kind="rectified_camera_model", view=view, camera_matrix=matrix.tolist(), width=1280, height=960, + raw_k=matrix.ravel().tolist(), raw_d=identity["distortion"], rectification_r=identity["rectification"], + projection_p=projection, input_is_rectified=True, matrix_source="CameraInfo.P[:3,:3]") for view in cameras] + fixed = dict(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( + {view: matrix.tolist() for view in sorted(cameras)}, sort_keys=True).encode()).hexdigest()}, + fixed_corners_by_view={view: [[0., 0.], [1., 0.], [1., 1.], [0., 1.]] for view in cameras}, + fixed_poses={view: dict(rotation_xyzw=[0., 0., 0., 1.], translation_xyz_m=[0., 0., 1.]) for view in cameras}, + moving_tag_poses={}) + journal = [header, *models, fixed] + tags = {tag.role: tag for view in profile.vision.views for tag in view.tags} + + def observed_tag(role, pose): + tag = tags[role] + half = tag.size_m/2 + local = np.array([[-half, half, 0.], [half, half, 0.], [half, -half, 0.], [-half, -half, 0.]]) + points = Rotation.from_quat(pose["quaternion_xyzw"]).apply(local) + pose["translation_xyz_m"] + pixels = points @ matrix.T + return dict(tag_id=tag.tag_id, tag_size_m=tag.size_m, + corners_xy=(pixels[:, :2]/pixels[:, 2:]).tolist(), selected_pose=pose, + reprojection_valid_candidates=[{**pose, "reprojection_error_px": 0.}]) + + by_unit, by_joint = defaultdict(list), {} + for row in records: + if row["kind"] == "joint_zero_reference": + by_joint[row["joint"]] = row + else: + by_unit[record_scan_identity(row)].append(row) + authorizations, references = {}, {} + + def evidence(row, frozen): + spec = profile.measurement.measurements[row["joint"]] + return {side: dict(tag_role=role, pose_source="current_image", candidate_diagnostics=dict( + observation_stamp_ns=row["image_stamp_ns"], branch_revision=1, branch_status="tracking", + branch_frozen=frozen, motion_evidence_verified=True, motion_evidence_id=authorizations[row["joint"]][role])) + for side, role in (("parent", spec.parent_role), ("child", spec.child_role)) if not tags[role].fixed_reference} + + for task in profile.motion.tasks: + for name in task.joints: + ref = by_joint[name] + spec = profile.measurement.measurements[name] + roles = [role for role in (spec.parent_role, spec.child_role) if not tags[role].fixed_reference] + initial = next(u for u in engine.scan_units() if u.task_key == task.key + and (not u.segment_key or name in next(s for s in task.segments if s.key == u.segment_key).joints)) + source_rows = [r for r in by_unit[initial.identity] if r.get("joint") == name and r["sample_phase"] == "sweep"] + source_frames = [dict(kind="motion_branch_observation", task_name=task.key, view=spec.view, + zero_joints=[name], session_epoch=1, motion_version=ref["motion_version"], + image_stamp_ns=ref["samples"][0]["image_stamp_ns"] - 2 + i, + command_vector=row["command_vector_u8"], feedback_vector=row["state_u8"], + camera_matrix=matrix.tolist(), sample_phase="zero_approach", + tags={role: observed_tag(role, row[f"{side}_pose_common"]) + for side, role in (("parent", spec.parent_role), ("child", spec.child_role)) if role in roles}) + for i, row in enumerate((source_rows[0], source_rows[-1]))] + payload = dict(task_name=task.key, view=spec.view, zero_joints=[name], session_epoch=1, + motion_version=ref["motion_version"], source_image_stamps=[r["image_stamp_ns"] for r in source_frames], + source_frame_hashes={str(r["image_stamp_ns"]): source_frame_sha256(r) for r in source_frames}, + constraints=[dict(child_role=role, axis=[1., 0., 0.]) for role in roles]) + payloads = {role: {**payload, "tag_role": role} for role in roles} + authorizations[name] = {role: evidence_digest(value) for role, value in payloads.items()} + journal.extend(source_frames) + journal.append(dict(kind="motion_branch_initialization", status="resolved", reason="", **payload, + evidence_payloads=payloads, evidence_ids=authorizations[name])) + zeros = [] + for sample in ref["samples"]: + row = {**sample, "kind": "joint_zero_sample", "joint": name, "task_name": task.key, + "sample_phase": "joint_zero", "command_vector_u8": sample["command"], + "state_u8": sample["feedback"], "command_direction_by_index": sample["arrival_directions"]} + row["pnp_observation_evidence"] = evidence(row, False) + zeros.append(row) + journal.extend(zeros) + references[name] = build_joint_zero_reference(profile, name, zeros, + session_epoch=1, motion_version=ref["motion_version"]) + journal.append(references[name].as_record()) + units = [u for u in engine.scan_units() if u.task_key == task.key] + for unit in units: + if unit.cycle == 2 and profile.quality.task_training_cycles.get(task.key) == (0, 1): + continue + frames = {} + samples = by_unit[unit.identity] + for row in samples: + row["pnp_observation_evidence"] = evidence(row, True) + frame_key = (row["view"], row["image_stamp_ns"]) + frame = frames.setdefault(frame_key, dict(kind="pnp_candidate_frame", view=row["view"], + image_stamp_ns=row["image_stamp_ns"], task_name=task.key, cycle=unit.cycle, + direction=unit.direction, attempt=1, sample_phase=row["sample_phase"], + **segment_metadata(unit.segment_key), roles={}, camera_matrix=matrix.tolist(), + camera_matrix_source="CameraInfo.P[:3,:3]", input_is_rectified=True)) + spec = profile.measurement.measurements[row["joint"]] + for side, role in (("parent", spec.parent_role), ("child", spec.child_role)): + frame["roles"][role] = observed_tag(role, row[f"{side}_pose_common"]) + if row["sample_phase"] == "steady": + frame.update(command_vector=row["command_vector_u8"], feedback_vector=row["state_u8"], + command_direction_by_index=row["command_direction_by_index"], state_image_sync_error_ns=0) + journal.extend(frames.values()) + journal.extend({**frame, "kind": "image_observation_frame", "tags": frame["roles"], + "policy": IMAGE_OBSERVATION_POLICY, "command_unit": "u8"} + for frame in frames.values() if frame["sample_phase"] == "steady") + journal.extend(samples) + journal.append(dict(kind="scan_unit_complete", task_name=task.key, cycle=unit.cycle, + direction=unit.direction, **segment_metadata(unit.segment_key), attempt=1, passed=True)) + if adaptive and unit.cycle in {1, 2} and unit == [u for u in units if u.cycle == unit.cycle][-1]: + from linkerhand_calibration.core.fitting.training_quality import assess_training, training_snapshot + cycles = tuple(range(unit.cycle + 1)) + decision = assess_training(profile, task.key, cycles, + training_snapshot(profile, journal, task.key, cycles), references, source) + journal.append(decision) + if decision["decision"] == "freeze": + profile = select_task_training(profile, task.key, cycles) + elif decision["decision"] == "fail": + raise AssertionError(decision) + return profile, source, journal, hashes, camera_file, truth diff --git a/src/linkerhand_calibration/test/runtime_host_fixture.py b/src/linkerhand_calibration/test/runtime_host_fixture.py index a548166..34ff438 100644 --- a/src/linkerhand_calibration/test/runtime_host_fixture.py +++ b/src/linkerhand_calibration/test/runtime_host_fixture.py @@ -2,6 +2,7 @@ from pathlib import Path from types import SimpleNamespace +import json import yaml @@ -28,6 +29,12 @@ def coordinator_fixture(tmp_path, *, model="o6", finalization=None, diagnostic_c "resume_raw_samples_path": str(resume_raw_samples_path) if resume_raw_samples_path else "", "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 profile.vision_motion: + seed = tmp_path / "offline_initial_command.json" + seed.write_text(json.dumps(dict(source="sdk_target_position_register", commands_sent=False, + model=profile.key.model, side=profile.key.side, device_uid="OFFLINE_DEVICE", + values=list(profile.command.baseline_values)))) + values["initial_command_file"] = str(seed) parameters = load_runtime_parameters(profile, values.__getitem__) clock = SimpleNamespace(now=10., publishers=1, positions=[], settings=[], health_receiver=None) ports = RuntimePorts(lambda: int(clock.now*1e9), lambda: clock.now, lambda: clock.publishers, @@ -35,7 +42,9 @@ def coordinator_fixture(tmp_path, *, model="o6", finalization=None, diagnostic_c def subscribe_health(callback): clock.health_receiver = callback - callback('{"position_mode": true, "active_faults": []}') + callback(json.dumps(dict(hand_type=profile.key.side, model=profile.key.model, + side=profile.key.side.upper(), uid="OFFLINE_DEVICE", joint_names=list(profile.command.names), + online=True, position_mode=True, active_faults=[], joint_faults={}))) def factory(publish, set_speed, fresh): return bind_ros_sdk(profile, SdkBindingPorts(fresh, ports.monotonic, subscribe_health), diff --git a/src/linkerhand_calibration/test/test_adaptive_command_sampling.py b/src/linkerhand_calibration/test/test_adaptive_command_sampling.py index f88d25f..dc8194d 100644 --- a/src/linkerhand_calibration/test/test_adaptive_command_sampling.py +++ b/src/linkerhand_calibration/test/test_adaptive_command_sampling.py @@ -60,6 +60,21 @@ def test_all_models_reduce_linear_training_and_keep_validation_unchanged(name): 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]) diff --git a/src/linkerhand_calibration/test/test_adaptive_training.py b/src/linkerhand_calibration/test/test_adaptive_training.py new file mode 100644 index 0000000..97bc697 --- /dev/null +++ b/src/linkerhand_calibration/test/test_adaptive_training.py @@ -0,0 +1,159 @@ +"""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_all_view_image_capture.py b/src/linkerhand_calibration/test/test_all_view_image_capture.py index 66795a8..cc7c8c1 100644 --- a/src/linkerhand_calibration/test/test_all_view_image_capture.py +++ b/src/linkerhand_calibration/test/test_all_view_image_capture.py @@ -12,15 +12,18 @@ from runtime_host_fixture import coordinator_fixture, ready @pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) -def test_non_task_camera_records_measured_corners_without_a_pose(tmp_path, monkeypatch, model): +@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] - host._motion = MotionCommand("sweep", host.profile.command.baseline_values, 1., - task_key=task.key, command_index=task.command_index, cycle=2, direction="decreasing") + 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.)) @@ -35,7 +38,7 @@ def test_non_task_camera_records_measured_corners_without_a_pose(tmp_path, monke 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, "decreasing", "sweep") + 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) diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index 8375f63..366a6c0 100644 --- a/src/linkerhand_calibration/test/test_architecture.py +++ b/src/linkerhand_calibration/test/test_architecture.py @@ -147,15 +147,21 @@ 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(item, ast.Constant) and isinstance(item.value, str) and item.value in forbidden + 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 item in ast.walk(operand)), path + 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 { diff --git a/src/linkerhand_calibration/test/test_axis_line_gauge.py b/src/linkerhand_calibration/test/test_axis_line_gauge.py index 9e53d0e..5be1367 100644 --- a/src/linkerhand_calibration/test/test_axis_line_gauge.py +++ b/src/linkerhand_calibration/test/test_axis_line_gauge.py @@ -18,7 +18,8 @@ from linkerhand_calibration.profiles import load_bundled_hand_profile def test_physical_zero_is_recovered_for_any_axis_reference_point(layout, view): package = Path(__file__).resolve().parents[1] model_name = layout.split("_")[0] - source, = (package / "urdf" / f"{model_name}_right").glob("*.urdf") + 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, {}, {}, {}) diff --git a/src/linkerhand_calibration/test/test_capture_journal.py b/src/linkerhand_calibration/test/test_capture_journal.py index 64a753e..e708b60 100644 --- a/src/linkerhand_calibration/test/test_capture_journal.py +++ b/src/linkerhand_calibration/test/test_capture_journal.py @@ -37,14 +37,16 @@ def test_sweep_records_do_not_wait_for_disk_sync_and_pause_persists_them(tmp_pat finally: release.set() assert host.raw_records[0] == storage.load_jsonl(host.raw_path)[0] - assert host.raw_records[1:] == rows - assert storage.load_jsonl(host.raw_path)[-2:] == rows + 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][-3:-1] == rows + assert persisted[-1][-len(captured)-1:-1] == captured finally: release.set() host.close() diff --git a/src/linkerhand_calibration/test/test_capture_plan.py b/src/linkerhand_calibration/test/test_capture_plan.py new file mode 100644 index 0000000..4990f9f --- /dev/null +++ b/src/linkerhand_calibration/test/test_capture_plan.py @@ -0,0 +1,153 @@ +"""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.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"] + + +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 index 0b575cb..d38b066 100644 --- a/src/linkerhand_calibration/test/test_capture_provenance.py +++ b/src/linkerhand_calibration/test/test_capture_provenance.py @@ -77,8 +77,8 @@ def test_unknown_capture_schedule_cannot_publish(schedule): validate_capture_provenance(profile, "VIRTUAL", hashes, rows) -def test_offline_units_use_live_quality_and_require_completed_last_attempt(): - profile, _, rows = journal() +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) @@ -95,6 +95,11 @@ def test_offline_units_use_live_quality_and_require_completed_last_attempt(): 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 @@ -103,3 +108,76 @@ def test_offline_units_use_live_quality_and_require_completed_last_attempt(): 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 new file mode 100644 index 0000000..01cb1fe --- /dev/null +++ b/src/linkerhand_calibration/test/test_capture_runtime_index.py @@ -0,0 +1,69 @@ +"""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_checkpoint_extension.py b/src/linkerhand_calibration/test/test_checkpoint_extension.py new file mode 100644 index 0000000..a81b5cc --- /dev/null +++ b/src/linkerhand_calibration/test/test_checkpoint_extension.py @@ -0,0 +1,77 @@ +"""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_command_motion.py b/src/linkerhand_calibration/test/test_command_motion.py new file mode 100644 index 0000000..8e2417b --- /dev/null +++ b/src/linkerhand_calibration/test/test_command_motion.py @@ -0,0 +1,71 @@ +"""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_diagnostic_presets_cli.py b/src/linkerhand_calibration/test/test_diagnostic_presets_cli.py new file mode 100644 index 0000000..8f0408a --- /dev/null +++ b/src/linkerhand_calibration/test/test_diagnostic_presets_cli.py @@ -0,0 +1,54 @@ +"""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_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_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']) + 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 + + +@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_feedback_reversal_progress.py b/src/linkerhand_calibration/test/test_feedback_reversal_progress.py new file mode 100644 index 0000000..1e194a9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_feedback_reversal_progress.py @@ -0,0 +1,131 @@ +"""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_sources.py b/src/linkerhand_calibration/test/test_final_image_sources.py index 23f6137..1b20a84 100644 --- a/src/linkerhand_calibration/test/test_final_image_sources.py +++ b/src/linkerhand_calibration/test/test_final_image_sources.py @@ -1,6 +1,7 @@ """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 @@ -18,6 +19,14 @@ def scene(): 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 diff --git a/src/linkerhand_calibration/test/test_frozen_evidence_storage.py b/src/linkerhand_calibration/test/test_frozen_evidence_storage.py new file mode 100644 index 0000000..0cd294d --- /dev/null +++ b/src/linkerhand_calibration/test/test_frozen_evidence_storage.py @@ -0,0 +1,84 @@ +"""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_held_joint_replay.py b/src/linkerhand_calibration/test/test_held_joint_replay.py index e82b1bd..d75a405 100644 --- a/src/linkerhand_calibration/test/test_held_joint_replay.py +++ b/src/linkerhand_calibration/test/test_held_joint_replay.py @@ -24,7 +24,7 @@ def virtual_capture(tmp_path): ''') 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(command=NS(unit="rad", urdf_joint_by_joint={}), + 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=( diff --git a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py index 24127a2..fa02a0d 100644 --- a/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py +++ b/src/linkerhand_calibration/test/test_image_motion_capture_evidence.py @@ -3,11 +3,13 @@ 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 @@ -87,6 +89,10 @@ def test_actual_capture_writes_replayable_image_model_when_candidate_strategy_is 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): diff --git a/src/linkerhand_calibration/test/test_image_motion_live_flow.py b/src/linkerhand_calibration/test/test_image_motion_live_flow.py index 56ede84..aeac63a 100644 --- a/src/linkerhand_calibration/test/test_image_motion_live_flow.py +++ b/src/linkerhand_calibration/test/test_image_motion_live_flow.py @@ -5,6 +5,7 @@ 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 @@ -18,12 +19,15 @@ from test_diagnostic_coordinator import run_until, THUMB_JOINTS from runtime_host_fixture import coordinator_fixture, ready -def test_recorded_image_geometry_completes_thumb_coordinator_without_publication(tmp_path, monkeypatch): +@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") + 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(): diff --git a/src/linkerhand_calibration/test/test_image_motion_provenance.py b/src/linkerhand_calibration/test/test_image_motion_provenance.py index 5fd42df..a9d0b1b 100644 --- a/src/linkerhand_calibration/test/test_image_motion_provenance.py +++ b/src/linkerhand_calibration/test/test_image_motion_provenance.py @@ -154,6 +154,11 @@ def image_motion_capture(): 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} diff --git a/src/linkerhand_calibration/test/test_interleaved_motion_efficiency.py b/src/linkerhand_calibration/test/test_interleaved_motion_efficiency.py new file mode 100644 index 0000000..a28a45b --- /dev/null +++ b/src/linkerhand_calibration/test/test_interleaved_motion_efficiency.py @@ -0,0 +1,88 @@ +"""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(): + profile = load_bundled_hand_profile("o30_right_18", scope="full") + 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_measured_transfer.py b/src/linkerhand_calibration/test/test_measured_transfer.py new file mode 100644 index 0000000..3bfcab3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_measured_transfer.py @@ -0,0 +1,198 @@ +"""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 diff --git a/src/linkerhand_calibration/test/test_native_position_feedback.py b/src/linkerhand_calibration/test/test_native_position_feedback.py new file mode 100644 index 0000000..1b1638d --- /dev/null +++ b/src/linkerhand_calibration/test/test_native_position_feedback.py @@ -0,0 +1,386 @@ +"""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") + 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_o30_artifacts.py b/src/linkerhand_calibration/test/test_o30_artifacts.py new file mode 100644 index 0000000..fa41887 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o30_artifacts.py @@ -0,0 +1,124 @@ +"""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 new file mode 100644 index 0000000..3910722 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o30_launch.py @@ -0,0 +1,101 @@ +"""Construct protected O30 launch actions using synthetic cameras, without ROS processes.""" + +from pathlib import Path +import hashlib +import yaml + +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() + + +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 new file mode 100644 index 0000000..26a5bc7 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o30_profile.py @@ -0,0 +1,338 @@ +"""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 == .0165 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,113,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_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) == 16 + for cycle in range(4): + 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.,113.,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), + 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)) <= 30*.01+1. + assert math.pi*255/(2*execution.duration) <= 30.000001 + previous = actual + + +@pytest.mark.parametrize(('scope', 'expected_units'), [('full', 144), ('without_finger_roll', 128)]) +def test_complete_motion_driver_clearances_own_channels_and_retry(scope, expected_units): + profile = load_bundled_hand_profile('o30_right_18', scope=scope) + 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(.0165,(.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 new file mode 100644 index 0000000..60ab895 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o30_side_observer.py @@ -0,0 +1,69 @@ +"""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_observation_feedback_dependencies.py b/src/linkerhand_calibration/test/test_observation_feedback_dependencies.py new file mode 100644 index 0000000..8059f08 --- /dev/null +++ b/src/linkerhand_calibration/test/test_observation_feedback_dependencies.py @@ -0,0 +1,74 @@ +"""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_parallel_image_replay.py b/src/linkerhand_calibration/test/test_parallel_image_replay.py new file mode 100644 index 0000000..e5fe57b --- /dev/null +++ b/src/linkerhand_calibration/test/test_parallel_image_replay.py @@ -0,0 +1,49 @@ +"""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 new file mode 100644 index 0000000..e65c2fd --- /dev/null +++ b/src/linkerhand_calibration/test/test_parent_reference_provenance.py @@ -0,0 +1,168 @@ +"""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"]) +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 + 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 new file mode 100644 index 0000000..cc134f5 --- /dev/null +++ b/src/linkerhand_calibration/test/test_parent_reference_reuse.py @@ -0,0 +1,196 @@ +"""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.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) + 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_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 + 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_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 new file mode 100644 index 0000000..0900dfc --- /dev/null +++ b/src/linkerhand_calibration/test/test_partial_o30_calibration.py @@ -0,0 +1,161 @@ +"""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, 113, 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, 113, 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 new file mode 100644 index 0000000..92b2204 --- /dev/null +++ b/src/linkerhand_calibration/test/test_passed_resume.py @@ -0,0 +1,125 @@ +"""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 + + +@pytest.mark.parametrize('failure', [None, 'missing', 'failed', 'support', 'new_attempt']) +def test_last_group_failure_keeps_128_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) == (144 if failure is None else 128) + assert len(resume.references) == (20 if failure is None else 16) + assert resume.verified == set(resume.references) + + +@pytest.mark.parametrize('interruption', [None, 'abort', 'feedback']) +def test_direct_resume_imports_durably_then_skips_old_joint_motion(tmp_path, monkeypatch, interruption): + host, clock = coordinator_fixture(tmp_path) + 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) + for i in range(257)] + 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) + passed_resume.stage_passed_tasks(resume, host.execution.session.engine, rows) + host._resume_checkpoint = SimpleNamespace(joints=resume, + passed_import=passed_resume.reference_import(resume), + 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: + for _ in range(20): + 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 + assert host._resumed_count == expected_units + assert host.execution.zero_references == references + assert host.segment is None + import json + imported = [json.loads(line) for line in host.raw_path.read_text().splitlines()] + 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_preparation_admission.py b/src/linkerhand_calibration/test/test_preparation_admission.py new file mode 100644 index 0000000..24a94ba --- /dev/null +++ b/src/linkerhand_calibration/test/test_preparation_admission.py @@ -0,0 +1,96 @@ +"""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_prepared_resume.py b/src/linkerhand_calibration/test/test_prepared_resume.py index 2089fde..bc797a1 100644 --- a/src/linkerhand_calibration/test/test_prepared_resume.py +++ b/src/linkerhand_calibration/test/test_prepared_resume.py @@ -123,3 +123,68 @@ def test_large_task_import_yields_to_feedback_and_commits_only_after_fsync( 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 types import SimpleNamespace + 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 = SimpleNamespace(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_recorded_side_preparation.py b/src/linkerhand_calibration/test/test_recorded_side_preparation.py index c714d6d..57d0a25 100644 --- a/src/linkerhand_calibration/test/test_recorded_side_preparation.py +++ b/src/linkerhand_calibration/test/test_recorded_side_preparation.py @@ -2,11 +2,12 @@ import gzip import json +from dataclasses import replace from pathlib import Path -from types import SimpleNamespace 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, ) @@ -21,13 +22,13 @@ def test_recorded_side_preserves_holdout_and_uses_source_geometry(): profile = config.calibration_contract.typed_profile first = rows[0] owner = BranchInitialization(profile, source_urdf=config.source_urdf) - motion = SimpleNamespace(phase="zero_approach", task_key=first["task_name"], + 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.phase, motion.zero_joints = "joint_zero", motion.reference_joints + 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, diff --git a/src/linkerhand_calibration/test/test_resume_storage.py b/src/linkerhand_calibration/test/test_resume_storage.py new file mode 100644 index 0000000..fbcdc9e --- /dev/null +++ b/src/linkerhand_calibration/test/test_resume_storage.py @@ -0,0 +1,76 @@ +"""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.core.domain.motion_path import record_scan_identity +from linkerhand_calibration.runtime.diagnostic_capture import reject_diagnostic_capture + + +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.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) diff --git a/src/linkerhand_calibration/test/test_resumed_image_reference.py b/src/linkerhand_calibration/test/test_resumed_image_reference.py new file mode 100644 index 0000000..776aaeb --- /dev/null +++ b/src/linkerhand_calibration/test/test_resumed_image_reference.py @@ -0,0 +1,177 @@ +"""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_runtime_gc_lifecycle.py b/src/linkerhand_calibration/test/test_runtime_gc_lifecycle.py new file mode 100644 index 0000000..25dc494 --- /dev/null +++ b/src/linkerhand_calibration/test/test_runtime_gc_lifecycle.py @@ -0,0 +1,77 @@ +"""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 index 475fbb5..7aff909 100644 --- a/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py +++ b/src/linkerhand_calibration/test/test_runtime_pose_branch_guard.py @@ -70,8 +70,9 @@ def test_branch_events_pause_only_current_required_roles_after_version_checks(tm 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 host.raw_records[1:] == [row] - assert host._unit_rows == host._steady_rows == [] + 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 diff --git a/src/linkerhand_calibration/test/test_runtime_readiness.py b/src/linkerhand_calibration/test/test_runtime_readiness.py index b6b5efb..9a94bfd 100644 --- a/src/linkerhand_calibration/test/test_runtime_readiness.py +++ b/src/linkerhand_calibration/test/test_runtime_readiness.py @@ -40,6 +40,7 @@ def test_missing_stale_and_recovered_camera_pipeline_are_named_without_tag_gate( 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, @@ -55,6 +56,9 @@ def test_steady_checkpoint_does_not_erase_direction_statistics(): 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): diff --git a/src/linkerhand_calibration/test/test_safety_policy.py b/src/linkerhand_calibration/test/test_safety_policy.py index a13ea3f..b50387f 100644 --- a/src/linkerhand_calibration/test/test_safety_policy.py +++ b/src/linkerhand_calibration/test/test_safety_policy.py @@ -1,10 +1,15 @@ 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"]) @@ -78,3 +83,35 @@ def test_two_second_no_progress_stops_but_ordinary_lag_does_not(profile) -> None 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_session_execution.py b/src/linkerhand_calibration/test/test_session_execution.py index 9dd7a19..c40583f 100644 --- a/src/linkerhand_calibration/test/test_session_execution.py +++ b/src/linkerhand_calibration/test/test_session_execution.py @@ -48,7 +48,7 @@ def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout, capt 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: + if not failed_once and (capture_mode == "separate" or motion.phase == "steady"): failed_once = True else: start = initial[task.command_index] @@ -75,13 +75,17 @@ def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout, capt 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] == ["steady"] + ["sweep", "steady"]*(len(points)-1) + 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) @@ -91,7 +95,7 @@ def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout, capt for _, initial, motion in trace: delta = motion.target[task.command_index]-initial[task.command_index] if motion.phase == "sweep": - assert sign*delta > 0 + assert sign*delta > 0 or (capture_mode == "interleaved" and (initial, motion) == trace[0][1:]) elif capture_mode == "interleaved": assert delta == pytest.approx(0.) else: diff --git a/src/linkerhand_calibration/test/test_shared_pose_conditioning.py b/src/linkerhand_calibration/test/test_shared_pose_conditioning.py new file mode 100644 index 0000000..bd9b05e --- /dev/null +++ b/src/linkerhand_calibration/test/test_shared_pose_conditioning.py @@ -0,0 +1,170 @@ +"""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_tag_pose_bridge.py b/src/linkerhand_calibration/test/test_shared_tag_pose_bridge.py new file mode 100644 index 0000000..9b8b0f4 --- /dev/null +++ b/src/linkerhand_calibration/test/test_shared_tag_pose_bridge.py @@ -0,0 +1,243 @@ +"""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 + + +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 + 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 diff --git a/src/linkerhand_calibration/test/test_shared_zero_conditioning.py b/src/linkerhand_calibration/test/test_shared_zero_conditioning.py new file mode 100644 index 0000000..746e38e --- /dev/null +++ b/src/linkerhand_calibration/test/test_shared_zero_conditioning.py @@ -0,0 +1,169 @@ +"""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 index 3e8cfeb..cb6b196 100644 --- a/src/linkerhand_calibration/test/test_single_pass_capture.py +++ b/src/linkerhand_calibration/test/test_single_pass_capture.py @@ -114,7 +114,7 @@ def test_formal_yaw_four_round_trips_use_one_pass_and_disjoint_sample_phases(tmp targets = command_nodes(host.profile, task, holdout=unit.cycle == 3) if unit.direction == "decreasing": targets = tuple(reversed(targets)) - expected = [("steady", targets[0], 0)] + 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 diff --git a/src/linkerhand_calibration/test/test_stationary_image_reference.py b/src/linkerhand_calibration/test/test_stationary_image_reference.py new file mode 100644 index 0000000..2b526c8 --- /dev/null +++ b/src/linkerhand_calibration/test/test_stationary_image_reference.py @@ -0,0 +1,271 @@ +"""Fixed image witnesses do not need a physical planar normal. + +Recorded frontal corners test the original failure. Independent projected +hinges test geometry, transactional capture and the immutable replay boundary. +""" +from dataclasses import asdict, replace +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.core.geometry.tag_pose.image_reference import ( + IMAGE_REFERENCE_SOURCE, ReferenceImage, StationaryImageReference, + coordinate_origin, image_reference_from_dict, reference_from_evidence, +) +from linkerhand_calibration.core.geometry.tag_pose.image_motion_model import image_model_payload +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, select_image_motion_frame, image_motion_model_from_dict, +) +from linkerhand_calibration.core.geometry.tag_pose.tracking import SquareTagPoseTracker +from linkerhand_calibration.core.geometry.image_motion_replay import ( + replay_image_models, validate_image_sample, image_model_sha256, +) +from linkerhand_calibration.runtime.image_reference import ImageReferenceTracker +from linkerhand_calibration.runtime.pose_evidence import snapshot_pose_evidence +from test_motion_image_diagnostics import frames, RELATIONS + + +def reference_from_frame(frame): + observation = frame.observations[0] + images = tuple(ReferenceImage(i, observation.corners_xy) for i in range(1, 11)) + return StationaryImageReference(observation.role, observation.tag_size_m, frame.camera_matrix, + images, coordinate_origin(images, observation.tag_size_m, frame.camera_matrix)) + + +def in_reference(frame, reference): + return replace(frame, reference_frames=(reference,), evidence=replace(frame.evidence, roles=( + RoleCandidates(reference.role, (reference.pose,), True), *frame.evidence.roles[1:]))) + + +@pytest.fixture(scope="module") +def resolved_reference(): + original = frames(depth_error=.008) + reference = reference_from_frame(original[0]) + source = tuple(in_reference(frame, reference) for frame in original) + result = resolve_image_motion(source, RELATIONS) + assert result.resolved, result + return source, result.model, reference + + +def test_cached_reference_properties_preserve_identity_and_corner_isolation(resolved_reference): + import hashlib + + reference = resolved_reference[2] + payload = asdict(reference) + expected = hashlib.sha256(json.dumps(payload, sort_keys=True, + separators=(",", ":"), allow_nan=False).encode()).hexdigest() + assert reference.identity == expected + corners = reference.corners_xy.copy() + reference.corners_xy[:] = 0 + assert np.array_equal(reference.corners_xy, corners) + assert asdict(reference) == payload + changed = replace(reference, origin_xyz_m=(reference.origin_xyz_m[0] + .001, + *reference.origin_xyz_m[1:])) + assert changed.identity != reference.identity + + +def test_recorded_frontal_base_locks_without_choosing_either_normal(): + fixture = json.loads((Path(__file__).parent / "fixtures/o30_frontal_reference.json").read_text()) + reference, physical = ImageReferenceTracker(10), SquareTagPoseTracker() + physical_selected = [] + for image in fixture["images"]: + args = dict(stamp_ns=image["stamp_ns"]) + pose, reason = reference.estimate("base", image["corners_xy"], matrix=fixture["camera_matrix"], + size=fixture["tag_size_m"], locking=True, **args) + actual, _ = physical.estimate("base", image["corners_xy"], camera_matrix=fixture["camera_matrix"], + tag_size_m=fixture["tag_size_m"], **args) + physical_selected.append(actual) + assert not any(physical_selected) + assert pose is not None and not reason + candidates, diagnostics, _ = reference.observation_snapshot("base", image["stamp_ns"]) + assert len(candidates) == 2 and pose not in candidates + separation = (Rotation.from_quat(candidates[0].quaternion_xyzw).inv() + * Rotation.from_quat(candidates[1].quaternion_xyzw)).magnitude() + assert np.rad2deg(separation) > 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 index e52fc06..131182d 100644 --- a/src/linkerhand_calibration/test/test_steady_feedback_window.py +++ b/src/linkerhand_calibration/test/test_steady_feedback_window.py @@ -112,10 +112,10 @@ def test_feedback_before_trajectory_end_cannot_establish_the_hold_window(profile assert segment.finished_at is None arrival = segment.duration + .1 for delay in (0., .02, .04): - assert not observe(segment, arrival + delay) + assert not observe(segment, arrival + delay, feedback=target) assert segment.finished_at == arrival for delay in (.10, .20, .30): - stable = observe(segment, arrival + delay) + stable = observe(segment, arrival + delay, feedback=target) assert stable diff --git a/src/linkerhand_calibration/test/test_task_input_support.py b/src/linkerhand_calibration/test/test_task_input_support.py new file mode 100644 index 0000000..6da33d8 --- /dev/null +++ b/src/linkerhand_calibration/test/test_task_input_support.py @@ -0,0 +1,97 @@ +"""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_transfer_artifact_contract.py b/src/linkerhand_calibration/test/test_transfer_artifact_contract.py index 475f99d..2524fea 100644 --- a/src/linkerhand_calibration/test/test_transfer_artifact_contract.py +++ b/src/linkerhand_calibration/test/test_transfer_artifact_contract.py @@ -45,11 +45,13 @@ def transfer_artifacts(tmp_path): 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), + measurement=SimpleNamespace(transferred_motion_sources=transfers, release_basis="feedback_and_command"), artifacts=SimpleNamespace(directional_command_joints=frozenset()), - acquisition=SimpleNamespace(command_capture_mode="interleaved"), + 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)) diff --git a/src/linkerhand_calibration/test/test_transferred_hinge_geometry.py b/src/linkerhand_calibration/test/test_transferred_hinge_geometry.py new file mode 100644 index 0000000..12bbefd --- /dev/null +++ b/src/linkerhand_calibration/test/test_transferred_hinge_geometry.py @@ -0,0 +1,126 @@ +"""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_unified_runner.py b/src/linkerhand_calibration/test/test_unified_runner.py index d965adb..28e1fc0 100644 --- a/src/linkerhand_calibration/test/test_unified_runner.py +++ b/src/linkerhand_calibration/test/test_unified_runner.py @@ -113,6 +113,41 @@ def test_start_service_response_has_its_own_deadline(): 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", @@ -180,11 +215,32 @@ def test_passed_capture_cannot_be_reused_to_fake_an_independent_session(tmp_path 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"]) +@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 @@ -202,7 +258,16 @@ def test_common_online_process_chain_with_virtual_ros_and_no_hardware(monkeypatc calls = [] launched = [] ros_active = [False] - def init(): + 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 @@ -211,8 +276,15 @@ def test_common_online_process_chain_with_virtual_ros_and_no_hardware(monkeypatc launched.append((command, kwargs)) return harness def spin(_node, **_kwargs): - if launched: + 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 @@ -234,10 +306,16 @@ def test_common_online_process_chain_with_virtual_ros_and_no_hardware(monkeypatc monkeypatch.setattr(online_artifacts, "finish_online_artifacts", finish) code = runner.run_online(config, allow_resume=False) - assert code == (0 if terminal == "COMPLETE" else 3) + 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 diff --git a/src/linkerhand_calibration/test/test_visual_motion.py b/src/linkerhand_calibration/test/test_visual_motion.py new file mode 100644 index 0000000..3069117 --- /dev/null +++ b/src/linkerhand_calibration/test/test_visual_motion.py @@ -0,0 +1,266 @@ +"""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_zero_approach_path.py b/src/linkerhand_calibration/test/test_zero_approach_path.py new file mode 100644 index 0000000..537031a --- /dev/null +++ b/src/linkerhand_calibration/test/test_zero_approach_path.py @@ -0,0 +1,105 @@ +"""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_recovery.py b/src/linkerhand_calibration/test/test_zero_recovery.py index 902bf48..e1e870b 100644 --- a/src/linkerhand_calibration/test/test_zero_recovery.py +++ b/src/linkerhand_calibration/test/test_zero_recovery.py @@ -1,5 +1,6 @@ """Bounded fresh preparation, no replacement of already frozen joint zeros.""" +from dataclasses import replace from types import SimpleNamespace import numpy as np @@ -13,10 +14,12 @@ 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")) + 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" @@ -25,7 +28,8 @@ def test_recovery_is_bounded_and_rejects_frozen_or_faulty_inputs(): @pytest.mark.parametrize("persistent", [False, True]) -def test_transient_preparation_retries_locally_and_persistent_failure_stops(tmp_path, persistent): +@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: @@ -34,6 +38,12 @@ def test_transient_preparation_retries_locally_and_persistent_failure_stops(tmp_ 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 diff --git a/src/linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf b/src/linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf new file mode 100644 index 0000000..669d612 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o30_right/linkerhand_O30i_right-V2_0819.urdf @@ -0,0 +1,1209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/hand_base_link.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/hand_base_link.STL new file mode 100644 index 0000000..5b2192c Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/hand_base_link.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/index_distal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/index_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/index_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/index_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/index_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/index_middle.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/index_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/index_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/index_proximal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/index_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/middle_distal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/middle_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/middle_middle.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/middle_proximal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_distal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_middle.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_proximal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/ring_distal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_distal.STL new file mode 100644 index 0000000..f9e29d1 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/ring_metacarpals.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_metacarpals.STL new file mode 100644 index 0000000..d3de7a3 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/ring_middle.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_middle.STL new file mode 100644 index 0000000..4c0649f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/ring_proximal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_proximal.STL new file mode 100644 index 0000000..911c629 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_distal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_distal.STL new file mode 100644 index 0000000..0dec965 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..af0e866 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..0a1a390 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_metacarpals_base2.STL differ diff --git a/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_proximal.STL b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_proximal.STL new file mode 100644 index 0000000..c517c21 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o30_right/meshes/thumb_proximal.STL differ