O30标定urdf
This commit is contained in:
@@ -0,0 +1,60 @@
|
||||
schema_version: 1
|
||||
reference_view: front
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
width: 1624
|
||||
height: 1240
|
||||
intrinsics_sha256: 5752443dfe64fb47440151d793544ec3a996891b03fa387459a7ac8c2328dc07
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
width: 1624
|
||||
height: 1240
|
||||
intrinsics_sha256: 25ca0a3f68ccfd1f85d1bdeb9149052bad00c4d4b1f5a88a438a4a143e07e655
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
width: 1624
|
||||
height: 1240
|
||||
intrinsics_sha256: 11cca902dc5493c92d1191cda4d25a30facbb5947428e7141c3ebfe078500446
|
||||
front_from_view:
|
||||
front:
|
||||
translation_xyz_m:
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
quaternion_xyzw:
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 1.0
|
||||
side:
|
||||
translation_xyz_m:
|
||||
- -0.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
|
||||
@@ -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
|
||||
@@ -0,0 +1,8 @@
|
||||
# 标定包协作约定
|
||||
|
||||
- 全程用中文；保留工作区已有修改。
|
||||
- 测试选择遵循 [TESTING.md](TESTING.md)。日常运行受影响用例及必要契约，常规验证目标 60 秒内。
|
||||
- 不默认运行全量 pytest、colcon test 或所有型号整手拟合。公共契约变更与发布节点再扩大范围。
|
||||
- 同一代码版本已通过的测试不无理由重复执行；修复失败后只重跑失败或受修改影响的用例，并记录耗时。
|
||||
- 优先复用不可变输入及已生成产物，在验收边界测试拒绝条件；不为每种错误重复生成整手标定。
|
||||
- 软件回放通过与实机连续通过分开报告；不能为缩短测试或减少暂停放宽精度、可观测性和发布门限。
|
||||
@@ -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。
|
||||
|
||||
本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。
|
||||
@@ -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_<view>.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 移除原被动关节的 `<mimic>`，全部关节角由 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，并进一步说明：
|
||||
非线性关节也必须保留 `<mimic>` 自动联动，单独 URDF 尽量接近实机；
|
||||
需要实测非线性结果时再由标定 JSON 查找表控制。上一轮删除 mimic 的正式导出方向
|
||||
不符合这一使用要求。已核对网站当前构建及官方 URDFAdapter：滑条直接设置 URDF 关节，
|
||||
没有本项目标定 JSON 的读取逻辑，不能期望它自动获得 JSON 中的非线性曲线。
|
||||
|
||||
### 方案评估与实现
|
||||
|
||||
标准 mimic 使用 `q_child = a*q_parent+b`。将 `b` 强制为 0，再约束父子全行程，
|
||||
使拇指的两个微小负端点决定全局倍率上限，这是旧版 1.16556 倍率失真的原因。
|
||||
仅恢复 `<mimic>`、手填倍率或扩大限位都不能解决模型与约束之间的矛盾。
|
||||
|
||||
现在使用两端子关节角度作为拟合变量：
|
||||
|
||||
- 父关节输出范围 `[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 的边界保持原记录。
|
||||
@@ -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` 通过。
|
||||
- 软件回归及合成数据验收不代表实机精度已达标；当前仍没有完成整手最终产物验收。
|
||||
- 安装环境解析到当前源码目录，新增的共享验收模块可直接导入；没有启动实机程序验证。
|
||||
@@ -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
|
||||
```
|
||||
@@ -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 哈希保持不变。
|
||||
本次未启动实机；新的现场精度及正式产物仍需实际完整采集与独立验收。
|
||||
@@ -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；尚未轮到的手指遮挡不阻塞当前任务。
|
||||
零位和稳态点的反馈稳定窗口以最新独立反馈为终点，并保留跨越窗口起点的一条反馈。
|
||||
|
||||
@@ -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 秒。记录保留开发期间
|
||||
的失败及修复后复查关系，不将不同代码版本的增量检查称为一次最终全量回归。
|
||||
@@ -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]
|
||||
@@ -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
|
||||
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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="",
|
||||
|
||||
@@ -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={}))
|
||||
@@ -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 {}
|
||||
@@ -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}
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
+33
-25
@@ -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)}
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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))
|
||||
|
||||
+62
-178
@@ -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(
|
||||
|
||||
+14
-6
@@ -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),
|
||||
)
|
||||
|
||||
+4
-1
@@ -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(
|
||||
[
|
||||
|
||||
+4
-4
@@ -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
|
||||
|
||||
+23
@@ -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}"
|
||||
)
|
||||
|
||||
+31
-4
@@ -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)
|
||||
|
||||
+7
-2
@@ -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)
|
||||
|
||||
+1
-1
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)}
|
||||
+15
-6
@@ -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')
|
||||
|
||||
@@ -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)
|
||||
+27
-8
@@ -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)
|
||||
|
||||
+38
-9
@@ -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]
|
||||
|
||||
+26
-1
@@ -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")
|
||||
|
||||
+63
@@ -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)
|
||||
+13
-4
@@ -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)
|
||||
|
||||
+44
-5
@@ -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
|
||||
|
||||
+10
@@ -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):
|
||||
|
||||
+180
@@ -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
|
||||
+109
@@ -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))
|
||||
+81
@@ -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)
|
||||
+18
@@ -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]
|
||||
|
||||
+11
@@ -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):
|
||||
|
||||
+2
@@ -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)
|
||||
|
||||
+132
@@ -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)))
|
||||
+71
-44
@@ -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
|
||||
|
||||
+103
@@ -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)
|
||||
+42
@@ -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"]))
|
||||
+14
-1
@@ -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],
|
||||
})
|
||||
|
||||
@@ -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"])
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -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:
|
||||
|
||||
@@ -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():
|
||||
|
||||
@@ -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,
|
||||
)
|
||||
|
||||
@@ -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",)),
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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}})
|
||||
@@ -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)}")
|
||||
|
||||
@@ -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"]
|
||||
|
||||
@@ -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()
|
||||
@@ -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))
|
||||
@@ -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):
|
||||
|
||||
@@ -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":
|
||||
|
||||
+103
@@ -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)
|
||||
@@ -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:
|
||||
|
||||
@@ -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}")
|
||||
|
||||
@@ -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)))
|
||||
|
||||
+41
-12
@@ -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,
|
||||
|
||||
+22
-3
@@ -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)
|
||||
|
||||
+3
-1
@@ -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)
|
||||
|
||||
@@ -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()}))
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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}")
|
||||
|
||||
+35
-5
@@ -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")},
|
||||
|
||||
+1
-1
@@ -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}}
|
||||
|
||||
+169
-32
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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})
|
||||
@@ -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()
|
||||
@@ -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,
|
||||
))
|
||||
|
||||
@@ -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
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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)))
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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: {
|
||||
|
||||
@@ -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))
|
||||
@@ -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)
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user