标定改造方案:按关节锁定零位,输出实测 JSON 与近似 URDF

This commit is contained in:
lxp
2026-09-11 14:43:44 +08:00
parent f94cf2c500
commit 69c2da6808
71 changed files with 1986 additions and 148 deletions
@@ -0,0 +1,122 @@
# 按关节零位与独立实测 JSON v3 实施记录
日期:2026-09-11。范围:`linkerhand_calibration`。
## 实现与坐标约定
`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` |
| 独立线性 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 每个运动关节来自自己的视觉样本,同一 SDK 通道允许绑定多个关节。
正式单条指令曲线只用前三轮,约束 baseline=0;两条方向诊断曲线保留回差,共用同一参考。
第四轮分别检验两方向,不能用于重新拟合曲线、零位、Tag 安装或基座。
序列化只导出冻结结果;u8 必须实测支持 0–255 才能生成 256 项,rad 只在 `input_values` 范围内插值。
URDF mimic 使用前三轮成对实测角度拟合经过零点的直线,输出 offset 精确为 0。
非线性 JSON 与 mimic 近似分别保存。近似误差不否决完整映射;机械范围、拓扑、授权字段及标准加载仍必须合法。
缺少独立运动观测或绝对几何依据时保留相对运动诊断,拒绝完整发布,不能使用旧 mimic、迁移或默认零偏补全。
## 验收与消费
发布器从最终文件回读并分别检验:
1. 最终 JSON 对第四轮原始视觉角度;逐关节、逐方向检查。
2. 最终 JSON+修正 URDF 的完整 FK 对第四轮 Tag 位姿;显式使用所有被动角。
3. 相同主动映射配合标准 mimic 的损失;使用同一份训练冻结安装关系,只报告近似误差。
前两项精度门限仍为角度 MAE ≤1°、P95 ≤2°、最大 ≤3°,Tag 位置 P95 ≤3 mm。
三文件哈希、保护输入、观测覆盖和通道绑定一并检查;拟合失败不自动运动,取消后不能提交。
公共读取器和 `calibrated_joint_state_bridge` 支持 v3 的完整关节映射及历史 v1/v2 原语义。
完整 FK 使用 `independent_mimic_angles=True`。标准 mimic 加载器用于结构兼容检查,不能覆盖完整非线性关节状态。
本轮未修改 MuJoCo、相机时间同步和接触动力学;现有 MuJoCo 脚本尚不能视为支持 v3。
## 当前型号的发布边界
| 型号 | 独立运动观测 | 仍缺少的运动观测 | 绝对几何边界 |
| --- | --- | --- | --- |
| G20 | 21 / 21 个运动关节 | 无 | 五个末端被动关节仍缺 CAD baseline 依据 |
| L6 | 5 / 11 | 食指、中指、无名指的 MCP、DIP | 旧 CAD 保留及迁移关系不能充当独立依据 |
| O6 | 5 / 11 | 食指、中指、无名指的 MCP、DIP | 同上 |
| O12 | 16 / 19 | 无名指 MCP pitch、PIP、DIP | 部分末端/原 CAD 保留关节仍缺依据 |
`--validate-only` 的 `independent_measurement_coverage` 返回具体关节列表。
配置检查通过表示合同可执行,不能解释为全关节精度通过。
这四份现有配置目前均不能发布完整 v3;补齐 Tag、任务、避让与几何依据需要实机确认。
现有 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` |
合成数据的已知 baseline 几何明确标注 `synthetic_fixture_truth`,不写入产品 Profile。
完整 v3 非线性回放覆盖合成 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
```
+53 -26
View File
@@ -3,10 +3,10 @@
G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF
验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter,不再调用型号节点。
本包校准关节零位、有效行程及标准线性 mimic;保留 CAD 连杆尺寸、轴位置、mesh、惯量。
JSON 描述 SDK 输入与修正 URDF 角度的转换,不能补偿错误的 URDF。
本包按关节冻结实机 baseline 零位,分别输出独立实测非线性 JSON v3 和线性 mimic 近似 URDF;
保留 CAD 连杆尺寸、轴位置、mesh、惯量。完整运动由 JSON 的全部关节角度配合修正 URDF 计算。
软件仿真通过不等于实机精度通过;当前原始 G20/O12 存在需要确认的 mimic/限位冲突。
详见 [执行与验收记录](CALIBRATION_REFACTOR_PROGRESS.md)。
本轮实现与验证见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
## 启动
@@ -32,15 +32,19 @@ ros2 run linkerhand_calibration calibrate_hand --config \
| Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 |
| --- | ---: | ---: | ---: | --- |
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v2,256 项指令查表 |
| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v2,256 项指令查表 |
| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v2,256 项指令查表 |
| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v2,rad 输入节点查表 |
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v3,256 项指令查表 |
| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v3,256 项指令查表 |
| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v3,256 项指令查表 |
| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v3,rad 输入节点查表 |
任务数、主动关节数和实测关节数不必相等。一条运动可观测多个主动/被动关节。
L6/O6 未贴 Tag 的同机构关节使用显式迁移,不能把迁移称为独立实测。
**这些配置目前不能发布全关节独立实测产物。** L6/O6 的食指、中指、无名指缺少独立观测;
O12 无名指缺少独立观测。G20 虽有完整运动观测,末端被动关节仍缺少绝对 CAD 零位依据。
其他型号保留 CAD 零位的关节也须补充几何依据;启动检查的 `independent_measurement_coverage` 列出确切关节。
历史迁移关系保留供旧格式读取与诊断,v3 不用迁移、mimic 或补零填满 JSON。
本次沿用现有 Tag 和避让布局声明延后零位动作,没有把新增动作或缺失 Tag 布局标为实机验证通过。
每任务四个往返周期:前三轮训练,第四轮独立验证。
每个方向先连续扫描,再单通道访问稳态点。训练每方向默认 9 点,第四轮为交错点加端点;
每个方向先连续扫描,再单通道访问稳态点。训练每方向默认 9 点,并包含该通道 baseline,第四轮为交错点加端点;
每点至少 3 个稳定同步图像,轨迹结束后最多等待 2 秒,不能用精确指令/反馈相等判断稳定。
稳态点缺失与连续扫描不足统一在方向结束处理,最多同速重扫一次。
字节拟合不再改写原始 command/feedback,也不将接近端点的读数伪装成 0/255。
@@ -51,12 +55,24 @@ L6/O6 未贴 Tag 的同机构关节使用显式迁移,不能把迁移称为独
- 开始后清空预览缓存,先安全回基准,再为每机位固定 Tag 收集至少 10 个有效帧。
缺少基准时保持并显示缺少的 ID,不因为等待时间长暂停。
- 锁定后禁止人工移动手掌、相机、支架、Tag 粘贴位置;程序驱动关节和避让仍是允许的。
- 全手 baseline 时只要求固定参考可见,不要求四指运动 Tag 同时可见。
每关节在 `motion.joint_zero_references` 指定任务前,按声明的到达姿态和避让分组回到自身 baseline,
反馈稳定后收集至少 10 个稳定视觉帧,冻结 `JointZeroReference`。此后方向、轮次、重扫均不重新归零。
例如 G20 食指开始前,已完成四轮的其他手指可以保持非零避让;这些完整指令和反馈写入参考。
task.start 不代表零位,例如侧摆 baseline=127、扫描起点=255,会分别执行两种姿态。
- 相机相对位置改变时须重新确认/标定外参;仅重新锁定掌心 Tag 不能替代外参标定。
- 固定 Tag 被避让遮住时可用已锁定参考继续采集;软件不能保证检测完全不可见对象的移动。
运动 Tag 的刚性和几何一致性仍须通过最终独立验证。
- 断点版本为 `unified_engine_v4_dual_mapping`,不复用缺少稳态证据的旧 v1/v2/v3。
新断点检查配置哈希、基准及所复用 Tag 安装关系(5 px、2°、5 mm)。缺失或不一致即放弃复用,
自动完整重采,旧会话不修改。恢复时重新执行准备与避让,不直接跳到旧姿态。
- 断点版本为 `unified_engine_v5_joint_zero`,不复用旧策略数据。
首先检查配置哈希与整场固定参考;各关节轮到时,在同一完整保持姿态核对原零位与父子 Tag 位姿
(5 px、2°、5 mm),通过后才导入对应已完成扫描单元,并保留原零位记录。
整场参考不匹配则放弃旧数据;逐关节安装验证失败则暂停,禁止在旧样本上新建零位拼接。
排除原因后用 `--no-resume` 开始新会话。旧会话文件不修改。
共同坐标约定:`q_CAD = q_output + delta`;每个关节在自身冻结 baseline 时 `q_output=0`。
局部零位通过采样时的完整保持姿态和运动链进入同一掌部参考。固定 Tag 不提供未知末端连杆的 CAD 朝向。
`zero.known_baseline_geometry` 可声明有独立依据的 CAD 角度、误差界与来源;不能填入猜测的 0、旧 mimic
或 Tag 安装偏角冒充几何证据。相应 `origin.rpy` 和限位修正仍须在 Profile 中明确授权。
O12 避让:小指和无名指到 Profile 的最大弯曲指令;标定食指前,中指 MCP/PIP 到最大弯曲指令,
中指侧摆为 0 rad。到位采用完成轨迹、正确方向、至少 80% 请求位移和短时稳定,
@@ -72,6 +88,7 @@ POSITION 和活动故障由 SDK 确认;无法单独回读温度时使用 SDK
实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、
物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。
开始后相机投影参数变化也会拒绝继续使用混合坐标数据。
当前关节持续缺失零位观测会暂停并列出关节、机位和所需 Tag;尚未轮到的手指遮挡不阻塞当前任务。
短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动
不会实时中断轨迹。无效视觉帧直接丢弃。
@@ -119,7 +136,8 @@ linkerhand_calibration/
├── runtime/capture.py 多机位视觉/反馈采集
├── runtime/scan_quality.py 唯一方向数据门
├── runtime/reference_lock.py 开始后固定基准
├── runtime/resume.py 唯一断点证据检查
├── runtime/resume.py 整场断点证据检查
├── runtime/joint_resume.py 逐关节原零位验证后导入旧样本
├── runtime/safety.py 最小实时保护
├── runtime/status.py 统一状态与中文显示
├── runtime/snapshot.py 类型化状态快照与旧状态映射
@@ -167,8 +185,8 @@ Profile 声明:
1. Adapter、SDK 通道顺序、单位、正负方向、命令/反馈范围、基准值、独立速度槽布局。
2. Tag ID/尺寸/机位/安装 link,以及每次运动的父子观测关系;不明确的 link 不能靠名称猜。
3. 扫描任务、速度、准备/避让/收尾 waypoint;必要时指定不可见 Tag 的恢复验证姿态。
4. 哪些零位可观测、哪些固定保留 CAD、哪些参数同机构迁移;声明空间观测约束和拟合原语。
3. 扫描任务、速度、准备/避让/收尾 waypoint;每关节首次零位任务、完整保持指令及固定到达姿态。
4. 声明空间可观测约束和有依据的已知 baseline 几何;完整 v3 每个运动关节均须独立观测。
5. 每关节允许修正的 `origin.rpy`、`limit.lower/upper`、`mimic.multiplier/offset`。
这仍需要正确的可观测性设计:单轴加一对未知安装角的 Tag 不能自动辨识绝对 CAD 零位。
@@ -177,10 +195,10 @@ Profile 声明:
复用 SDK 不增加 Python;新协议只增加 `runtime/adapters/` 实现及协议工厂注册。
没有型号节点、runner、状态机、质量/断点/发布器;新数学只能增加通用拟合原语。
新型号使用 `artifacts.output_schema_version: 2` 选择统一指令查表格式;产物自身为
`format: unified_calibration_v2, schema_version: 2`。详细双映射保留在独立标定报告中。
新型号使用 `artifacts.output_schema_version: 3`;产物为
`format: unified_calibration_v3, schema_version: 3`。详细双映射保留在独立标定报告中。
旧 generic v1、v4/v6/v7 只保留历史兼容工具,不把反馈表改名为指令表来伪造兼容。
正式 Profile 校验及 finalizer 只接受 `output_schema_version: 2`,不能用旧格式跳过指令映射拟合。
v1/v2 按原语义读取,不会自动升级为独立实测;保留的 v2 数学入口用于历史回归,不能证明 v3 精度。
配置错字、错误绑定、未知修正字段在启动前失败;修改受保护文件后应审查并更新对应 SHA256。
## 产物与离线验证
@@ -192,13 +210,20 @@ manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和
主 JSON 以原始 URDF 关节名为键,每个关节仅包含 `sdk_channel` 和 `angle_rad`;
O12 等 rad 输入还包含一一对应的 `input_values`(在有效节点间线性插值,不外推)。
字节型号固定 256 项:SDK 指令 `v` 对应 `angle_rad[v]`,只能在训练指令实际覆盖 0–255 时导出。
输出均是修正 URDF 的 q_output,不再加零偏。顶层保留 `format/schema_version/profile_id/model/side/serial_number/input_unit`。
被动关节的查表由导出 URDF 的标准 mimic 递归生成,绑定同一驱动通道,必须与 URDF 完全一致。
单张查表取训练正反向指令曲线的平均值,再用实际导出表重放第四轮视觉数据;回差等因素导致超差时禁止发布,
不通过隐藏方向分支、外推、放宽精度或 JSON 非线性被动联动制造通过结果。
输出均是修正 URDF 的 q_output,不再加零偏。顶层记录身份、单位、完整 `baseline_command` 和共同零位约定。
主动和被动关节均直接导出各自视觉实测曲线,同一驱动通道可绑定多个关节。
最终单条曲线在前三轮拟合中约束 baseline=0,正反向诊断分支共用冻结参考、保留回差。
第四轮分别检验两个方向;超差时禁止发布,不隐藏切换分支。
URDF mimic 单独用前三轮成对视觉角度拟合 `q_child≈a*q_parent`,共同零位下 offset=0。
其近似误差不会否决已达标的完整映射,但结构、授权字段与机械限位始终强制检查。
验收分别记录:最终 JSON 对第四轮独立视觉角度;完整 JSON+URDF FK 对第四轮 Tag 位姿;
同一主动映射配合标准 mimic 的近似损失。前两项决定精度通过,第三项仅报告。
门限保持角度 MAE≤1°、P95≤2°、最大≤3°,Tag 位置 P95≤3 mm。
两种 FK 使用相同的训练冻结安装与基座;第四轮不拟合零位、曲线、Tag 安装或基座。
`calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、
零位、来源、迁移和训练/holdout 指标。报告中的 `applicability` 记录保持姿态;
零位原始参考帧、完整指令/反馈/到达方向、来源和训练/holdout 指标。报告中的 `applicability` 记录保持姿态;
未采集多姿态证据不能宣称任意多轴工况已验证。主 JSON 用于指令查表,不能误作反馈转换。
旧 unified v1 会话继续只读兼容,不自动覆盖或转换历史产物;新输出单表必须重新通过最终文件验收。
@@ -212,9 +237,12 @@ ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \
需要反馈角度显示时显式选择 `input_kind:=feedback`,桥读取配套报告中的反馈映射。
桥检查三文件哈希、拒绝越域输入,
被动角由实际导出 URDF 计算,不加第二次零偏。仿真输出不能与实机反馈话题同名。
v3 全部关节角由 JSON 显式计算,完整 FK 必须启用 `independent_mimic_angles=True`,不加第二次零偏。
标准 `robot_state_publisher` 的 mimic 更新会覆盖被动角;仅检查其加载兼容性,不用它验证完整非线性运动。
仿真输出不能与实机反馈话题同名。
MuJoCo 是用户后续使用的外部验收项目,本轮不改动它、不增加 MuJoCo 依赖,自动标定不要求打开 GUI。
外部对比须确认加载的模型来自这份 URDF,指令转换使用配套 JSON;不能用实机反馈直接驱动仿真来证明指令映射。
现有 MuJoCo 脚本尚不能视为支持 v3:后续须消费完整关节映射并停止冲突的线性被动更新。
```bash
# 同一算法离线回放新策略的完整采集;不启动硬件,默认不发布
@@ -228,8 +256,7 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \
离线回放也检查原始会话策略、SDK 输入域、配置/相机证据、每方向完整数据及最终文件。
旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出,且不会绕过验证。
算法和坐标规则见 [CALIBRATION_FLOW.md](CALIBRATION_FLOW.md),批准的原始方案保持在
[CALIBRATION_REFACTOR_PLAN.md](CALIBRATION_REFACTOR_PLAN.md)。
当前坐标规则、生产职责与验收范围见 [v3 实施记录](CALIBRATION_V3_IMPLEMENTATION.md)。
当前代码审查、已修复问题与未完成的精度验收见
[2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)。相机图像目前采用主机取帧时间;
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: G20/right/g20_right_19/v1
profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml
profile_config_sha256: 35cad4432a01b3e1af157f1e914c2054e71522a44c6786db2519e4c2a0966a46
profile_config_sha256: 049a8d7b75d3f7881d6990115416718792e775f513154d59f5843191df03d068
model: G20
side: right
tag_layout: g20_right_19
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: L6/right/l6_right_8/v1
profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml
profile_config_sha256: b9654ce998b56c10d7094364e8b715663163d2101e1f70e077cb0723fc47f02e
profile_config_sha256: 71cfe506126ffc485de878ee620f2b453b4afaccde66bcf585231edb6daf6dd2
model: L6
side: right
tag_layout: l6_right_8
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: O12/right/o12_right_16/v1
profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml
profile_config_sha256: 0b5cd27168bc863e5c1aed5382c08e33ab4c02a23359a325dc9925fd30599e5f
profile_config_sha256: a1faa89632a9e3ce38f5083abf080c194f19bd906f6ec40ae377f3399cc3c00a
model: O12
side: right
tag_layout: o12_right_16
@@ -1,7 +1,7 @@
schema_version: 3
profile_id: O6/right/o6_right_8/v1
profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml
profile_config_sha256: ab1d86cb0c3e514de5322a44c1444e99d9b1f1ce43f8a2451cd032ed9c31e4ff
profile_config_sha256: 4872b6eee080eb8bb3596a53f4b731d0dc317881f64589ea2e55676ad2a0a2cf
model: O6
side: right
tag_layout: o6_right_8
@@ -927,6 +927,112 @@ motion:
- 5
- 10
- 15
joint_zero_references:
thumb_cmc_pitch:
task_key: thumb_cmc_pitch_front
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [0, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
thumb_cmc_roll:
task_key: thumb_cmc_roll_front
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 0, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
thumb_mcp:
task_key: thumb_mcp_ip_front
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 0, 255, 255, 255, 255]
thumb_ip:
task_key: thumb_mcp_ip_front
command: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 0, 255, 255, 255, 255]
thumb_cmc_yaw:
task_key: thumb_cmc_yaw_top
command: [255, 255, 255, 255, 255, 145, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 145, 127, 127, 127, 127, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255]
pinky_mcp_roll:
task_key: pinky_roll_multiview
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
pinky_mcp_pitch:
task_key: pinky_pitch_side
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 0, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
pinky_pip:
task_key: pinky_pip_side
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
pinky_dip:
task_key: pinky_pip_side
command: [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
ring_mcp_roll:
task_key: ring_roll_multiview
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
approach_commands:
- [255, 255, 255, 255, 0, 255, 255, 255, 0, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
ring_mcp_pitch:
task_key: ring_pitch_side
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
approach_commands:
- [255, 255, 255, 0, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
ring_pip:
task_key: ring_pip_side
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
approach_commands:
- [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
ring_dip:
task_key: ring_pip_side
command: [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 0]
approach_commands:
- [255, 255, 255, 255, 0, 255, 255, 255, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
middle_mcp_roll:
task_key: middle_roll_multiview
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
approach_commands:
- [255, 255, 255, 0, 0, 255, 255, 0, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
middle_mcp_pitch:
task_key: middle_pitch_side
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
approach_commands:
- [255, 255, 0, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
middle_pip:
task_key: middle_pip_side
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
approach_commands:
- [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
middle_dip:
task_key: middle_pip_side
command: [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0]
approach_commands:
- [255, 255, 255, 0, 0, 255, 255, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
index_mcp_roll:
task_key: index_roll_multiview
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
approach_commands:
- [255, 255, 0, 0, 0, 255, 0, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
index_mcp_pitch:
task_key: index_pitch_side
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
approach_commands:
- [255, 0, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
index_pip:
task_key: index_pip_side
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
approach_commands:
- [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0]
index_dip:
task_key: index_pip_side
command: [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0]
approach_commands:
- [255, 255, 0, 0, 0, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0]
measurement:
measurements:
thumb_cmc_pitch:
@@ -1364,7 +1470,7 @@ scope:
- thumb_mcp
default_scope: full
artifacts:
output_schema_version: 2
output_schema_version: 3
calibration_filename: g20_{side}_{serial_number}_calibration.json
corrected_urdf_filename: linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf
protected_input_fields:
@@ -1385,7 +1491,7 @@ artifacts:
- urdf_zero_publication
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
policy_version: unified_engine_v5_joint_zero
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
@@ -155,6 +155,32 @@ motion:
stall_timeout_seconds: 2.0
precheck_sweeps: false
steady_command_checkpoints: false
joint_zero_references:
rh_thumb_cmc_roll:
task_key: thumb_roll_top
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 0, 255, 255, 255, 255]
rh_thumb_cmc_pitch:
task_key: thumb_pitch_dip_front
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [0, 255, 255, 255, 255, 255]
rh_thumb_dip:
task_key: thumb_pitch_dip_front
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [0, 255, 255, 255, 255, 255]
rh_pinky_mcp_pitch:
task_key: pinky_pitch_dip_side
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 0]
rh_pinky_dip:
task_key: pinky_pitch_dip_side
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 0]
measurement:
measurements:
rh_thumb_cmc_roll:
@@ -288,7 +314,7 @@ scope:
- rh_ring_mcp_pitch
default_scope: partial
artifacts:
output_schema_version: 2
output_schema_version: 3
calibration_filename: l6_right_{serial_number}_partial_calibration.json
corrected_urdf_filename: linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf
protected_input_fields:
@@ -303,7 +329,7 @@ artifacts:
- l6_partial_v1
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
policy_version: unified_engine_v5_joint_zero
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
@@ -563,6 +563,87 @@ motion:
- 3
- 5
- 6
joint_zero_references:
thumb_cmc_pitch:
task_key: thumb_pitch_front
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, 0.0, -0.8272860654453121, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
thumb_cmc_roll:
task_key: thumb_roll_front
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.9424777960769379, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
thumb_mcp:
task_key: thumb_mcp_dip_front
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, 0.0, 0.0, -1.2915436464758039, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
thumb_dip:
task_key: thumb_mcp_dip_front
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, 0.0, 0.0, -1.2915436464758039, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
thumb_cmc_yaw:
task_key: thumb_yaw_top
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, -1.387536755335492, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pinky_mcp_pitch:
task_key: pinky_chain_side
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501]
pinky_pip:
task_key: pinky_chain_side
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501]
pinky_dip:
task_key: pinky_chain_side
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.53588974175501]
middle_mcp_roll:
task_key: middle_roll_front
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.17, 0.0, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, -0.2617993877991494, 0.17, 0.0, 1.53588974175501, 1.53588974175501]
middle_mcp_pitch:
task_key: middle_mcp_side
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 1.3578661580515883, 0.0, 1.53588974175501, 1.53588974175501]
middle_pip:
task_key: middle_pip_dip_side
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
middle_dip:
task_key: middle_pip_dip_side
command: [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 0.0, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, -0.17453292519943295, 0.11, 0.0, 0.0, 0.0, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
index_mcp_roll:
task_key: index_roll_front
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.17, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, -0.2617993877991494, 0.17, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
index_mcp_pitch:
task_key: index_mcp_side
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, 0.0, 1.3526301702956054, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
index_pip:
task_key: index_pip_dip_side
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.530653753999027, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
index_dip:
task_key: index_pip_dip_side
command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
approach_commands:
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.530653753999027, 0.0, 1.3578661580515883, 1.8151424220741028, 1.53588974175501, 1.53588974175501]
measurement:
candidate_selection_tasks:
- thumb_mcp_dip_front
@@ -815,7 +896,7 @@ scope:
frozen_joints:
full: []
artifacts:
output_schema_version: 2
output_schema_version: 3
calibration_filename: o12_right_{serial_number}_calibration.json
corrected_urdf_filename: linkerhand_o12_right_{serial_number}_zero_calibrated.urdf
protected_input_fields:
@@ -833,7 +914,7 @@ artifacts:
- full_sdk_range_v2
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
policy_version: unified_engine_v5_joint_zero
mapping_probe_maximum_rad: 0.05235987755982989
urdf:
authorized_fields:
@@ -151,6 +151,32 @@ motion:
stall_timeout_seconds: 2.0
precheck_sweeps: false
steady_command_checkpoints: false
joint_zero_references:
rh_thumb_cmc_yaw:
task_key: thumb_yaw_top
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 0, 255, 255, 255, 255]
rh_thumb_cmc_pitch:
task_key: thumb_pitch_ip_front
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [0, 255, 255, 255, 255, 255]
rh_thumb_ip:
task_key: thumb_pitch_ip_front
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [0, 255, 255, 255, 255, 255]
rh_pinky_mcp_pitch:
task_key: pinky_pitch_dip_side
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 0]
rh_pinky_dip:
task_key: pinky_pitch_dip_side
command: [255, 255, 255, 255, 255, 255]
approach_commands:
- [255, 255, 255, 255, 255, 0]
measurement:
measurements:
rh_thumb_cmc_yaw:
@@ -284,7 +310,7 @@ scope:
- rh_ring_mcp_pitch
default_scope: partial
artifacts:
output_schema_version: 2
output_schema_version: 3
calibration_filename: o6_right_{serial_number}_partial_calibration.json
corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf
protected_input_fields:
@@ -299,7 +325,7 @@ artifacts:
- o6_partial_v1
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
policy_version: unified_engine_v5_joint_zero
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
@@ -276,7 +276,7 @@ def load_calibrated_command_mapper(
if not path.is_file():
raise ValueError(f"calibration JSON does not exist: {path}")
payload = json.loads(path.read_text(encoding="utf-8"))
if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2"}:
if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2", "unified_calibration_v3"}:
from .runtime.artifacts.reader import load_unified_mapper
return load_unified_mapper(path, expected_side=expected_side, input_kind=input_kind)
return CalibratedCommandMapper(payload, expected_side=expected_side)
@@ -44,7 +44,7 @@ def build_typed_profile():
command_index_by_joint=command_index,
urdf_joint_by_joint={name: name for name in active},
),
motion=replace(right.motion, tasks=tasks),
motion=replace(right.motion, tasks=tasks, joint_zero_references={}),
measurement=replace(
right.measurement,
measurements=measurements,
@@ -65,6 +65,7 @@ def build_typed_profile():
for k, v in right.zero.transferred_mimic_sources.items()},
# Runtime-only mirrored products do not acquire/solve new geometry.
spatial={},
known_baseline_geometry={},
active_joints=active,
passive_joints=passive,
direct_zero_joints=tuple(
@@ -111,6 +112,7 @@ def build_typed_profile():
),
artifacts=replace(
right.artifacts,
output_schema_version=2,
calibration_filename=(
"l6_left_{serial_number}_transferred_calibration.json"
),
@@ -44,7 +44,7 @@ def build_typed_profile():
command_index_by_joint=command_index,
urdf_joint_by_joint={name: name for name in active},
),
motion=replace(right.motion, tasks=tasks),
motion=replace(right.motion, tasks=tasks, joint_zero_references={}),
measurement=replace(
right.measurement,
measurements=measurements,
@@ -65,6 +65,7 @@ def build_typed_profile():
for k, v in right.zero.transferred_mimic_sources.items()},
# Runtime-only mirrored products do not acquire/solve new geometry.
spatial={},
known_baseline_geometry={},
active_joints=active,
passive_joints=passive,
direct_zero_joints=tuple(
@@ -111,6 +112,7 @@ def build_typed_profile():
),
artifacts=replace(
right.artifacts,
output_schema_version=2,
calibration_filename=(
"o6_left_{serial_number}_transferred_calibration.json"
),
@@ -17,9 +17,12 @@ from .profile import (
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
JointZeroSpec,
KnownBaselineGeometry,
validate_profile,
)
from .result import CalibrationResult, JointMapping
from .reference import JointZeroReference, JointZeroSample
from .status import (
AcquisitionStatus,
CalibrationStatus,
@@ -35,6 +38,10 @@ __all__ = [
"CalibrationProfile",
"CalibrationResult",
"JointMapping",
"JointZeroReference",
"JointZeroSample",
"JointZeroSpec",
"KnownBaselineGeometry",
"CalibrationStatus",
"CommandLayout",
"MeasurementPolicy",
@@ -8,7 +8,7 @@ from pathlib import PurePath
from typing import Mapping
ACQUISITION_POLICY_VERSION = "unified_engine_v4_dual_mapping"
ACQUISITION_POLICY_VERSION = "unified_engine_v5_joint_zero"
@dataclass(frozen=True, order=True)
@@ -215,6 +215,24 @@ class ReferenceWaypoint:
tag_ids_by_view: Mapping[str, tuple[int, ...]]
@dataclass(frozen=True)
class JointZeroSpec:
"""A reviewed full-hand pose at which one joint's baseline is visible."""
task_key: str
command: tuple[float, ...]
approach_commands: tuple[tuple[float, ...], ...] = ()
@dataclass(frozen=True)
class KnownBaselineGeometry:
"""Independent CAD angle at the measured baseline, never an SDK prior."""
cad_angle_rad: float
uncertainty_rad: float
evidence_source: str
@dataclass(frozen=True)
class MotionPolicy:
"""Reviewed motion tasks and optional safe waypoint sequences."""
@@ -227,6 +245,7 @@ class MotionPolicy:
steady_command_checkpoints: bool = False
return_groups: tuple[tuple[int, ...], ...] = ()
resume_verification_waypoints: tuple[ReferenceWaypoint, ...] = ()
joint_zero_references: Mapping[str, JointZeroSpec] = field(default_factory=dict)
@dataclass(frozen=True)
@@ -280,6 +299,7 @@ class ZeroSolvePolicy:
transferred_mimic_sources: Mapping[str, str] = field(default_factory=dict)
# Geometry policy only; no Python callback/import strings are permitted.
spatial: Mapping[str, object] = field(default_factory=dict)
known_baseline_geometry: Mapping[str, KnownBaselineGeometry] = field(default_factory=dict)
@dataclass(frozen=True)
@@ -548,6 +568,32 @@ def validate_profile(profile: CalibrationProfile) -> None:
errors.append("return groups must contain unique valid channels")
zero = profile.zero
for name, reference in profile.motion.joint_zero_references.items():
task = next((t for t in profile.motion.tasks if t.key == reference.task_key), None)
if task is None or name not in task.joints:
errors.append(f"zero reference must precede an observing task:{name}")
continue
for pose in (reference.command, *reference.approach_commands):
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"zero reference command outside declared domain:{name}")
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]:
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]):
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:
errors.append(f"zero reference must precede first training task:{name}")
for name, geometry in zero.known_baseline_geometry.items():
if name not in zero.active_joints | zero.passive_joints or not geometry.evidence_source.strip():
errors.append(f"known baseline requires a joint and evidence source:{name}")
if not math.isfinite(geometry.cad_angle_rad) or not 0 <= geometry.uncertainty_rad <= math.radians(1):
errors.append(f"known baseline geometry is invalid or uncertain:{name}")
if zero.active_joints & zero.passive_joints:
errors.append("active and passive joints must be disjoint")
all_joints = zero.active_joints | zero.passive_joints
@@ -0,0 +1,173 @@
"""Immutable visual baselines, acquired once before each joint's training."""
from __future__ import annotations
from dataclasses import asdict, dataclass
import math
from typing import Mapping, Sequence
import numpy as np
from scipy.spatial.transform import Rotation
@dataclass(frozen=True)
class JointZeroPose:
quaternion_xyzw: tuple[float, ...]
translation_xyz_m: tuple[float, ...]
@classmethod
def from_mapping(cls, row):
q, t = tuple(row["quaternion_xyzw"]), tuple(row["translation_xyz_m"])
if (len(q) != 4 or len(t) != 3 or not np.all(np.isfinite(q+t))
or not np.isclose(np.linalg.norm(q), 1, atol=1e-5)):
raise ValueError("joint_zero_pose_invalid")
return cls(q, t)
@dataclass(frozen=True)
class JointZeroSample:
sample_id: str
view: str
image_stamp_ns: int
relative_quaternion_xyzw: tuple[float, ...]
relative_translation_xyz_m: tuple[float, ...]
command: tuple[float, ...]
feedback: tuple[float, ...]
arrival_directions: tuple[str, ...]
parent_pose_common: JointZeroPose
child_pose_common: JointZeroPose
@classmethod
def from_row(cls, row, unit):
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", ())),
JointZeroPose.from_mapping(row["parent_pose_common"]), JointZeroPose.from_mapping(row["child_pose_common"]))
@dataclass(frozen=True)
class JointZeroReference:
joint: str
task_key: str
baseline_command: float
channel: int
session_epoch: int
motion_version: int
samples: tuple[JointZeroSample, ...]
def quaternion(self, view):
rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in self.samples if s.view == view])
return rotations.mean().as_quat()
def input_reference(self, view, *, feedback=False):
values = [(s.feedback if feedback else s.command)[self.channel]
for s in self.samples if s.view == view]
if not values:
raise ValueError(f"joint_zero_missing_view:{self.joint}:{view}")
return float(np.median(values))
def as_record(self):
return {"kind": "joint_zero_reference", **asdict(self)}
@classmethod
def from_record(cls, row):
for sample in row["samples"]:
if sample["sample_id"] != f"{sample['view']}:{int(sample['image_stamp_ns'])}":
raise ValueError(f"joint_zero_image_identity_invalid:{row['joint']}")
samples = tuple(JointZeroSample(**{**s,
"parent_pose_common": JointZeroPose.from_mapping(s["parent_pose_common"]),
"child_pose_common": JointZeroPose.from_mapping(s["child_pose_common"]),
**{key: tuple(s[key]) for key in ("relative_quaternion_xyzw", "relative_translation_xyz_m",
"command", "feedback", "arrival_directions")}}) for s in row["samples"])
return cls(str(row["joint"]), str(row["task_key"]), float(row["baseline_command"]),
int(row["channel"]), int(row["session_epoch"]), int(row["motion_version"]), samples)
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)
views = {profile.measurement.measurements[joint].view}
secondary = profile.measurement.cross_view_sources.get(joint)
if secondary:
views.add(profile.measurement.measurements[secondary].view)
samples = tuple(JointZeroSample.from_row(row, profile.command.unit) for row in rows)
if session_epoch < 1 or motion_version < 1:
raise ValueError(f"joint_zero_session_version_invalid:{joint}")
for sample in samples:
if sample.image_stamp_ns < 1 or sample.sample_id != f"{sample.view}:{sample.image_stamp_ns}":
raise ValueError(f"joint_zero_image_identity_invalid:{joint}")
if len(sample.relative_quaternion_xyzw) != 4 or not np.isclose(np.linalg.norm(sample.relative_quaternion_xyzw), 1, atol=1e-5):
raise ValueError(f"joint_zero_rotation_invalid:{joint}")
parent, child = sample.parent_pose_common, sample.child_pose_common
rp, rc = Rotation.from_quat(parent.quaternion_xyzw), Rotation.from_quat(child.quaternion_xyzw)
if ((Rotation.from_quat(sample.relative_quaternion_xyzw).inv()*rp.inv()*rc).magnitude() > 1e-7
or not np.allclose(rp.inv().apply(np.asarray(child.translation_xyz_m)-parent.translation_xyz_m),
sample.relative_translation_xyz_m, atol=1e-8, rtol=0)):
raise ValueError(f"joint_zero_relative_pose_inconsistent:{joint}")
minimum = profile.acquisition.fixed_reference_minimum_frames
for view in views:
selected = [s for s in samples if s.view == view]
if len({s.sample_id for s in selected}) < minimum:
measurements = [profile.measurement.measurements[n] for n in (joint, secondary) if n is not None]
roles = {role for m in measurements if m.view == view for role in (m.parent_role, m.child_role)}
ids = [t.tag_id for v in profile.vision.views if v.name == view for t in v.tags if t.role in roles]
raise ValueError(f"joint_zero_samples_missing:{joint}:{view}:required_tags={ids}:{len(selected)}/{minimum}")
rotations = Rotation.from_quat([s.relative_quaternion_xyzw for s in selected])
if np.max((rotations.mean().inv()*rotations).magnitude()) > math.radians(1):
raise ValueError(f"joint_zero_not_stable:{joint}:{view}")
identities = [(s.view, s.sample_id) for s in samples]
if len(set(identities)) != len(identities):
raise ValueError(f"duplicate_joint_zero_image:{joint}")
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)):
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:
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)
def read_joint_zero_references(profile, records):
"""Reject invented, repeated or late baselines in online and offline inputs."""
result = {}
first_training_stamp = {}
for row in records:
if row.get("kind") in {"joint_sample", "secondary_joint_sample"}:
name = row.get("joint", row.get("observation_joint"))
stamp = int(row["image_stamp_ns"])
first_training_stamp[name] = min(first_training_stamp.get(name, stamp), stamp)
if row.get("kind") != "joint_zero_reference":
continue
reference = JointZeroReference.from_record(row)
name = reference.joint
if name in result or name not in profile.motion.joint_zero_references:
raise ValueError(f"duplicate_or_unknown_joint_zero:{name}")
spec = profile.motion.joint_zero_references[name]
if (not 0 <= reference.channel < profile.command.command_count or reference.task_key != spec.task_key
or reference.baseline_command != profile.command.baseline_values[reference.channel]):
raise ValueError(f"joint_zero_contract_changed:{name}")
# Revalidate the complete stored evidence, not just an averaged quaternion.
rows = [{"view": s.view, "image_stamp_ns": s.image_stamp_ns,
"relative_quaternion_xyzw": s.relative_quaternion_xyzw,
"relative_translation_xyz_m": s.relative_translation_xyz_m,
f"command_vector_{profile.command.unit}": s.command, f"state_{profile.command.unit}": s.feedback,
"command_direction_by_index": s.arrival_directions,
"parent_pose_common": asdict(s.parent_pose_common), "child_pose_common": asdict(s.child_pose_common)}
for s in reference.samples]
checked = build_joint_zero_reference(profile, name, rows,
session_epoch=reference.session_epoch, motion_version=reference.motion_version)
if checked.channel != reference.channel:
raise ValueError(f"joint_zero_channel_changed:{name}")
result[name] = checked
for name, reference in result.items():
if name in first_training_stamp and max(s.image_stamp_ns for s in reference.samples) >= first_training_stamp[name]:
raise ValueError(f"joint_zero_not_before_training:{name}")
return result
@@ -34,6 +34,8 @@ class CalibrationResult:
command_mappings: Mapping[str, JointMapping] = field(default_factory=dict)
command_holdout_metrics: Mapping = field(default_factory=dict)
command_applicability: Mapping = field(default_factory=dict)
joint_zero_references: Mapping = field(default_factory=dict)
mimic_approximations: Mapping = field(default_factory=dict)
@dataclass(frozen=True)
@@ -0,0 +1,17 @@
"""The declared command nodes used by acquisition and fitting alike."""
import numpy as np
def command_nodes(profile, task, *, holdout=False):
lo, hi = sorted((task.start_value, task.end_value))
train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
if profile.artifacts.output_schema_version >= 3:
baseline = profile.command.baseline_values[task.command_index]
if not lo <= baseline <= hi:
raise ValueError(f"baseline_outside_calibrated_support:{task.key}")
train = np.unique(np.r_[train, baseline])
values = np.r_[lo, (train[:-1]+train[1:])/2, hi] if holdout else train
if profile.command.unit == "u8":
values = np.floor(values+.5)
return tuple(sorted(set(float(v) for v in values)))
@@ -0,0 +1,22 @@
"""Training-only monotonic projection with an exact, already observed zero."""
import math
import numpy as np
from .curve import isotonic_nonincreasing
def anchored_mean(knots, increasing, decreasing, baseline, sign):
x = np.asarray(knots, dtype=float)
values = (np.asarray(increasing) + decreasing) / 2
indices = np.flatnonzero(np.isclose(x, baseline, atol=1e-9, rtol=0))
if len(indices) != 1:
raise ValueError("baseline_requires_an_observed_training_knot")
i = int(indices[0])
oriented = values * sign
left = np.minimum(-isotonic_nonincreasing(-oriented[:i]), 0) if i else []
right = np.maximum(-isotonic_nonincreasing(-oriented[i+1:]), 0) if i+1 < len(x) else []
fitted = np.r_[left, 0.0, right] * sign
if np.max(np.abs(fitted-values)) > math.radians(1):
raise ValueError("baseline_constraint_inconsistent_with_training")
return tuple(float(v) for v in fitted)
@@ -12,12 +12,13 @@ from ..domain.result import JointMapping
from ..urdf.acceptance import angular_metrics
from .curve import isotonic_nonincreasing
from .spatial import measure_joint_curve_observation
from ..domain.sampling import command_nodes
def fit_command_mappings(profile, fit, records_by_joint):
mappings, metrics, applicability = {}, {}, {}
for joint in sorted(fit.output_mappings):
donor = profile.zero.transferred_zero_sources.get(joint, joint)
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}")
@@ -25,7 +26,7 @@ def fit_command_mappings(profile, fit, records_by_joint):
gauge = float(np.interp(fit.reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad))
# Byte curves already carry their shared coordinate reference. Native
# curves retain the arbitrary observed Tag reference in fit.curves.
if profile.command.unit == "u8":
if profile.command.unit == "u8" or profile.artifacts.output_schema_version >= 3:
gauge = 0.0
observed = {r["sample_id"]: measure_joint_curve_observation(curve,
quaternion_xyzw=r.get("relative_quaternion_xyzw"),
@@ -37,11 +38,10 @@ def fit_command_mappings(profile, fit, records_by_joint):
branches = {}
task = next(t for t in profile.motion.tasks if donor in t.joints)
lo, hi = sorted((task.start_value, task.end_value))
train_knots = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
if profile.command.unit == "u8":
train_knots = np.floor(train_knots+0.5)
knots = tuple(sorted(set(float(v) for v in train_knots)))
knots = command_nodes(profile, task)
sign = profile.command.joint_directions[source_channel]
if profile.artifacts.output_schema_version >= 3:
sign = 1 if curve.angle_rad[-1] > curve.angle_rad[0] else -1
for direction in ("increasing", "decreasing"):
values = []
for knot in knots:
@@ -62,20 +62,21 @@ def fit_command_mappings(profile, fit, records_by_joint):
raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}")
branches[direction] = tuple(float(v) for v in projected)
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)
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"] == 3]
expected = np.r_[lo, (np.asarray(knots[:-1])+knots[1:])/2, hi]
if profile.command.unit == "u8":
# Acquisition interleaves the unrounded grid, then encodes bytes.
grid = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
expected = np.floor(np.r_[lo, (grid[:-1]+grid[1:])/2, hi]+0.5)
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"])
errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]),
"" if profile.artifacts.output_schema_version >= 3 else r["direction"])
- observed[r["sample_id"]] for r in validation]
quality = angular_metrics(errors)
if not quality.passed:
@@ -84,6 +85,12 @@ def fit_command_mappings(profile, fit, records_by_joint):
from dataclasses import asdict
metrics[joint] = {**asdict(quality), "independently_measured": donor == joint,
"source_joint": donor, "training_cycles": [0, 1, 2], "holdout_cycle": 3}
if profile.artifacts.output_schema_version >= 3:
directional = {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 directional.values()):
raise ValueError(f"single_command_curve_direction_holdout_failed:{joint}")
metrics[joint]["by_direction"] = {key: asdict(value) for key, value in directional.items()}
training = [r for r in rows if r["cycle"] in {0, 1, 2}]
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)
@@ -0,0 +1,44 @@
"""A training-only URDF approximation, separate from measured joint curves."""
from dataclasses import dataclass
import math
from .coupling import LinearMimicFit, fit_standard_mimic
from .observed_motion import paired_visual_observations
@dataclass(frozen=True)
class MimicApproximation:
output_fit: LinearMimicFit
training_sample_ids: tuple[str, ...]
holdout_sample_ids: tuple[str, ...]
holdout_errors_rad: tuple[float, ...]
def in_cad_coordinates(self, offsets):
from dataclasses import replace
fit = self.output_fit
# The offset is supported by the TWO baseline geometry datums.
return replace(fit, offset_rad=offsets[fit.target_joint]-fit.multiplier*offsets[fit.source_joint],
offset_observed=True)
def fit_mimic_approximations(model, curves, records):
result = {}
for name, joint in model.joints.items():
parent = joint.mimic_joint
if parent is None or name not in curves or parent not in curves:
continue
rows = paired_visual_observations(records[parent], records[name], curves[parent], curves[name])
for cycle in range(4):
for direction in ("increasing", "decreasing"):
if sum(r.cycle == cycle and r.direction == direction for r in rows) < 40:
raise ValueError(f"paired_mimic_observations_missing:{name}:{cycle}:{direction}")
training = [r for r in rows if r.cycle != 3]
holdout = [r for r in rows if r.cycle == 3]
fit = fit_standard_mimic(parent, name, [r.source_rad for r in training],
[r.target_rad for r in training], cycles=[r.cycle for r in training], offset_rad=0.0)
if not math.isfinite(fit.multiplier) or fit.multiplier == 0:
raise ValueError(f"invalid_mimic_approximation:{name}")
result[name] = MimicApproximation(fit, tuple(r.sample_id for r in training),
tuple(r.sample_id for r in holdout), tuple(fit.multiplier*r.source_rad-r.target_rad for r in holdout))
return result
@@ -71,7 +71,7 @@ def native_coordinate_curve(curve, baseline):
"coordinate_reference_angle_rad": reference_angle}), reference_input
def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_records=None):
def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_records=None, zero_references=None):
expected = {name for name, spec in profile.measurement.measurements.items()
if spec.view is not None and spec.kind != "axis_cross_view_validation"}
if set(records_by_joint) != expected:
@@ -79,7 +79,20 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
if any(profile.measurement.measurements[name].kind not in {"relative_rotation", "urdf_axis_chain", "curve"} for name in expected):
raise ValueError("measurement primitive is not implemented in this pipeline")
mimic_sources = {name: profile.zero.mimic_source_by_joint[name] for name in profile.zero.fitted_mimic_joints}
if profile.command.unit == "u8":
if profile.artifacts.output_schema_version >= 3:
if set(zero_references or {}) != expected:
raise ValueError("joint_zero_references_incomplete")
observed, holdout, evidence, references = {}, {}, {}, {}
for name, rows in sorted(records_by_joint.items()):
view = profile.measurement.measurements[name].view
reference = zero_references[name]
observed[name], holdout[name] = fit_native_observed_rotation(rows,
input_domain=profile.curve_input_domain,
input_to_joint_direction=joint_input_direction(profile, source_model, name),
reference_xyzw=reference.quaternion(view))
references[name] = reference.input_reference(view, feedback=True)
coordinates = observed
elif profile.command.unit == "u8":
observed, holdout, evidence = fit_byte_observed_motion(records_by_joint,
source_model=source_model, mimic_sources=mimic_sources,
baseline_by_joint={name: profile.command.baseline_values[channel_for_joint(profile, name)] for name in expected},
@@ -109,7 +122,9 @@ def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_re
metrics = {}
for name, rows in secondary.items():
curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain,
input_to_joint_direction=joint_input_direction(profile, source_model, name))
input_to_joint_direction=joint_input_direction(profile, source_model, name),
reference_xyzw=(zero_references[name].quaternion(rows[0]["view"])
if profile.artifacts.output_schema_version >= 3 else None))
if "input_knots" not in observed[name].circle:
raise ValueError("cross-view comparison requires native input knots")
comparison = compare_native_views(observed[name], curve)
@@ -44,7 +44,7 @@ def paired_visual_observations(source_rows, target_rows, source_fit, target_fit)
return output
def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction):
def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction, reference_xyzw=None):
"""Native-coordinate 3+1 hinge primitive with the shared geometric result.
The metadata carries real knots, not a synthetic byte command domain.
@@ -58,7 +58,8 @@ def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction
if sum(row.cycle == 3 and row.direction == direction for row in observations) < 40:
raise ValueError("holdout requires 40 images in each direction")
fitted = fit_rotation_curve([row for row in observations if row.cycle != 3],
input_domain=input_domain, sdk_to_joint_direction=input_to_joint_direction)
input_domain=input_domain, sdk_to_joint_direction=input_to_joint_direction,
reference_xyzw=reference_xyzw)
errors = fitted.holdout_errors([row for row in observations if row.cycle == 3])
metrics = angular_metrics(errors)
if not metrics.passed:
@@ -95,6 +95,7 @@ def _validate_observations(rows: Sequence[RotationObservation], *, cycles: set[i
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,
) -> RotationCurve:
"""Fit three complete training cycles without integerizing physical inputs.
@@ -119,7 +120,7 @@ def fit_rotation_curve(
raise ValueError("byte observations are outside the SDK domain")
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))]
reference = rotations[int(np.argmin(inputs))] if reference_xyzw is None else _rotation(reference_xyzw)
vectors = (reference.inv() * rotations).as_rotvec()
_, singular, vh = np.linalg.svd(vectors, full_matrices=False)
if not singular.size or singular[0] < math.radians(1):
@@ -2,7 +2,7 @@
from __future__ import annotations
from dataclasses import dataclass, field
from dataclasses import dataclass, field, replace
import math
from pathlib import Path
from typing import Mapping
@@ -46,25 +46,27 @@ def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfi
hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints),
stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias),
direct_zero_joints=direct, axis_joints=axes,
inherited_zero_joints=dict(profile.zero.transferred_zero_sources),
inherited_static_zero_joints=dict(profile.zero.transferred_zero_sources),
inherited_zero_joints={} if profile.artifacts.output_schema_version >= 3 else dict(profile.zero.transferred_zero_sources),
inherited_static_zero_joints={} if profile.artifacts.output_schema_version >= 3 else dict(profile.zero.transferred_zero_sources),
constrained_circle_joints=frozenset(axes),
root_anchor_joints=frozenset(data.get("root_anchor_joints", ())),
axis_parent_joint=dict(data.get("axis_parent_joint", {})),
phase_parent_joint=dict(data.get("phase_parent_joint", {})),
offset_observer_joint=observers,
same_view_axis_pair_by_offset={name: tuple(pair) for name, pair in data.get("same_view_axis_pair_by_offset", {}).items()},
fixed_direct_zero_offsets_rad={}, static_output_zero_offsets_rad={},
fixed_direct_zero_offsets_rad={name: value.cad_angle_rad for name, value in profile.zero.known_baseline_geometry.items()
if name in direct}, static_output_zero_offsets_rad={},
base_pose_strategy=str(data.get("base_pose_strategy", "full_hand")),
orientation_anchor_joint=data.get("orientation_anchor_joint"),
directed_base_axis_joints=frozenset(data.get("directed_base_axis_joints", ())),
accept_validated_zero_in_confidence_interval=bool(data.get("accept_validated_zero_in_confidence_interval", False)),
project_axis_gauge_before_image=bool(data.get("project_axis_gauge_before_image", False)),
parallel_root_pattern=bool(data.get("parallel_root_pattern", False)))
parallel_root_pattern=bool(data.get("parallel_root_pattern", False)),
independent_observed_motion=profile.artifacts.output_schema_version >= 3)
def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, records_by_joint,
*, cross_view_records=None) -> CalibrationResult:
*, cross_view_records=None, zero_references=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
@@ -81,8 +83,15 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
raise ValueError(f"spatial calibration requires identified common-frame pose observations:{joint}")
model = UrdfKinematicModel(source_urdf)
zero_profile = compile_spatial_profile(profile)
motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records)
motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records,
zero_references=zero_references)
motion_fitted(motion)
curves = motion.coordinate_curves
measured = profile.artifacts.output_schema_version >= 3
spatial_curves = {name: replace(curve, circle={**curve.circle,
"known_cad_baseline_rad": profile.zero.known_baseline_geometry[name].cad_angle_rad})
if measured and name in profile.zero.known_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 = {}
@@ -97,7 +106,7 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
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 = (task.start_value if profile.command.unit == "rad"
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),
@@ -111,7 +120,7 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
observations.append(axis)
by_joint[joint] = axis
zero = solve_urdf_zero_offsets(source_urdf=source_urdf, measurements=observations,
curves=curves, motor_by_joint={joint: channel_for_joint(profile, joint) for joint in curves},
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,
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),
@@ -121,6 +130,16 @@ def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, reco
raise ValueError("spatial_zero_not_observable_or_inaccurate:" + str(zero.failure_reasons))
offsets = {name: float(zero.direct_offsets_rad[name]) for name in profile.zero.direct_zero_joints}
methods = {name: "urdf_serial_axis_geometry" for name in offsets}
if measured:
from .mimic_approximation import fit_mimic_approximations
for name, datum in profile.zero.known_baseline_geometry.items():
offsets[name], methods[name] = datum.cad_angle_rad, "independent_known_baseline_geometry"
mappings = {name: JointMapping(name, channel_for_joint(profile, name), profile.curve_input_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),
mimic_approximations=fit_mimic_approximations(model, curves, records_by_joint))
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}")
@@ -192,6 +192,7 @@ def _angles_from_state(
if not knots[0] <= value <= knots[-1]:
raise ValueError(f"spatial condition outside measured native support:{joint}:{value}")
result[joint] = float(np.interp(value, knots, curve.angle_rad))
result[joint] += float(curve.circle.get("known_cad_baseline_rad", 0.0))
else:
if not 0 <= value <= 255:
raise ValueError(f"spatial condition outside byte domain:{joint}")
@@ -57,6 +57,7 @@ class ObservationGeometry:
measurement.joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
def predicted_palm_orientation_local(
@@ -78,6 +79,7 @@ class ObservationGeometry:
measurement.model_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
return axis / np.linalg.norm(axis)
@@ -201,11 +203,13 @@ class ObservationGeometry:
anchor_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
predicted_observer, _ = model.axis_line(
observer_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
observed_anchor = np.asarray(
anchor.axis_common_xyz, dtype=float
@@ -272,6 +276,7 @@ class ObservationGeometry:
parent_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
parent_axis = base_rotation.apply(parent_axis)
error = _zero_sensitive_axis_error_rad(
@@ -298,6 +303,7 @@ class ObservationGeometry:
parent_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
parent_axis = base_rotation.apply(parent_axis)
parent_point = (
@@ -66,6 +66,7 @@ class ZeroCalibrationProfile:
parallel_root_pattern: bool = False
common_mode_zero_joints: tuple[str, ...] = ()
small_offset_joints: frozenset[str] = frozenset()
independent_observed_motion: bool = False
@property
def reference_finger(self) -> str:
@@ -75,11 +75,13 @@ def check_observation_geometry(
observer_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
parent_axis, _ = model.axis_line(
parent_joint,
zero_offsets=offsets,
joint_angles=angles,
independent_mimic_angles=profile.independent_observed_motion,
)
cone_mismatches.append(
_axis_cone_mismatch_rad(
@@ -42,7 +42,7 @@ def _trajectory_score(parent, child):
def resolve_chain_observations(records, *, task_key, role_pairs,
projection_override=None, solve_pose=solve_square_tag_ippe):
projection_override=None, solve_pose=solve_square_tag_ippe, fixed_roles=()):
"""Return copied observations and auditable candidate-selection evidence.
Older captures did not save P. They remain on the legacy path; K is never
@@ -54,7 +54,7 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
if not role_pairs or len(roles) > 8 or any(len(pair) != 2 or pair[0] == pair[1] for pair in role_pairs.values()):
raise ValueError('candidate selection requires a bounded articulated observation graph')
joint_rows = [r for r in rows if r.get('joint') in role_pairs
and r.get('task_name') == task_key]
and r.get('task_name') == task_key and r.get('sample_phase') != 'joint_zero']
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}
@@ -87,13 +87,18 @@ def resolve_chain_observations(records, *, task_key, role_pairs,
matrix = np.asarray(frame['camera_matrix'] if projection_override is None else projection_override)
for role in roles:
data = frame['roles'][role]
poses = solve_pose(data['corners_xy'], tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=matrix)
limit = float(data.get('maximum_reprojection_error_px', 1.5))
poses = [p for p in poses if p.reprojection_error_px <= limit]
if not poses:
raise ValueError(f'corner replay has no eligible pose:{stamp}:{role}')
cs = [dict(quaternion_xyzw=list(p.quaternion_xyzw), translation_xyz_m=list(p.translation_xyz_m),
reprojection_error_px=p.reprojection_error_px) for p in poses[:2]]
if data.get('locked_reference'):
if role not in fixed_roles or not data.get('selected'):
raise ValueError('cached fixed reference has no frozen pose')
cs = [dict(data['selected'], reprojection_error_px=0.0)]
else:
poses = solve_pose(data['corners_xy'], tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=matrix)
limit = float(data.get('maximum_reprojection_error_px', 1.5))
poses = [p for p in poses if p.reprojection_error_px <= limit]
if not poses:
raise ValueError(f'corner replay has no eligible pose:{stamp}:{role}')
cs = [dict(quaternion_xyzw=list(p.quaternion_xyzw), translation_xyz_m=list(p.translation_xyz_m),
reprojection_error_px=p.reprojection_error_px) for p in poses[:2]]
if len(cs) == 1:
cs.append(cs[0])
if paths[role][0]:
@@ -65,11 +65,12 @@ class SerializedJointMapping:
def __init__(self, payload: Mapping[str, Any], *, input_kind=None) -> None:
self.payload = payload
self.compact = payload.get("format") == "unified_calibration_v2"
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")
self.unified = payload.get("format") == "unified_calibration_v1"
self.unified = payload.get("format") in {"unified_calibration_v1", "unified_calibration_report_v3"}
if self.compact:
if input_kind != "command" or payload.get("schema_version") != 2:
if input_kind != "command" or payload.get("schema_version") != (3 if self.measured else 2):
raise ValueError("compact JSON is command-only; feedback mapping belongs to the report")
unit = payload.get("input_unit")
if unit not in {"rad", "u8"}:
@@ -84,7 +85,7 @@ class SerializedJointMapping:
"curve_input_knots_rad": row.get("input_values", ())}
payload = {"joints": joints, "curve_input_domain": f"command_{unit}"}
elif self.unified:
if input_kind not in {"command", "feedback"} or payload.get("schema_version") != 1:
if input_kind not in {"command", "feedback"} or payload.get("schema_version") != (3 if self.measured else 1):
raise ValueError("unsupported unified mapping kind/version")
joints = {}
unit = payload["command_unit"]
@@ -94,7 +95,7 @@ class SerializedJointMapping:
for name, row in payload["joints"].items():
if row.get("urdf_joint") != name:
raise ValueError("unified keys must be original URDF joint names")
if row.get("passive"):
if row.get("passive") and not self.measured:
if "command_to_rad" in row or "feedback_to_rad" in row:
raise ValueError("passive runtime curves cannot bypass standard URDF mimic")
continue
@@ -273,7 +274,7 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str
for branch in ("angle_rad", "increasing_rad", "decreasing_rad"):
validate_runtime_curve_limits(corrected_urdf,
{name: row.get(branch, row["angle_rad"]) for name, row in mapping.joints.items()})
if payload.get("format") == "unified_calibration_v1":
if payload.get("format") in {"unified_calibration_v1", "unified_calibration_report_v3"}:
commands = SerializedJointMapping(payload, input_kind="command")
for branch in ("angle_rad", "increasing_rad", "decreasing_rad"):
validate_runtime_curve_limits(corrected_urdf,
@@ -283,7 +284,7 @@ def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str
joint = after.joints[name]
expected = {"joint": joint.mimic_joint, "multiplier": joint.mimic_multiplier,
"offset_rad": joint.mimic_offset}
actual = row.get("mimic", {})
actual = row.get("mimic_approximation" if mapping.measured else "mimic", {})
if (actual.get("joint") != expected["joint"] or any(not math.isclose(
float(actual.get(field, float("nan"))), expected[field], abs_tol=1e-8, rel_tol=0)
for field in ("multiplier", "offset_rad"))):
@@ -304,9 +305,24 @@ def validate_compact_urdf_tables(payload, model):
raise ValueError(f"compact table exceeds exported URDF range:{name}")
if joint.mimic_joint:
parent = mapping.joints[joint.mimic_joint]
if mapping.measured:
if row["motor_index"] != parent["motor_index"] or abs(joint.mimic_offset) > 1e-10:
raise ValueError(f"v3 mimic channel or shared zero differs:{name}")
continue
if (row["motor_index"] != parent["motor_index"]
or row["curve_input_knots_rad"] != parent["curve_input_knots_rad"]
or len(values) != len(parent["angle_rad"])
or not np.allclose(values, joint.mimic_multiplier*np.asarray(parent["angle_rad"])
+ joint.mimic_offset, atol=1e-8, rtol=0)):
raise ValueError(f"compact passive table differs from exported URDF mimic:{name}")
if mapping.measured:
baseline = payload.get("baseline_command", ())
if not baseline or not np.all(np.isfinite(baseline)):
raise ValueError("v3 baseline vector is missing or invalid")
for name, row in mapping.joints.items():
index = row["motor_index"]
if index >= len(baseline):
raise ValueError("v3 baseline channel missing")
value = mapping.evaluate(baseline, "", active_joints=(name,))[name]
if abs(value) > 1e-10:
raise ValueError(f"v3 baseline is not zero:{name}")
@@ -174,8 +174,8 @@ class UrdfKinematicModel:
transform = transform @ joint.origin @ self._motion(joint, float(zero_offsets.get(joint.name, 0.0)) + resolved[joint.name])
return transform
def axis_line(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float]) -> tuple[np.ndarray, np.ndarray]:
resolved = self.resolve_angles(joint_angles)
def axis_line(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float], independent_mimic_angles: bool = False) -> tuple[np.ndarray, np.ndarray]:
resolved = self.resolve_angles(joint_angles, independent_mimic_angles=independent_mimic_angles)
transform = np.eye(4)
for joint in self._chain(target_joint):
frame = transform @ joint.origin @ self._motion(joint, float(zero_offsets.get(joint.name, 0.0)))
@@ -0,0 +1,34 @@
"""Read-back v3 command errors against raw, independent fourth-round angles."""
from dataclasses import asdict
from .acceptance import SerializedJointMapping, angular_metrics
from ..fitting.spatial import measure_joint_curve_observation
def validate_measured_joint_holdout(payload, curves, observations):
mapping = SerializedJointMapping(payload, input_kind="command")
if not mapping.measured or not mapping.compact or set(mapping.joints) != set(curves):
raise ValueError("v3 final mapping differs from independently measured joints")
errors = {name: {d: [] for d in ("increasing", "decreasing")} for name in curves}
seen = set()
unit = payload["input_unit"]
for row in observations:
name, identity = row["joint"], row["sample_id"]
if name not in curves or row["cycle"] != 3 or row.get("sample_phase") != "steady":
raise ValueError("v3 angle holdout has wrong partition or joint")
if (name, identity) in seen or identity in curves[name].circle["training_sample_ids"]:
raise ValueError("v3 angle holdout duplicates or overlaps training")
seen.add((name, identity))
measured = measure_joint_curve_observation(curves[name], quaternion_xyzw=row["relative_quaternion_xyzw"])
predicted = mapping.evaluate(row[f"command_vector_{unit}"], "", active_joints=(name,))[name]
errors[name][row["direction"]].append(predicted-measured)
results = {}
for name, branches in errors.items():
if any(len(values) < 3 for values in branches.values()):
raise ValueError(f"missing_v3_joint_holdout:{name}")
metrics = {direction: angular_metrics(values) for direction, values in branches.items()}
if not all(value.passed for value in metrics.values()):
raise ValueError(f"serialized_measured_mapping_inaccurate:{name}:{metrics}")
results[name] = {key: asdict(value) for key, value in metrics.items()}
return results
@@ -49,6 +49,7 @@ def build_standard_correction_plan(
measured_ranges_output_rad: Mapping[str, tuple[float, float]] | None = None,
fitted_mimics_cad: Mapping[str, LinearMimicFit] | None = None,
transferred_zero_sources: Mapping[str, str] | None = None,
require_shared_mimic_zero: bool = False,
) -> StandardUrdfPlan:
"""Generate authorized standard-URDF edits, never clamp an invalid fit.
@@ -116,6 +117,10 @@ def build_standard_correction_plan(
if not math.isfinite(multiplier) or not math.isfinite(offset):
raise ValueError(f"nonfinite mimic fit: {name}")
shifted = corrected_mimic_offset(multiplier, offset, offsets.get(joint.mimic_joint, 0), delta)
if require_shared_mimic_zero:
if fit is None or not fit.offset_observed or abs(shifted) > 1e-12:
raise ValueError(f"mimic lacks two supported baseline datums:{name}")
shifted = 0.0 # Normalize only floating-point cancellation in the coordinate conversion.
output_mimics[name] = (joint.mimic_joint, multiplier, shifted)
if multiplier != joint.mimic_multiplier:
fields["mimic_multiplier"] = f"{multiplier:.15g}"
@@ -21,6 +21,8 @@ class PreparedCalibration:
def prepare_standard_result(profile: CalibrationProfile, source: Path,
source_sha256: str, fit: CalibrationResult) -> PreparedCalibration:
model = UrdfKinematicModel(source)
if profile.artifacts.output_schema_version >= 3:
return prepare_measured_result(profile, model, source_sha256, fit)
mimics = {name: evidence.fit for name, evidence in fit.standard_mimic_evidence.items()}
for target, donor in profile.zero.transferred_mimic_sources.items():
original = model.joints[target]
@@ -55,3 +57,30 @@ def prepare_standard_result(profile: CalibrationProfile, source: Path,
for joint in plan.mimic_output:
resolve(joint)
return PreparedCalibration(plan, mappings)
def prepare_measured_result(profile, model, source_sha256, fit):
moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"}
missing = moving - fit.output_mappings.keys()
geometry = moving - fit.zero_offsets_rad.keys()
if missing or geometry:
raise ValueError(f"independent_release_incomplete:unmeasured={sorted(missing)}:absolute_geometry={sorted(geometry)}")
if set(fit.output_mappings) != moving or any(m.transferred_from for m in fit.output_mappings.values()):
raise ValueError("independent_release_contains_transferred_or_unknown_joints")
passive = {name for name, joint in model.joints.items() if joint.mimic_joint}
if set(fit.mimic_approximations) != passive:
raise ValueError("independent_release_missing_paired_mimic_observations")
mimics = {name: evidence.in_cad_coordinates(fit.zero_offsets_rad)
for name, evidence in fit.mimic_approximations.items()}
# Keep legal mechanical limits for passive approximations; do not expand
# them to disguise an invalid linear reachable range.
ranges = {name: (min(fit.output_mappings[name].bounds_rad[0], fit.command_mappings[name].bounds_rad[0], 0),
max(fit.output_mappings[name].bounds_rad[1], fit.command_mappings[name].bounds_rad[1], 0))
for name in moving - passive}
plan = build_standard_correction_plan(source_urdf=model.source, source_sha256=source_sha256,
zero_offsets_rad=fit.zero_offsets_rad, measured_ranges_output_rad=ranges,
fitted_mimics_cad=mimics, authorized_fields=profile.urdf_authorized_fields, require_shared_mimic_zero=True)
for name, (_, _, offset) in plan.mimic_output.items():
if abs(offset) > 1e-12:
raise ValueError(f"mimic_does_not_pass_shared_zero:{name}")
return PreparedCalibration(plan, dict(fit.output_mappings))
@@ -34,6 +34,8 @@ def validate_serialized_tag_holdout(
observations: Sequence[TagHoldout], required_roles: Sequence[str],
minimum_samples: int = 40,
input_kind: str = "feedback",
mimic_approximation: bool = False,
enforce_precision: bool = True,
) -> dict[str, dict[str, Any]]:
model = UrdfKinematicModel(corrected_urdf)
mapping = SerializedJointMapping(payload, input_kind=input_kind)
@@ -43,6 +45,9 @@ def validate_serialized_tag_holdout(
raise ValueError("final Tag holdout requires declared frozen installations")
active = tuple(name for name, joint in model.joints.items()
if joint.kind != "fixed" and joint.mimic_joint is None)
independent = mapping.measured and not mimic_approximation
if independent:
active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed")
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}
@@ -60,12 +65,13 @@ def validate_serialized_tag_holdout(
raise ValueError("frozen Tag installation has a different role/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).items():
for name, value in model.resolve_angles(angles, independent_mimic_angles=independent).items():
joint = model.joints[name]
if joint.lower is not None and not joint.lower - 1e-9 <= value <= joint.upper + 1e-9:
raise ValueError(f"serialized URDF motion exceeds joint limit:{name}")
transform = np.eye(4) if mount.link == model.root_link else model.link_transform(
model.parent_joint_by_child[mount.link], zero_offsets={}, joint_angles=angles)
model.parent_joint_by_child[mount.link], zero_offsets={}, joint_angles=angles,
independent_mimic_angles=independent)
predicted = base @ transform @ rigid_matrix(mount.link_from_tag)
observed = rigid_matrix(row.common_from_tag)
angle = float(Rotation.from_matrix(predicted[:3, :3].T @ observed[:3, :3]).magnitude())
@@ -77,7 +83,7 @@ def validate_serialized_tag_holdout(
raise ValueError(f"missing independent Tag holdout samples:{role}:{len(values)}")
metrics = angular_metrics([value[0] for value in values])
translation_p95 = float(np.percentile([value[1] for value in values], 95))
if not metrics.passed or translation_p95 > 0.003:
if enforce_precision and (not metrics.passed or translation_p95 > 0.003):
raise ValueError(f"serialized_urdf_tag_holdout_failed:{role}:mae_deg={metrics.mae_deg}:p95_deg={metrics.p95_deg}:max_deg={metrics.maximum_deg}:translation_p95_m={translation_p95}")
results[role] = {"count": metrics.count, "mae_deg": metrics.mae_deg,
"p95_deg": metrics.p95_deg, "maximum_deg": metrics.maximum_deg,
@@ -8,7 +8,7 @@ import re
from typing import Any, Mapping
import yaml
from ..core.domain.profile import ReferenceWaypoint
from ..core.domain.profile import ReferenceWaypoint, JointZeroSpec, KnownBaselineGeometry
from ..core import (
AcquisitionPolicy,
@@ -65,6 +65,10 @@ def _validate_keys(payload):
_keys(task, {f.name for f in fields(TaskSpec)}, "task")
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():
_keys(spec, {f.name for f in fields(JointZeroSpec)}, "joint zero reference")
for spec in payload.get("zero", {}).get("known_baseline_geometry", {}).values():
_keys(spec, {f.name for f in fields(KnownBaselineGeometry)}, "baseline geometry")
for name, measurement in payload.get("measurement", {}).get("measurements", {}).items():
_keys(measurement, {f.name for f in fields(MeasurementSpec)}, "measurement item")
if measurement.get("joint", name) != name:
@@ -253,6 +257,9 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile:
tuple(ReferenceWaypoint(str(row["key"]), tuple(map(float, row["command"])),
{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", ())))
for name, row in motion.get("joint_zero_references", {}).items()},
),
measurement=MeasurementPolicy(
measurements,
@@ -279,6 +286,9 @@ def load_hand_profile(path: str | Path) -> CalibrationProfile:
dict(zero.get("transferred_zero_sources", {})),
dict(zero.get("transferred_mimic_sources", {})),
dict(zero.get("spatial", {})),
{str(name): KnownBaselineGeometry(float(row["cad_angle_rad"]),
float(row["uncertainty_rad"]), str(row["evidence_source"]))
for name, row in zero.get("known_baseline_geometry", {}).items()},
),
quality=QualityPolicy(
tuple(int(value) for value in quality.get("training_cycles", ())),
@@ -14,15 +14,19 @@ def compile_tag_links(profile: CalibrationProfile, model: UrdfKinematicModel) ->
Ambiguous branches require an explicit ``TagSpec.link``; no name guessing.
"""
result = {}
def bind(role, link):
if role in result and result[role] != link:
raise ValueError(f"physical Tag has conflicting link bindings:{role}")
result[role] = link
for view in profile.vision.views:
for tag in view.tags:
if tag.link is not None:
if tag.link not in model.links:
raise ValueError(f"Tag references missing URDF link:{tag.role}:{tag.link}")
result[tag.role] = tag.link
bind(tag.role, tag.link)
continue
if tag.fixed_reference:
result[tag.role] = model.root_link
bind(tag.role, model.root_link)
continue
candidates = {
profile.command.urdf_joint_by_joint.get(name, name)
@@ -36,5 +40,5 @@ def compile_tag_links(profile: CalibrationProfile, model: UrdfKinematicModel) ->
if candidates <= {joint.name for joint in model._chain(name)}]
if len(descendants) != 1:
raise ValueError(f"Tag is assigned to different kinematic branches:{tag.role}")
result[tag.role] = model.joints[descendants[0]].child
bind(tag.role, model.joints[descendants[0]].child)
return result
@@ -26,8 +26,13 @@ def validate_executable_profile(profile, source_urdf):
raise ValueError(f"unsupported measurement primitive:{name}:{spec.kind}")
if not set(profile.zero.axis_joints) <= measured:
raise ValueError("spatial observer axes require declared measured tasks")
if profile.artifacts.output_schema_version != 2:
raise ValueError("production calibration requires unified output_schema_version: 2; legacy formats are read-only")
if profile.artifacts.output_schema_version not in {2, 3}:
raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only")
if profile.artifacts.output_schema_version == 3:
required = {name for task in profile.motion.tasks for name in task.joints}
if required != set(profile.motion.joint_zero_references):
raise ValueError("v3 observing tasks require explicit per-joint baseline references")
_validate_separable_observations(profile, model, links)
for target, source in profile.zero.mimic_source_by_joint.items():
if target not in model.joints or model.joints[target].mimic_joint != source:
raise ValueError(f"Profile mimic topology differs from CAD:{target}")
@@ -46,10 +51,29 @@ def validate_executable_profile(profile, source_urdf):
conflicts.append({"joint": name, "source_joint": joint.mimic_joint,
"reachable_rad": [lo, hi], "cad_limits_rad": [joint.lower, joint.upper],
"measured_mimic_authorized": name in profile.zero.fitted_mimic_joints})
coverage = {"unmeasured_joints": sorted({n for n, j in model.joints.items() if j.kind != "fixed"}-measured),
"missing_baseline_geometry": sorted((profile.zero.active_joints | profile.zero.passive_joints)
-set(profile.zero.direct_zero_joints)-set(profile.zero.known_baseline_geometry)),
"transferred_joints": sorted(set(profile.zero.transferred_zero_sources)|set(profile.zero.transferred_mimic_sources))}
return {"profile_id": profile.key.profile_id, "tag_links": links,
"independent_measurement_coverage": coverage,
"zero_observers": dict(spatial.offset_observer_joint),
"cad_retained": sorted(profile.zero.cad_frozen_joints),
"transferred": dict(profile.zero.transferred_zero_sources),
"cad_constraint_conflicts": conflicts,
"observability": "structural_checks_only; numerical_rank_and_holdout_required",
"arbitrary_multiaxis_validated": False}
def _validate_separable_observations(profile, model, links):
from ..core.fitting.motion_fit import channel_for_joint
def chain(link):
return set() if link == model.root_link else {j.name for j in model._chain(model.parent_joint_by_child[link])
if j.kind != "fixed"}
for task in profile.motion.tasks:
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)}")
@@ -21,8 +21,9 @@ def finish_online_artifacts(
if not path.is_file() or not path.resolve().is_relative_to(session.resolve()):
raise ValueError(f"node completion refers to a missing/external artifact: {path}")
payload = json.loads(json_path.read_text(encoding="utf-8"))
unified = profile.artifacts.output_schema_version == 2
if unified and (payload.get("format") != "unified_calibration_v2" or payload.get("schema_version") != 2):
version = profile.artifacts.output_schema_version
unified = version in {2, 3}
if unified and (payload.get("format") != f"unified_calibration_v{version}" or payload.get("schema_version") != version):
raise ValueError("completed JSON format differs from selected Profile")
if not unified and payload.get("schema_version") != profile.artifacts.output_schema_version:
raise ValueError("completed JSON schema differs from selected Profile")
@@ -39,8 +39,8 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
immediately before publication and never returns the hand to a pose.
"""
del timestamp
if profile.artifacts.output_schema_version != 2:
raise ValueError("production calibration requires unified output_schema_version: 2; legacy formats are read-only")
if profile.artifacts.output_schema_version not in {2, 3}:
raise ValueError("production calibration requires unified output_schema_version: 2 or 3; legacy formats are read-only")
directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve()
directory.mkdir(parents=True, exist_ok=True)
def check_cancelled():
@@ -54,6 +54,7 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
if task.key not in profile.measurement.candidate_selection_tasks:
continue
records, report = resolve_chain_observations(records, task_key=task.key,
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})
pose_reports[task.key] = report
@@ -63,7 +64,19 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
accepted = accepted_joint_records(profile, records, directions_are_task_relative=directions_are_task_relative)
secondary = accepted_secondary_records(profile, records, directions_are_task_relative=directions_are_task_relative)
try:
fit = fit_profile_calibration(profile, source, accepted, cross_view_records=secondary)
from ...core.domain.reference import read_joint_zero_references
if profile.artifacts.output_schema_version >= 3:
from ...profiles.validator import validate_executable_profile
validate_executable_profile(profile, source)
references = read_joint_zero_references(profile, records) if profile.artifacts.output_schema_version >= 3 else None
def save_relative_motion(motion):
if profile.artifacts.output_schema_version >= 3:
atomic_write_json(directory / "relative_motion_diagnostics.json", {
"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)
@@ -73,7 +86,12 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
raise
check_cancelled()
phase_changed("fit_complete")
prepared = prepare_standard_result(profile, source, protected_inputs["source_urdf_sha256"], fit)
try:
prepared = prepare_standard_result(profile, source, protected_inputs["source_urdf_sha256"], fit)
except ValueError as error:
atomic_write_json(directory / "fit_diagnostics.json", {"passed": False, "reason": str(error),
"publication_allowed": False, "relative_motion": {name: asdict(curve) for name, curve in fit.curves.items()}})
raise
zero = fit.spatial_zero
replay_records = [row for rows in accepted.values() for row in rows]
replay_records.extend(dict(row, joint=profile.measurement.cross_view_sources[name])
@@ -93,9 +111,15 @@ def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf
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)
report = serialize_report(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs)
payload = from_report(report)
command_observations=command_holdout,
joint_observations=tuple(r for rows in steady.values() for r in rows if r["cycle"] == 3),
joint_curves=fit.curves,
joint_channels={name: mapping.motor_index for name, mapping in fit.output_mappings.items()})
report_writer, command_writer = serialize_report, from_report
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)
payload = command_writer(report)
naming = {"serial_number": serial_number, "side": profile.key.side, "model": profile.key.model.lower()}
json_path = directory / profile.artifacts.calibration_filename.format(**naming)
urdf_path = directory / profile.artifacts.corrected_urdf_filename.format(**naming)
@@ -19,6 +19,7 @@ from ...core.fitting.tag_installation import FrozenTagInstallation
from ...core.urdf.tag_acceptance import TagHoldout, validate_serialized_tag_holdout
from ...core.domain.profile import ProfileKey
from .serializers.compact_v2 import FORMAT as COMPACT_FORMAT, REPORT_FILENAME, from_report
from .serializers.measured_v3 import FORMAT as MEASURED_FORMAT, from_report as measured_from_report
def _validate_identity(payload, profile_id):
@@ -101,16 +102,25 @@ class FrozenTagArtifactValidator:
minimum_samples: int = 40
protected_inputs: Mapping[str, str] = field(default_factory=dict)
command_observations: tuple[TagHoldout, ...] = ()
joint_observations: tuple[Mapping, ...] = ()
joint_curves: Mapping = field(default_factory=dict)
joint_channels: Mapping[str, int] = field(default_factory=dict)
def __call__(self, json_path: Path, urdf_path: Path) -> Mapping[str, Any]:
payload = json.loads(json_path.read_text(encoding="utf-8"))
_validate_identity(payload, self.profile_id)
feedback_payload = payload
if payload.get("format") == COMPACT_FORMAT:
measured = payload.get("format") == MEASURED_FORMAT
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 from_report(feedback_payload) != payload:
if (measured_from_report if measured else from_report)(feedback_payload) != payload:
raise ValueError("compact JSON differs from the frozen calibration report")
if measured:
if feedback_payload.get("protected_inputs") != dict(self.protected_inputs):
raise ValueError("v3 report protected inputs differ from the session")
if {name: row["sdk_channel"] for name, row in payload["joints"].items()} != dict(self.joint_channels):
raise ValueError("v3 SDK channel bindings differ from measured Profile bindings")
validate_artifact_structure(source_urdf=self.source_urdf,
corrected_urdf=urdf_path, source_sha256=self.source_sha256,
authorized_fields=self.authorized_fields, payload=feedback_payload,
@@ -124,7 +134,7 @@ class FrozenTagArtifactValidator:
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}:
if payload.get("format") in {"unified_calibration_v1", COMPACT_FORMAT, MEASURED_FORMAT}:
try:
command_metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path,
payload=payload, common_from_base=self.common_from_base,
@@ -139,7 +149,20 @@ class FrozenTagArtifactValidator:
"installations": {k: asdict(v) for k, v in self.installations.items()},
"observations": [asdict(row) for row in self.observations]}
evidence["steady_command_observations"] = [asdict(row) for row in self.command_observations]
return {"acceptance": "serialized_standard_urdf_tag_replay_v1",
extra = {}
if measured:
from ...core.urdf.measured_acceptance import validate_measured_joint_holdout
extra = {"independent_mapping_holdout": validate_measured_joint_holdout(payload,
self.joint_curves, self.joint_observations),
"full_json_urdf_holdout": command_metrics,
"urdf_mimic_approximation": validate_serialized_tag_holdout(corrected_urdf=urdf_path,
payload=payload, common_from_base=self.common_from_base, installations=self.installations,
observations=self.command_observations, required_roles=self.required_roles, minimum_samples=3,
input_kind="command", mimic_approximation=True, enforce_precision=False)}
evidence["joint_observations"] = self.joint_observations
evidence["frozen_joint_curves"] = {key: asdict(value) for key, value in self.joint_curves.items()}
return {"acceptance": "independently_measured_json_urdf_v3" if measured else "serialized_standard_urdf_tag_replay_v1",
**extra,
"profile_id": self.profile_id, "source_urdf_sha256": self.source_sha256,
"protected_inputs": dict(self.protected_inputs),
"holdout_cycle": 3, "tag_holdout": metrics,
@@ -211,7 +234,7 @@ class ArtifactPublisher:
value = json.loads(json_path.read_text(encoding="utf-8"))
if not isinstance(value, dict):
raise ValueError("calibration JSON root must be an object")
report_path = session / REPORT_FILENAME if value.get("format") == COMPACT_FORMAT else None
report_path = session / REPORT_FILENAME if value.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT} else None
report_hash = None
if report_path is not None:
if report_path in {json_path, urdf_path} or not report_path.is_file() or report_path.resolve().parent != session:
@@ -8,21 +8,25 @@ from pathlib import Path
from ...core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables
from ...core.urdf.kinematics import UrdfKinematicModel
from .serializers.compact_v2 import FORMAT as COMPACT_FORMAT, from_report
from .serializers.measured_v3 import FORMAT as MEASURED_FORMAT, from_report as measured_from_report
class UnifiedCommandMapper:
def __init__(self, payload, corrected_urdf, *, input_kind="command", expected_side=None, report=None):
compact = payload.get("format") == COMPACT_FORMAT
if payload.get("format") not in {"unified_calibration_v1", COMPACT_FORMAT}:
self.measured = payload.get("format") == MEASURED_FORMAT
compact = payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT}
if payload.get("format") not in {"unified_calibration_v1", COMPACT_FORMAT, MEASURED_FORMAT}:
raise ValueError("requires a unified calibration artifact")
if expected_side and payload["side"] != expected_side:
raise ValueError("calibration side does not match requested side")
if compact and (report is None or from_report(report) != payload):
if compact and (report is None or (measured_from_report if self.measured else from_report)(report) != payload):
raise ValueError("compact JSON requires its matching calibration report")
metadata = report if compact else payload
self.mapping = SerializedJointMapping(metadata if compact and input_kind == "feedback" else payload, input_kind=input_kind)
self.urdf = UrdfKinematicModel(corrected_urdf)
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")
if compact:
validate_compact_urdf_tables(payload, self.urdf)
if not self.mapping.compact and set(self.mapping.joints) != set(self.active):
@@ -56,7 +60,7 @@ class UnifiedCommandMapper:
directions[i] = "increasing" if new > old else "decreasing"
active = self.mapping.evaluate(values, "", active_joints=self.active,
directions_by_joint={name: directions[row["motor_index"]] for name, row in self.mapping.joints.items()})
result = self.urdf.resolve_angles(active)
result = self.urdf.resolve_angles(active, independent_mimic_angles=self.measured)
for name, angle in result.items():
joint = self.urdf.joints[name]
if joint.lower is not None and not joint.lower-1e-9 <= angle <= joint.upper+1e-9:
@@ -84,6 +88,9 @@ def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="com
if validation.get("final_file_spatial_replay") != "passed" or not validation.get("steady_command_tag_holdout"):
raise ValueError("release lacks independent command and feedback URDF evidence")
payload = json.loads(json_path.read_text())
report = json.loads(artifact("calibration_report").read_text()) if payload.get("format") == COMPACT_FORMAT else None
if payload.get("format") == MEASURED_FORMAT and (validation.get("acceptance") != "independently_measured_json_urdf_v3"
or not validation.get("independent_mapping_holdout") or not validation.get("full_json_urdf_holdout")):
raise ValueError("v3 release lacks independent measured acceptance")
report = json.loads(artifact("calibration_report").read_text()) if payload.get("format") in {COMPACT_FORMAT, MEASURED_FORMAT} else None
return UnifiedCommandMapper(payload, urdf_path,
input_kind=input_kind, expected_side=expected_side, report=report)
@@ -0,0 +1,79 @@
"""Independent measured command tables. Serialization never fits or rebases."""
from dataclasses import asdict
import numpy as np
from .unified_v1 import _mapping
FORMAT = "unified_calibration_v3"
REPORT_FORMAT = "unified_calibration_report_v3"
ZERO_CONVENTION = "q_CAD=q_output+delta; joint_baseline:q_output=0"
def serialize_report(profile, fit, prepared, *, serial_number, protected_inputs):
expected = set(prepared.mappings)
if not expected or any(set(values) != expected for values in
(fit.command_mappings, fit.output_mappings, fit.zero_offsets_rad, fit.joint_zero_references)):
raise ValueError("independent_measurement_or_geometry_incomplete")
rows = {}
for name in sorted(expected):
if fit.command_mappings[name].transferred_from or fit.output_mappings[name].transferred_from:
raise ValueError(f"transferred_mapping_is_not_independent:{name}")
row = {"urdf_joint": name, "independently_measured": True,
"zero_offset_rad": fit.zero_offsets_rad[name], "zero_method": fit.zero_method_by_joint[name],
"zero_reference": fit.joint_zero_references[name].as_record(),
"command_to_rad": _mapping(fit.command_mappings[name]),
"feedback_to_rad": _mapping(fit.output_mappings[name]),
"applicability": fit.command_applicability[name],
"command_holdout": fit.command_holdout_metrics[name],
"passive": name in prepared.plan.mimic_output}
if row["passive"]:
parent, multiplier, offset = prepared.plan.mimic_output[name]
row["mimic_approximation"] = {"joint": parent, "multiplier": multiplier, "offset_rad": offset,
"evidence": asdict(fit.mimic_approximations[name])}
if name in profile.zero.known_baseline_geometry:
row["known_baseline_geometry"] = asdict(profile.zero.known_baseline_geometry[name])
rows[name] = row
return {"format": REPORT_FORMAT, "schema_version": 3,
"profile_id": profile.key.profile_id, "model": profile.key.model, "side": profile.key.side,
"serial_number": serial_number, "command_unit": profile.command.unit,
"command_names": list(profile.command.names), "baseline_command": list(profile.command.baseline_values),
"feedback_by_index": profile.command.feedback_by_index,
"feedback_name_aliases": dict(profile.command.feedback_name_aliases),
"coordinate_convention": ZERO_CONVENTION, "protected_inputs": dict(protected_inputs),
"joints": rows, "quality": {"training_cycles": [0, 1, 2], "holdout_cycle": 3,
"precision_scope": "observed_travel_directions_and_held_poses",
"final_file_acceptance": "release_manifest.json", "arbitrary_multiaxis_validated": False}}
def from_report(report):
from ....core.urdf.acceptance import SerializedJointMapping
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")
unit, baseline, rows = report["command_unit"], report["baseline_command"], {}
for name, data in mapping.joints.items():
evidence = report["joints"][name]
if evidence.get("independently_measured") is not True or evidence.get("transferred_from_joint"):
raise ValueError(f"joint_is_not_independently_measured:{name}")
reference = evidence.get("zero_reference", {})
index = data["motor_index"]
if (reference.get("joint") != name or reference.get("channel") != index
or reference.get("baseline_command") != baseline[index] or not reference.get("samples")):
raise ValueError(f"joint_zero_evidence_differs:{name}")
knots = np.asarray(data["curve_input_knots_rad"])
angles = np.asarray(data["angle_rad"])
if not knots[0] <= baseline[index] <= knots[-1] or abs(np.interp(baseline[index], knots, angles)) > 1e-10:
raise ValueError(f"joint_baseline_is_not_zero:{name}")
row = {"sdk_channel": index}
if unit == "u8":
if knots[0] != 0 or knots[-1] != 255:
raise ValueError(f"compact_byte_requires_measured_full_command_domain:{name}")
row["angle_rad"] = np.interp(np.arange(256), knots, angles).tolist()
else:
row.update(input_values=knots.tolist(), angle_rad=angles.tolist())
rows[name] = row
return {"format": FORMAT, "schema_version": 3,
**{key: report[key] for key in ("profile_id", "model", "side", "serial_number")},
"input_unit": unit, "baseline_command": baseline,
"coordinate_convention": ZERO_CONVENTION, "joints": rows}
@@ -97,12 +97,18 @@ class ObservationCapture:
return (), movement
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 not streams or frame.feedback is None or frame.command is None:
return (), movement
locked = []
if base.role not in selected and frame.view in self.locked_poses:
selected[base.role] = self.locked_poses[frame.view]
locked.append(base.role)
roles = {role for _, _, spec in streams for role in (spec.parent_role, spec.child_role)}
if task.key in self.profile.measurement.candidate_selection_tasks and motion.phase != "joint_zero":
if any(role not in selected or (role not in frame.corners and role not in locked) for role in roles):
return (), movement # An incomplete articulated frame is a discarded sample.
rows = []
common_from_view = self.extrinsics.transform(frame.view)
unit = self.profile.command.unit
@@ -117,7 +123,7 @@ 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,
"sample_phase": "steady" if motion.phase == "steady" else "sweep",
"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],
@@ -140,17 +146,23 @@ class ObservationCapture:
elif joint in self.profile.measurement.image_curve_joints:
continue # Image-circle observations cannot be synthesized from a cached pose.
rows.append(row)
roles = {role for _, _, spec in streams for role in (spec.parent_role, spec.child_role)}
if motion.phase == "joint_zero":
row["kind"] = "joint_zero_sample"
row["joint"] = joint
if task.key in self.profile.measurement.candidate_selection_tasks:
rows.append({"kind": "pnp_candidate_frame", "task_name": task.key,
"cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt,
"view": frame.view, "image_stamp_ns": frame.stamp_ns, "tag_size_m": self.tag_size_m,
"camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]",
"input_is_rectified": True,
"roles": {role: {"corners_xy": np.asarray(frame.corners[role]).tolist(),
"roles": {role: {"corners_xy": np.asarray(frame.corners[role]).tolist() if role in frame.corners else None,
"locked_reference": role in locked,
"tag_size_m": next(tag.size_m for tag in view.tags if tag.role == role),
"selected": None if role not in selected else {
"quaternion_xyzw": list(selected[role].quaternion_xyzw),
"translation_xyz_m": list(selected[role].translation_xyz_m)},
"maximum_reprojection_error_px": self.trackers[frame.view].maximum_reprojection_error_px}
for role in roles if role in frame.corners}})
for role in roles if role in frame.corners or role in locked}})
return tuple(rows), movement
def _observe_installations(self, view, base_role, poses, *, prefix=""):
@@ -17,6 +17,7 @@ import numpy as np
from ..acquisition import StateSample, interpolate_state_u8
from ..core.domain.profile import CalibrationProfile
from ..core.domain.reference import build_joint_zero_reference
from ..storage import append_jsonl, atomic_write_json
from .engine import ACQUISITION_POLICY_VERSION
from .reference_lock import ReferenceLock
@@ -71,9 +72,12 @@ class CalibrationCoordinator:
self._steady_capture_after_ns = None
self._resume_message = "not_requested"
self._resumed_count = 0
from .joint_resume import JointResume
self.joint_resume = JointResume(profile)
self._unit_rows = []
self._capture_unit_key = None
self._steady_rows = []
self._zero_reference_error = ""
self._reference_capture_key = None
self._reference_observation_started = None
self._current_fingerprint = {}
@@ -186,7 +190,7 @@ class CalibrationCoordinator:
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), ()))
record_motion = motion
if motion is not None and motion.phase == "steady" and (steady_after is None or stamp < steady_after):
if motion is not None and motion.phase in {"steady", "joint_zero"} and (steady_after is None or stamp < steady_after):
record_motion = None
# Pose estimation is deliberately outside the state lock. Start swaps
# capture objects; pause/abort invalidates the epoch; movement changes
@@ -196,7 +200,7 @@ class CalibrationCoordinator:
if (epoch != self._observation_epoch or capture is not self.capture
or motion is not self._motion):
return
if motion is not None and motion.phase == "steady" and steady_after != self._steady_capture_after_ns:
if motion is not None and motion.phase in {"steady", "joint_zero"} and steady_after != self._steady_capture_after_ns:
return
if movement is not None:
self._pause(f"fixed_reference_moved:{view}:{movement.drift_px}")
@@ -204,7 +208,7 @@ class CalibrationCoordinator:
for row in rows:
self.raw_records.append(row)
self._unit_rows.append(row)
if motion is not None and motion.phase == "steady":
if motion is not None and motion.phase in {"steady", "joint_zero"}:
self._steady_rows.append(row)
append_jsonl(self.raw_path, row)
@@ -263,6 +267,16 @@ class CalibrationCoordinator:
self._poll_finalization()
return
phase = session.phase
if phase in {Phase.PREPARE, Phase.MAPPING_PROBE} and self.segment is None and session.current_unit is not None:
used, imported = self.joint_resume.take_task(session.current_unit.task_key)
if used:
self.execution.first_cycle_spans.update(self.joint_resume.spans_for_task(session.current_unit.task_key))
for row in imported:
self.raw_records.append(row)
append_jsonl(self.raw_path, row)
self.execution.authorize_task_reuse(used)
self._resumed_count += len(used)
return
if phase == Phase.REFERENCE_LOCKING:
self._publish_command(list(current))
if not self.reference_lock.locking_enabled:
@@ -330,6 +344,19 @@ class CalibrationCoordinator:
else:
self._steady_capture_after_ns = None
self._steady_rows.clear()
if motion.phase == "joint_zero":
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)
if session.phase == Phase.PAUSED:
return
else:
self._steady_capture_after_ns = None
self._steady_rows.clear()
if self.segment.arrived(now) and not steady_complete:
self._pause("joint_zero_unavailable:"+self._zero_reference_error)
return
if self.segment.arrived(now) or steady_complete:
if motion.phase == "reference_pose":
waypoint = next(w for w in self.profile.motion.resume_verification_waypoints if w.key == motion.task_key)
@@ -350,6 +377,38 @@ class CalibrationCoordinator:
self._save_reference()
self.segment, self._motion = None, None
def _lock_joint_zeros(self, motion):
references = {}
try:
for name in motion.zero_joints:
rows = [r for r in self._steady_rows if r.get("kind") == "joint_zero_sample" and r.get("joint") == name]
# A transient bad pose must age out once vision has settled.
# Keep the raw journal intact; freeze only the latest distinct
# frames per view, with the unchanged minimum/stability gate.
frames = {(r["view"], r["image_stamp_ns"]): r for r in rows}
minimum = self.profile.acquisition.fixed_reference_minimum_frames
rows = [row for view in sorted({r["view"] for r in rows})
for row in sorted((r for r in frames.values() if r["view"] == view),
key=lambda r: r["image_stamp_ns"])[-minimum:]]
references[name] = build_joint_zero_reference(self.profile, name, rows,
session_epoch=self._observation_epoch, motion_version=self._segment_number)
except ValueError as error:
self._zero_reference_error = str(error)
return False
if references.keys() & self.execution.zero_references.keys():
raise RuntimeError("a frozen joint reference cannot be replaced")
try:
references = {name: self.joint_resume.verify(reference) for name, reference in references.items()}
except ValueError as error:
self._pause(str(error))
return False
self.execution.zero_references.update(references)
for reference in references.values():
row = reference.as_record()
self.raw_records.append(row)
append_jsonl(self.raw_path, row)
return True
def _begin_motion_capture(self, motion):
"""Keep direction statistics across its separate steady checkpoints."""
self._steady_rows = []
@@ -560,6 +619,8 @@ class CalibrationCoordinator:
for _, _, spec in observation_streams(self.profile, task)
for role in (spec.parent_role, spec.child_role)
if not any(tag.role == role and tag.fixed_reference for tag in self._view(spec.view).tags)}
if self.profile.artifacts.output_schema_version >= 3:
required = set()
verifier = ResumeVerifier(required_fixed_views=self.profile.vision.view_names,
required_fixed_poses=self.profile.vision.view_names, required_moving_poses=required,
required_hashes=self._current_fingerprint["protected_hashes"])
@@ -570,7 +631,11 @@ class CalibrationCoordinator:
decision = verifier.compare(old, new)
if decision.reuse:
from dataclasses import replace
decision = replace(decision, completed_units=tuple(sorted(complete)))
if self.profile.artifacts.output_schema_version >= 3:
self.joint_resume.stage(self.execution.session.engine, rows, complete)
decision = replace(decision, reason="整场参考通过,等待逐关节验证原零位后复用")
else:
decision = replace(decision, completed_units=tuple(sorted(complete)))
except (OSError, ValueError, KeyError, TypeError) as error:
decision = ResumeDecision(False, decision.reason, (str(error),))
decision = self.execution.restore(decision, rows)
@@ -11,7 +11,7 @@ from .session import CalibrationPhase as Phase, CalibrationSession
from .motion_execution import MotionCommand
from .scan_quality import evaluate_capture_unit
from .trajectory import (build_calibration_preparation_waypoints,
build_calibration_return_waypoints, build_calibration_motion_command)
build_calibration_return_waypoints, build_calibration_motion_command, build_joint_reference_waypoints)
class SessionExecution:
@@ -24,6 +24,7 @@ class SessionExecution:
self._effects = []
self._action_key = None
self._entered_task = None
self.zero_references = {}
@property
def action(self):
@@ -69,6 +70,21 @@ class SessionExecution:
entry.append(MotionCommand("clearance", tuple(pose), unit.speed, **common))
self._entered_task = task.key
current = tuple(pose)
if self.profile.artifacts.output_schema_version >= 3:
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}
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):
entry.extend(MotionCommand("zero_approach", point, unit.speed, **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)
for target in build_calibration_preparation_waypoints(task, profile=self.profile,
current_command=current, start_value=unit.start))
@@ -98,6 +114,8 @@ 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 for name in self._effects[0].zero_joints):
raise RuntimeError("joint zero acquisition cannot complete without frozen evidence")
self._effects.pop(0)
if self._effects:
return
@@ -150,5 +168,11 @@ class SessionExecution:
self.completed_units = set(valid)
return checked
def authorize_task_reuse(self, units):
self.session.authorize_task_reuse(units)
self.completed_units.update(units)
self._effects.clear()
self._action_key = None
__all__ = ["SessionExecution"]
@@ -0,0 +1,78 @@
"""Stage old data until its individual zero poses have been checked again."""
import numpy as np
from scipy.spatial.transform import Rotation
from ..core.domain.reference import read_joint_zero_references
from .scan_quality import evaluate_capture_unit
from .resume import ResumeVerifier, TagPoseFingerprint
def _pose(samples, field):
poses = [getattr(sample, field) for sample in samples]
return TagPoseFingerprint(tuple(Rotation.from_quat([p.quaternion_xyzw for p in poses]).mean().as_quat()),
tuple(np.median([p.translation_xyz_m for p in poses], axis=0)))
class JointResume:
def __init__(self, profile):
self.profile = profile
self.references = {}
self.verified = set()
self.units = set()
self.rows = ()
self.first_cycle_spans = {}
def stage(self, engine, rows, units):
references = read_joint_zero_references(self.profile, rows)
spans, valid = {}, set()
for unit in engine.scan_units():
key = (unit.task_key, unit.cycle, unit.direction)
task = next(t for t in self.profile.motion.tasks if t.key == unit.task_key)
if key not in units or not set(task.joints) <= references.keys():
continue
attempts = [int(r.get("attempt", 1)) for r in rows
if (r.get("task_name"), r.get("cycle"), r.get("direction")) == key]
if evaluate_capture_unit(self.profile, unit, max(attempts, default=1), rows,
first_cycle_spans=spans).passed:
valid.add(key)
self.references, self.units, self.rows = references, valid, tuple(rows)
self.first_cycle_spans = spans
def spans_for_task(self, task_key):
from .scan_quality import observation_streams
task = next(t for t in self.profile.motion.tasks if t.key == task_key)
keys = {f"{field}:{name}:{spec.view}:{direction}" for field, name, spec in observation_streams(self.profile, task)
for direction in ("increasing", "decreasing")}
return {key: value for key, value in self.first_cycle_spans.items() if key in keys}
def verify(self, reference):
previous = self.references.get(reference.joint)
if previous is None:
return reference
failures = []
if previous.task_key != reference.task_key or previous.channel != reference.channel:
failures.append("reference_contract")
verifier = ResumeVerifier()
for view in {s.view for s in previous.samples}:
old = [s for s in previous.samples if s.view == view]
new = [s for s in reference.samples if s.view == view]
if not new or {s.command for s in old} != {s.command for s in new}:
failures.append(f"held_pose:{view}")
continue
for field in ("parent_pose_common", "child_pose_common"):
verifier._compare_poses(field, {view: _pose(old, field)}, {view: _pose(new, field)}, failures)
if failures:
raise ValueError(f"joint_resume_reference_changed:{reference.joint}:{','.join(failures)}; 旧数据不能与新零位拼接,请重新开始")
self.verified.add(reference.joint)
return previous # Preserve the original measured zero, including its identity.
def take_task(self, task_key):
task = next(t for t in self.profile.motion.tasks if t.key == task_key)
if not set(task.joints) <= self.verified:
return (), ()
units = {key for key in self.units if key[0] == task_key}
rows = tuple(r for r in self.rows if (r.get("task_name"), r.get("cycle"), r.get("direction")) in units
and r.get("kind") not in {"joint_zero_reference", "joint_zero_sample"})
self.units.difference_update(units)
return tuple(sorted(units)), rows
@@ -25,10 +25,11 @@ class MotionCommand:
direction: str | None = None
attempt: int = 1
steady_index: int | None = None
zero_joints: tuple[str, ...] = ()
@property
def recording(self):
return self.direction is not None
return self.direction is not None or bool(self.zero_joints)
class MotionExecution:
@@ -108,7 +109,7 @@ class MotionExecution:
any(abs(self.deltas[i])*self.fraction > self.minimum_request for i in self.moving))
def arrived(self, now):
if self.command.phase == "steady":
if self.command.phase in {"steady", "joint_zero"}:
# Data sufficiency is evaluated after the direction; never wait
# indefinitely for a Tag or exact command/feedback equality.
return (self.finished_at is not None and
@@ -127,17 +128,17 @@ class MotionExecution:
return True
def steady_ready(self, now):
if self.command.phase != "steady" or self.finished_at is None:
if self.command.phase not in {"steady", "joint_zero"} or self.finished_at is None:
return False
window = self.profile.acquisition.steady_window_seconds
rows = [(stamp, value) for stamp, value in self.stamped_history
if stamp >= max(self.finished_at, now-window-0.05)]
if len(rows) < 3 or rows[-1][0]-rows[0][0] < window:
return False
i = self.command.command_index
threshold = max(2*self.resolution, 0.005*(self.profile.command.maximum_feedback_values[i]
- self.profile.command.minimum_feedback_values[i]))
return max(v[i] for _, v in rows)-min(v[i] for _, v in rows) <= threshold
indices = range(self.profile.command.command_count) if self.command.phase == "joint_zero" else (self.command.command_index,)
return all(max(v[i] for _, v in rows)-min(v[i] for _, v in rows) <= max(2*self.resolution,
0.005*(self.profile.command.maximum_feedback_values[i]-self.profile.command.minimum_feedback_values[i]))
for i in indices)
__all__ = ["MotionCommand", "MotionExecution"]
@@ -20,6 +20,8 @@ STATE_LABELS = {
"ABORTED": "已中止",
}
PHASE_LABELS = {
"joint_zero": "采集并锁定当前关节零位",
"zero_approach": "准备关节零位观测姿态",
"baseline": "安全恢复基准形态",
"preflight": "任务运动预检",
"prepare": "扫描起点准备",
@@ -11,6 +11,15 @@ def reason_zh(
) -> tuple[str, str, str]:
"""Map a common runtime reason to a stable code, explanation and action."""
reason = str(status.get("reason", "unknown"))
if "joint_zero_unavailable" in reason:
return ("OBS-JOINT-ZERO-107", f"当前关节未取得稳定零位观测:{reason}",
"查看所列关节、机位和 Tag;排除持续遮挡或抖动后重新开始,已有零位不会被覆盖。")
if "joint_resume_reference_changed" in reason:
return ("OBS-JOINT-RESUME-108", f"当前关节的原零位或 Tag 安装验证失败:{reason}",
"旧样本不能与新零位拼接。检查安装,使用 --no-resume 开始新会话。")
if "independent_release_incomplete" in reason:
return ("FIT-INDEPENDENT-COVERAGE-509", f"独立实测或绝对几何依据不完整,已禁止发布:{reason}",
"查看相对运动诊断及缺失关节,补充实际 Tag 观测与有依据的几何条件;不能用 mimic、迁移或补零替代。")
if "compact_byte_requires_measured_full_command_domain" in reason:
return ("FIT-COMPACT-DOMAIN-507", "实测指令范围没有完整覆盖 0–255,不能生成完整的 256 项查表。",
"核对扫描范围和端点稳态数据;不得用反馈表、外推或裁剪补齐缺失指令。")
@@ -124,6 +124,10 @@ def run_online(
from ..profiles.validator import validate_executable_profile
contract_report = validate_executable_profile(profile, config.source_urdf)
atomic_write_json(session/"measurement_contract.json", contract_report)
coverage = contract_report["independent_measurement_coverage"]
if profile.artifacts.output_schema_version >= 3 and (coverage["unmeasured_joints"] or coverage["missing_baseline_geometry"]):
print("当前 Profile 的全关节实测/绝对几何依据不完整,本次可采集诊断数据,但不能发布完整 v3 JSON/URDF;"
"缺失关节见 measurement_contract.json。", flush=True)
if contract_report["cad_constraint_conflicts"]:
print("原始 CAD 存在限位/mimic 冲突,未修改原文件;详见 measurement_contract.json。最终范围验证不通过时禁止发布。", flush=True)
log_path = session / "calibration.log"
@@ -92,7 +92,7 @@ class CalibrationSession:
def reference_locked(self) -> None:
self._require(CalibrationPhase.REFERENCE_LOCKING)
self._reference_locked = True
if self.profile.motion.resume_verification_waypoints:
if self.profile.motion.resume_verification_waypoints and self.profile.artifacts.output_schema_version < 3:
self.phase = CalibrationPhase.REFERENCE_POSES
return
self._after_reference_poses()
@@ -110,6 +110,8 @@ class CalibrationSession:
def resume_checked(self, decision: ResumeDecision) -> None:
self._require(CalibrationPhase.RESUME_VERIFY)
if self.profile.artifacts.output_schema_version >= 3 and decision.completed_units:
raise ValueError("v3 resume requires per-joint zero verification before skipping units")
self._resume_message = decision.reason
if not decision.reuse:
self._unit_index = 0
@@ -146,6 +148,18 @@ class CalibrationSession:
self.phase = CalibrationPhase.RESCAN if self._rescan_pending else CalibrationPhase.SWEEP
self._rescan_pending = False
def authorize_task_reuse(self, units):
"""Only the coordinator can call this after checking frozen joint zeros."""
self._require_any(CalibrationPhase.PREPARE, CalibrationPhase.MAPPING_PROBE)
current = self.current_unit
valid = {(u.task_key, u.cycle, u.direction) for u in self._units}
requested = set(units)
if current is None or not requested <= valid or any(key[0] != current.task_key for key in requested):
raise ValueError("deferred resume must belong to the current task")
self._reused_units.update(requested)
self._advance_reused_units()
self.phase = CalibrationPhase.PREPARE
def mapping_probe_complete(self) -> None:
self._require(CalibrationPhase.MAPPING_PROBE)
unit = self.current_unit
@@ -73,6 +73,18 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot:
session, profile, parameters = host.execution.session, host.profile, host.parameters
status = session.status()
task, motion = status.task, host._motion
if motion is not None and motion.phase == "joint_zero":
required = {}
for view in profile.vision.views:
roles = {role for name in motion.zero_joints
for key in (name, profile.measurement.cross_view_sources.get(name)) if key is not None
for spec in (profile.measurement.measurements[key],) if spec.view == view.name
for role in (spec.parent_role, spec.child_role)}
ids = tuple(tag.tag_id for tag in view.tags if tag.role in roles)
if ids:
required[view.name] = ids
task = replace(task, label="采集关节零位:"+", ".join(motion.zero_joints),
required_tag_ids_by_view=required)
if session.phase == Phase.REFERENCE_POSES and motion is not None:
waypoint = next((w for w in profile.motion.resume_verification_waypoints
if w.key == motion.task_key), None)
@@ -105,7 +117,8 @@ def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot:
bins=min(m["feedback_bins"] for m in metrics),
maximum_gap=max(m["maximum_bin_gap"] for m in metrics))
status = replace(status, task=task, acquisition=acquisition,
phase=motion.phase if steady else status.phase,
phase=motion.phase if motion is not None and motion.phase in
{"steady", "steady_prepare", "joint_zero", "zero_approach"} else status.phase,
recognized_tag_ids=tuple(sorted(required_ids-missing-filtered)),
missing_tag_ids=tuple(sorted(missing)), rejected_tag_ids=tuple(sorted(filtered)),
log_path=str(parameters.session_dir/"calibration.log"),
@@ -25,6 +25,7 @@ _PHASES = {
"baseline": "安全恢复基准", "preflight": "固定通道映射点动",
"prepare": "扫描起点准备", "retry_prepare": "同速重扫起点准备", "sweep": "正式扫描",
"steady_prepare": "稳态指令采样起点准备", "steady": "稳态指令→rad 采样",
"joint_zero": "采集并冻结关节零位", "zero_approach": "关节零位到达准备",
}
@@ -1,16 +1,12 @@
"""Bounded, direction-specific command observations, separate from sweeps."""
import math
import numpy as np
def steady_targets(profile, unit):
lo, hi = sorted((unit.start, unit.end))
train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes)
values = train if unit.cycle != 3 else np.r_[lo, (train[:-1]+train[1:])/2, hi]
if profile.command.unit == "u8":
values = np.floor(values+0.5)
values = sorted(set(float(v) for v in values))
from ..core.domain.sampling import command_nodes
task = next(t for t in profile.motion.tasks if t.key == unit.task_key)
values = command_nodes(profile, task, holdout=unit.cycle == 3)
return tuple(values if unit.direction == "increasing" else reversed(values))
@@ -183,6 +183,16 @@ def build_calibration_return_waypoints(
return _grouped_waypoints(current_command, target, profile.motion.return_groups, profile=profile)
def build_joint_reference_waypoints(task, target, *, profile, current_command):
"""Visit an explicitly declared reference pose, which may be inside a scan."""
declared = {pose for spec in profile.motion.joint_zero_references.values() if spec.task_key == task.key
for pose in (spec.command, *spec.approach_commands)}
if tuple(target) not in declared:
raise ValueError("undeclared joint zero reference pose")
return _grouped_waypoints(current_command, target, task.preparation_groups,
profile=profile, measured_channel=task.command_index)
def _grouped_waypoints(current_command, target, groups, *, profile, measured_channel=None):
current = list(profile.command.baseline_values if current_command is None else current_command)
final = list(target)
+1 -1
View File
@@ -40,7 +40,7 @@ setup(
"share/ament_index/resource_index/packages",
["resource/" + package_name],
),
("share/" + package_name, ["package.xml", "README.md"]),
("share/" + package_name, ["package.xml", *sorted(glob("*.md"))]),
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
] + package_data_tree("config") + package_data_tree("urdf"),
install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
@@ -0,0 +1,106 @@
"""Synthetic v3 evidence; these declared geometric datums are NOT hardware facts."""
from dataclasses import replace
import numpy as np
from scipy.spatial.transform import Rotation
from test_profile_finalization import virtual_capture, rigid
from linkerhand_calibration.core.domain.profile import KnownBaselineGeometry, ProfileKey
from linkerhand_calibration.core.domain.reference import build_joint_zero_reference
from linkerhand_calibration.core.geometry.extrinsics import matrix_payload, transform_matrix
from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel
def reordered_profile(profile):
count = profile.command.command_count
index = lambda old: count-1-old
vector = lambda values: tuple(reversed(values))
cameras = {view.name: f"sensor_{i}" for i, view in enumerate(profile.vision.views)}
keys = {task.key: f"motion_{i}" for i, task in enumerate(reversed(profile.motion.tasks))}
layout = profile.command
command = replace(layout, **{field: vector(getattr(layout, field)) for field in
("names", "baseline_u8", "baseline", "lower_bounds", "upper_bounds", "feedback_lower_bounds",
"feedback_upper_bounds", "sdk_to_joint_direction", "maximum_velocity")},
command_index_by_joint={j: index(i) for j, i in layout.command_index_by_joint.items()},
disabled_indices=frozenset(index(i) for i in layout.disabled_indices),
speed_slot_by_command_index={index(i): value for i, value in layout.speed_slot_by_command_index.items()})
tasks = tuple(replace(task, key=keys[task.key], view=cameras[task.view], command_index=index(task.command_index),
auxiliary_commands=tuple((index(i), v) for i, v in task.auxiliary_commands),
preparation_groups=tuple(tuple(index(i) for i in group) for group in task.preparation_groups),
entry_waypoints=tuple(tuple((index(i), value) for i, value in group) for group in task.entry_waypoints))
for task in reversed(profile.motion.tasks))
references = {name: replace(spec, task_key=keys[spec.task_key], command=vector(spec.command),
approach_commands=tuple(vector(pose) for pose in spec.approach_commands))
for name, spec in profile.motion.joint_zero_references.items()}
return replace(profile, key=ProfileKey("VIRTUAL", "right", "reordered_observations"), namespace="/virtual",
command=command,
vision=replace(profile.vision, views=tuple(replace(view, name=cameras[view.name]) for view in profile.vision.views),
extrinsic_reference_view=cameras[profile.vision.extrinsic_reference_view]),
motion=replace(profile.motion, tasks=tasks, joint_zero_references=references,
return_groups=tuple(tuple(index(i) for i in group) for group in profile.motion.return_groups),
resume_verification_waypoints=()),
measurement=replace(profile.measurement,
candidate_selection_tasks=frozenset(keys[key] for key in profile.measurement.candidate_selection_tasks),
measurements={name: replace(spec, view=cameras.get(spec.view)) for name, spec in profile.measurement.measurements.items()}))
def measured_capture(tmp_path, *, nonlinear=True, reordered=False):
profile, source, records, offsets = virtual_capture(tmp_path, "g20_right_19", include_baseline=True,
profile_transform=reordered_profile if reordered else None)
model = UrdfKinematicModel(source)
for name, joint in model.joints.items():
if joint.mimic_joint:
offsets[name] = joint.mimic_multiplier*offsets.get(joint.mimic_joint, 0)+joint.mimic_offset
specs, seeds = {}, {}
for task in profile.motion.tasks:
for name in task.joints:
selected = [r for r in records if r.get("joint") == name and r["cycle"] == 0
and r["direction"] == "decreasing"
and r["command_u8"] == profile.command.baseline_values[task.command_index]]
seed = selected[0]
specs[name] = replace(profile.motion.joint_zero_references[name], command=tuple(seed["command_vector_u8"]))
seeds[name] = seed
profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=3),
motion=replace(profile.motion, joint_zero_references=specs),
urdf_authorized_fields={name: tuple(sorted(set(profile.urdf_authorized_fields.get(name, ()))
| {"origin.rpy", "limit.lower", "limit.upper"})) for name, joint in model.joints.items() if joint.kind != "fixed"},
zero=replace(profile.zero, known_baseline_geometry={name: KnownBaselineGeometry(value, 0, "synthetic_fixture_truth")
for name, value in offsets.items() if name not in profile.zero.direct_zero_joints}))
mount = rigid((.12, -.09, .2), (.009, -.025, .016))
identities = {identity: (i+1)*1000 for i, identity in enumerate(dict.fromkeys(r["sample_id"] for r in records))}
updated = []
for row in records:
row = dict(row, image_stamp_ns=identities[row["sample_id"]]+100000)
row["sample_id"] = f"{row['view']}:{row['image_stamp_ns']}"
name = row.get("joint")
if nonlinear and name in profile.zero.passive_joints:
joint = model.joints[name]
channel = next(t.command_index for t in profile.motion.tasks if name in t.joints)
x = (profile.command.baseline_values[channel]-row["state_u8"][channel])/255
# A monotonic 0.35*x^2 additional passive angle, independently
# generated in the physical link frame, never from fitted JSON.
extra = np.eye(4)
extra[:3, :3] = Rotation.from_rotvec(joint.axis*.35*x*x).as_matrix()
child = transform_matrix(
row["child_pose_common"]["translation_xyz_m"], row["child_pose_common"]["quaternion_xyzw"])
child = child @ np.linalg.inv(mount) @ extra @ mount
parent = transform_matrix(row["parent_pose_common"]["translation_xyz_m"], row["parent_pose_common"]["quaternion_xyzw"])
relative = np.linalg.inv(parent) @ child
row.update(child_pose_common=matrix_payload(child),
relative_quaternion_xyzw=Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(),
relative_translation_xyz_m=relative[:3, 3].tolist())
updated.append(row)
references = []
for index, (name, spec) in enumerate(specs.items()):
task = next(t for t in profile.motion.tasks if t.key == spec.task_key)
selected = [seeds[name]]
if name in profile.measurement.cross_view_sources:
selected.extend(r for r in records if r.get("observation_joint") == name and r["cycle"] == 0
and r["direction"] == "decreasing" and r["command_u8"] == profile.command.baseline_values[task.command_index]
and r.get("sample_phase", "sweep") == "sweep")
arrival = "increasing" if spec.command[task.command_index] > spec.approach_commands[-1][task.command_index] else "decreasing"
samples = [dict(seed, image_stamp_ns=1000+index*100+i, command_direction_by_index=[arrival]*profile.command.command_count)
for seed in selected for i in range(profile.acquisition.fixed_reference_minimum_frames)]
reference = build_joint_zero_reference(profile, name, samples, session_epoch=1, motion_version=index+1)
references.append(reference.as_record())
return profile, source, references+updated, offsets
@@ -15,7 +15,7 @@ from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
from runtime_host_fixture import coordinator_fixture, ready
@pytest.mark.parametrize("change", ["start", "pause", "abort", "motion", "steady"])
@pytest.mark.parametrize("change", ["start", "pause", "abort", "motion", "steady", "joint_zero"])
def test_late_pose_result_cannot_cross_session_or_motion_boundary(tmp_path, monkeypatch, change):
host, clock = coordinator_fixture(tmp_path)
entered, release = Event(), Event()
@@ -24,7 +24,7 @@ def test_late_pose_result_cannot_cross_session_or_motion_boundary(tmp_path, monk
if change != "start":
assert host.start().success
host.execution.session.phase = Phase.SWEEP
host._motion = MotionCommand("steady" if change == "steady" else "sweep",
host._motion = MotionCommand(change if change in {"steady", "joint_zero"} else "sweep",
host.profile.command.baseline_values, 1., task_key=host.profile.motion.tasks[0].key,
command_index=0, cycle=0, direction="decreasing") if change != "start" else None
host._steady_capture_after_ns = host.ports.clock_ns()
@@ -11,6 +11,8 @@ from linkerhand_calibration.profiles import load_bundled_hand_profile
from linkerhand_calibration.runtime.engine import CalibrationEngine
from linkerhand_calibration.runtime.steady import steady_targets
from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution
from linkerhand_calibration.core.domain.reference import build_joint_zero_reference
from test_joint_zero_runtime import reference_rows
def independent_steady_fixture(layout):
@@ -32,8 +34,14 @@ def independent_steady_fixture(layout):
{"space": "relative_rotation_3d", "reference_quaternion_xyzw": [0, 0, 0, 1],
"axis_xyz": [0, 0, 1], "input_knots": knots}, 0., 0., {})
feedback = JointMapping(name, channel, f"feedback_{native}", knots, angles, angles, angles)
zero_rows = reference_rows(profile, name)
for row in zero_rows:
state = list(row[f"state_{native}"])
state[channel] = .8*baseline+offset
row[f"state_{native}"] = state
reference = build_joint_zero_reference(profile, name, zero_rows, session_epoch=1, motion_version=1)
fit = CalibrationResult({name: curve}, {name: feedback}, {name: 0.}, {}, {}, {}, None,
reference_inputs={name: .8*baseline+offset})
reference_inputs={name: .8*baseline+offset}, joint_zero_references={name: reference})
rows = []
for unit in CalibrationEngine(profile).scan_units():
if unit.task_key != task.key:
@@ -0,0 +1,218 @@
"""Per-joint references use the production executor without ROS or hardware."""
from dataclasses import replace
import numpy as np
import pytest
from linkerhand_calibration.profiles import load_bundled_hand_profile
from linkerhand_calibration.core.domain.reference import build_joint_zero_reference, read_joint_zero_references
from linkerhand_calibration.runtime.execution import SessionExecution
from linkerhand_calibration.runtime.session import CalibrationPhase as Phase
from linkerhand_calibration.runtime.scan_quality import observation_streams
from linkerhand_calibration.runtime.joint_resume import JointResume
@pytest.mark.parametrize("invalid", ["disabled_approach", "missing_approach", "wrong_baseline"])
def test_zero_pose_contract_rejects_invalid_motion_before_hardware(invalid):
from linkerhand_calibration.core.domain.profile import validate_profile
profile = load_bundled_hand_profile("g20_right_19")
name, spec = next(iter(profile.motion.joint_zero_references.items()))
if invalid == "disabled_approach":
pose = list(spec.approach_commands[0])
channel = min(profile.command.disabled_indices)
pose[channel] = 0 if pose[channel] else 255
changed = replace(spec, approach_commands=(tuple(pose),))
reason = "moves a disabled channel"
elif invalid == "missing_approach":
changed = replace(spec, approach_commands=())
reason = "explicit nonzero final arrival"
else:
channel = next(t.command_index for t in profile.motion.tasks if t.key == spec.task_key)
pose = list(spec.command)
pose[channel] = 0 if pose[channel] else 255
changed = replace(spec, command=tuple(pose))
reason = "joint baseline, not task start"
profile = replace(profile, motion=replace(profile.motion,
joint_zero_references={**profile.motion.joint_zero_references, name: changed}))
with pytest.raises(ValueError, match=reason):
validate_profile(profile)
def reference_rows(profile, name, *, stamp=1, tilt=0):
from scipy.spatial.transform import Rotation
spec = profile.motion.joint_zero_references[name]
channel = next(t.command_index for t in profile.motion.tasks if t.key == spec.task_key)
direction = "increasing" if spec.command[channel] > spec.approach_commands[-1][channel] else "decreasing"
views = {profile.measurement.measurements[name].view}
secondary = profile.measurement.cross_view_sources.get(name)
if secondary:
views.add(profile.measurement.measurements[secondary].view)
pose = {"quaternion_xyzw": [0, 0, 0, 1],
"translation_xyz_m": [0, 0, .5]}
child = {"quaternion_xyzw": Rotation.from_rotvec([tilt, 0, 0]).as_quat().tolist(), "translation_xyz_m": [0, 0, .6]}
return [{"view": view, "image_stamp_ns": stamp+i,
"relative_quaternion_xyzw": child["quaternion_xyzw"], "relative_translation_xyz_m": [0, 0, .1],
f"command_vector_{profile.command.unit}": spec.command, f"state_{profile.command.unit}": spec.command,
"command_direction_by_index": [direction]*profile.command.command_count,
"parent_pose_common": pose, "child_pose_common": child}
for view in views for i in range(profile.acquisition.fixed_reference_minimum_frames)]
@pytest.mark.parametrize("layout", ["o6_right_8", "l6_right_8", "o12_right_16", "g20_right_19"])
def test_deferred_zero_is_locked_once_before_own_training_in_every_model(layout):
profile = load_bundled_hand_profile(layout)
driver = SessionExecution(profile)
driver.session.device_ready()
driver.session.start()
command, rows, references, stamp = profile.command.baseline_values, [], {}, 100
trace = []
for _ in range(6000):
phase = driver.session.phase
if phase == Phase.FIT:
break
if phase == Phase.REFERENCE_LOCKING:
required = driver.session.status().task.required_tag_ids_by_view
assert sum(len(ids) for ids in required.values()) == len(profile.vision.views)
driver.session.reference_locked()
assert driver.session.phase == Phase.PREPARE # No all-finger installation sweep.
continue
if phase == Phase.EVALUATE:
assert driver.evaluate(rows).passed
continue
motion = driver.motion(command)
command = motion.target
trace.append((motion.phase, motion.task_key, motion.cycle))
if motion.zero_joints:
for name in motion.zero_joints:
assert name not in references
spec = profile.motion.joint_zero_references[name]
assert command == spec.command
assert command[motion.command_index] == profile.command.baseline_values[motion.command_index]
reference = build_joint_zero_reference(profile, name, reference_rows(profile, name),
session_epoch=1, motion_version=len(trace))
references[name] = reference
driver.zero_references[name] = reference
elif motion.recording:
task = next(t for t in profile.motion.tasks if t.key == motion.task_key)
assert set(task.joints) <= references.keys()
unit = driver.action.scan_unit
for field, name, spec in observation_streams(profile, task):
for i in range(129 if motion.phase == "sweep" else 3):
stamp += 1
rows.append({field: name, "view": spec.view, "task_name": task.key,
"cycle": unit.cycle, "direction": unit.direction, "attempt": 1,
"image_stamp_ns": stamp, "sample_phase": motion.phase,
"steady_index": motion.steady_index, "steady_target": command[task.command_index],
f"feedback_{profile.command.unit}": unit.start+(unit.end-unit.start)*i/128})
driver.motion_complete()
assert driver.session.phase == Phase.FIT
assert references == driver.zero_references
assert set(references) == set(profile.motion.joint_zero_references)
if layout == "g20_right_19":
index_zero = trace.index(("joint_zero", "index_roll_multiview", 0))
assert ("sweep", "middle_pip_side", 3) in trace[:index_zero]
reference = references["index_mcp_roll"]
assert reference.baseline_command == 127 # Task starts at 255.
assert any(np.any(np.asarray(s.command) != profile.command.baseline_values) for s in reference.samples)
def test_resume_verifies_same_pose_and_preserves_original_zero():
profile = load_bundled_hand_profile("o6_right_8")
name = next(iter(profile.motion.joint_zero_references))
old = build_joint_zero_reference(profile, name, reference_rows(profile, name), session_epoch=1, motion_version=2)
current = build_joint_zero_reference(profile, name, reference_rows(profile, name, stamp=100), session_epoch=2, motion_version=4)
resume = JointResume(profile)
resume.references[name] = old
assert resume.verify(current) is old
slipped = build_joint_zero_reference(profile, name, reference_rows(profile, name, stamp=100, tilt=.1),
session_epoch=2, motion_version=5)
with pytest.raises(ValueError, match="joint_resume_reference_changed"):
resume.verify(slipped)
with pytest.raises(ValueError, match="duplicate_or_unknown"):
read_joint_zero_references(profile, [old.as_record(), old.as_record()])
with pytest.raises(ValueError, match="not_before_training"):
read_joint_zero_references(profile, [old.as_record(), {"kind": "joint_sample", "joint": name, "image_stamp_ns": 1}])
tampered = old.as_record()
tampered["samples"][0]["sample_id"] = "different_image:1"
with pytest.raises(ValueError, match="joint_zero_image_identity_invalid"):
read_joint_zero_references(profile, [tampered])
def test_same_joint_in_later_task_cannot_acquire_a_second_reference():
profile = load_bundled_hand_profile("o6_right_8")
first = profile.motion.tasks[0]
repeated = replace(first, key="repeat_same_joint")
profile = replace(profile, motion=replace(profile.motion, tasks=profile.motion.tasks+(repeated,)))
driver = SessionExecution(profile)
for name in first.joints:
driver.zero_references[name] = build_joint_zero_reference(profile, name, reference_rows(profile, name),
session_epoch=1, motion_version=1)
before = dict(driver.zero_references)
driver.session._unit_index = next(i for i, u in enumerate(driver.session.engine.scan_units()) if u.task_key == repeated.key)
driver.session.phase = Phase.PREPARE
assert driver.motion(profile.command.baseline_values).phase != "joint_zero"
assert driver.zero_references == before
def test_native_resume_imports_training_travel_only_after_reference_verification(tmp_path):
from runtime_host_fixture import coordinator_fixture, ready
from linkerhand_calibration.runtime.steady import steady_targets
host, clock = coordinator_fixture(tmp_path, model="o12")
try:
profile = host.profile
task = profile.motion.tasks[0]
units = [u for u in host.execution.session.engine.scan_units() if u.task_key == task.key and u.cycle == 0]
records, stamp = [], 100
for name in task.joints:
reference = build_joint_zero_reference(profile, name, reference_rows(profile, name), session_epoch=1, motion_version=1)
records.append(reference.as_record())
for unit in units:
for field, name, spec in observation_streams(profile, task):
for phase, targets in (("sweep", np.linspace(unit.start, unit.end, 129)),
("steady", steady_targets(profile, unit))):
for index, target in enumerate(targets):
for _ in range(1 if phase == "sweep" else 3):
stamp += 1
records.append({"kind": "joint_sample" if field == "joint" else "secondary_joint_sample",
field: name, "view": spec.view, "task_name": task.key, "cycle": 0,
"direction": unit.direction, "attempt": 1, "image_stamp_ns": stamp,
"sample_phase": phase, "steady_index": index, "steady_target": target,
"feedback_rad": float(target)})
keys = {(u.task_key, u.cycle, u.direction) for u in units}
host.joint_resume.stage(host.execution.session.engine, records, keys)
assert host.joint_resume.take_task(task.key) == ((), ())
for name in task.joints:
current = build_joint_zero_reference(profile, name, reference_rows(profile, name, stamp=2000),
session_epoch=2, motion_version=3)
host.execution.zero_references[name] = host.joint_resume.verify(current)
ready(host, clock)
assert host.start().success
clock.now += .1
host.receive_feedback(profile.command.names, profile.command.baseline_values, host.ports.clock_ns())
host.last_command = profile.command.baseline_values
host.execution.session.phase = Phase.PREPARE
host.tick()
assert host._resumed_count == 2 and host.execution.session.current_unit.cycle == 1
assert host.execution.first_cycle_spans
assert all(value == pytest.approx(1) for value in host.execution.first_cycle_spans.values())
finally:
host.close()
def test_zero_capture_recovers_after_transient_bad_pose_without_loosening_stability(tmp_path):
from types import SimpleNamespace
from runtime_host_fixture import coordinator_fixture
host, _clock = coordinator_fixture(tmp_path, model="o6")
try:
name = next(iter(host.profile.motion.joint_zero_references))
bad = reference_rows(host.profile, name, tilt=.1)[:1]
stable = reference_rows(host.profile, name, stamp=100)
host._steady_rows = [dict(row, kind="joint_zero_sample", joint=name) for row in bad+stable]
host._observation_epoch = host._segment_number = 1
assert host._lock_joint_zeros(SimpleNamespace(zero_joints=(name,)))
reference = host.execution.zero_references[name]
assert all(sample.image_stamp_ns >= 100 for sample in reference.samples)
assert len(host._steady_rows) == len(bad)+len(stable) # Evidence was not erased.
assert host.execution.session.phase != Phase.PAUSED
finally:
host.close()
@@ -0,0 +1,127 @@
"""Production v3 files replay independent synthetic nonlinear passive motion."""
import hashlib
import json
import numpy as np
import pytest
from dataclasses import replace
from copy import deepcopy
import shutil
from measured_capture_fixture import measured_capture
from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session
from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper
@pytest.fixture(scope="module", params=[False, True], ids=["original_layout", "virtual_reordered"])
def measured_release(tmp_path_factory, request):
tmp_path = tmp_path_factory.mktemp("measured_release")
profile, source, records, offsets = measured_capture(tmp_path, reordered=request.param)
hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields}
hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest()
payload, fit, artifact = finalize_profile_session(profile=profile, session_dir=tmp_path/"session",
serial_number="SYNTHETIC_ONLY", source_urdf=source, protected_inputs=hashes,
records=records, standard_loader=lambda path: None)
return profile, source, records, hashes, payload, fit, artifact
def test_nonlinear_full_pair_passes_while_standard_mimic_loss_is_reported(measured_release):
profile, source, records, hashes, payload, fit, artifact = measured_release
assert payload["schema_version"] == 3
acceptance = json.loads(artifact.staged_release.validation_json)
assert acceptance["independent_mapping_holdout"]
assert max(v["maximum_deg"] for v in acceptance["urdf_mimic_approximation"].values()) > 3
assert max(v["maximum_deg"] for v in acceptance["full_json_urdf_holdout"].values()) < 1
reader = load_unified_mapper(artifact.path.parent/"release_manifest.json")
assert np.max(np.abs(reader.map_positions(profile.command.baseline_values))) < 1e-10
from types import SimpleNamespace
from linkerhand_calibration.runtime.artifacts.completion import finish_online_artifacts
completed = finish_online_artifacts(SimpleNamespace(calibration_contract=SimpleNamespace(typed_profile=profile),
session_root=artifact.path.parent.parent), artifact.path.parent,
{"outputs": {"json": str(artifact.staged_release.calibration_json), "urdf": str(artifact.path)}})
assert completed["URDF"] == str(artifact.path)
from linkerhand_calibration.runtime.adapters.legacy_byte_sdk import LegacyByteSdkAdapter
from linkerhand_calibration.runtime.adapters.base import HardwareHealth
from linkerhand_calibration.core.fitting.motion_fit import channel_for_joint
writes = []
sdk = LegacyByteSdkAdapter(profile.command, publish=writes.append,
set_speed_callback=lambda *_: None, health_callback=lambda: HardwareHealth(True, True))
values = [float(31+7*i) for i in range(profile.command.command_count)]
for index in profile.command.disabled_indices:
values[index] = profile.command.baseline_values[index]
sdk.publish_position(values)
assert writes[-1] == tuple(values)
angles = dict(zip(reader.urdf_joint_names, reader.map_positions(values)))
for name, row in payload["joints"].items():
assert row["sdk_channel"] == channel_for_joint(profile, name)
assert angles[name] == pytest.approx(row["angle_rad"][int(values[row["sdk_channel"]])], abs=1e-12)
@pytest.mark.parametrize("field", ["calibration_json", "corrected_urdf", "calibration_report"])
def test_v3_modified_artifact_is_rejected(measured_release, tmp_path, field):
*_, artifact = measured_release
shutil.copytree(artifact.path.parent, tmp_path/"copy")
manifest = tmp_path/"copy/release_manifest.json"
target = manifest.parent/json.loads(manifest.read_text())[field]
target.write_text(target.read_text()+"\n")
with pytest.raises(ValueError, match="SHA256 changed"):
load_unified_mapper(manifest)
def test_missing_geometry_or_transferred_curve_cannot_be_published(measured_release):
from linkerhand_calibration.core.urdf.result_plan import prepare_standard_result
profile, source, records, hashes, payload, fit, artifact = measured_release
offsets = dict(fit.zero_offsets_rad)
offsets.pop(next(iter(profile.zero.passive_joints)))
with pytest.raises(ValueError, match="absolute_geometry"):
prepare_standard_result(profile, source, hashes["source_urdf_sha256"], replace(fit, zero_offsets_rad=offsets))
mappings = dict(fit.output_mappings)
name = next(iter(mappings))
mappings[name] = replace(mappings[name], transferred_from="different_physical_joint")
with pytest.raises(ValueError, match="transferred"):
prepare_standard_result(profile, source, hashes["source_urdf_sha256"], replace(fit, output_mappings=mappings))
def test_v3_native_reader_uses_all_passive_curves_and_rejects_legacy_promotion(measured_release):
from linkerhand_calibration.runtime.artifacts.reader import UnifiedCommandMapper
from linkerhand_calibration.runtime.artifacts.serializers.measured_v3 import from_report
profile, source, records, hashes, payload, fit, artifact = measured_release
report = json.loads((artifact.path.parent/"calibration_report.json").read_text())
native = deepcopy(report)
native["command_unit"] = "rad"
native["baseline_command"] = [v/255 for v in native["baseline_command"]]
for row in native["joints"].values():
row["zero_reference"]["baseline_command"] /= 255
for kind in ("command", "feedback"):
data = row[kind+"_to_rad"]
data.update(input_domain=kind+"_rad", input_unit="rad",
knots=[v/255 for v in data["knots"]], valid_input_range=[v/255 for v in data["valid_input_range"]])
compact = from_report(native)
byte_reader = UnifiedCommandMapper(payload, artifact.path, report=report)
rad_reader = UnifiedCommandMapper(compact, artifact.path, report=native)
command = [100.]*profile.command.command_count
np.testing.assert_allclose(byte_reader.map_positions(command), rad_reader.map_positions([v/255 for v in command]), atol=1e-12)
wrong = deepcopy(report)
name = next(iter(wrong["joints"]))
wrong["joints"][name]["zero_reference"]["channel"] = 999
with pytest.raises(ValueError, match="zero_evidence"):
from_report(wrong)
wrong["format"] = "unified_calibration_v1"
with pytest.raises(ValueError, match="legacy promotion"):
from_report(wrong)
def test_final_table_cannot_hide_fourth_round_zero_shift(measured_release):
from scipy.spatial.transform import Rotation
from linkerhand_calibration.core.urdf.measured_acceptance import validate_measured_joint_holdout
from linkerhand_calibration.runtime.acquisition import accepted_joint_records
profile, source, records, hashes, payload, fit, artifact = measured_release
steady = accepted_joint_records(profile, records, sample_phase="steady")
heldout = [dict(r) for rows in steady.values() for r in rows if r["cycle"] == 3]
name = next(iter(fit.curves))
for row in heldout:
if row["joint"] == name:
extra = Rotation.from_rotvec(np.asarray(fit.curves[name].circle["axis_xyz"])*np.deg2rad(5))
row["relative_quaternion_xyzw"] = (Rotation.from_quat(row["relative_quaternion_xyzw"])*extra).as_quat().tolist()
with pytest.raises(ValueError, match="inaccurate"):
validate_measured_joint_holdout(payload, fit.curves, heldout)
@@ -0,0 +1,47 @@
"""Frozen zeros and nonlinear passive motion must survive independent holdout."""
import math
import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from linkerhand_calibration.core.fitting.anchored_mapping import anchored_mean
from linkerhand_calibration.core.fitting.rotation_curve import RotationObservation, fit_rotation_curve
from linkerhand_calibration.core.fitting.coupling import fit_standard_mimic
def observations(*, slip=0.0):
mount = Rotation.from_euler("xyz", [0.3, -.8, .4])
return [RotationObservation(f"{cycle}:{direction}:{i}", float(x),
tuple((mount * Rotation.from_rotvec([0, 0, .5*x+.3*x*x+(slip if cycle == 3 else 0)])).as_quat()),
cycle, direction)
for cycle in range(4) for direction in ("increasing", "decreasing")
for i, x in enumerate(np.linspace(0, 1, 65))], mount
def test_frozen_oblique_zero_preserves_nonlinear_motion_and_detects_holdout_slip():
rows, mount = observations()
fit = fit_rotation_curve(rows[:390], input_domain="feedback_rad", sdk_to_joint_direction=1,
reference_xyzw=mount.as_quat())
assert fit.measure(mount.as_quat())[0] == pytest.approx(0, abs=1e-12)
assert max(fit.holdout_errors(rows[390:])) < 1e-10
slipped, _ = observations(slip=math.radians(5))
assert min(fit.holdout_errors(slipped[390:])) > math.radians(4.9)
x = [r.input_value for r in rows[:390]]
y = [fit.measure(r.quaternion_xyzw)[0] for r in rows[:390]]
linear = fit_standard_mimic("parent", "child", x, y,
cycles=[r.cycle for r in rows[:390]], offset_rad=0)
assert linear.offset_rad == 0
assert max(abs(e) for e in linear.residuals_rad) > math.radians(3)
def test_baseline_anchor_is_a_constraint_not_branch_recentering():
increasing = [-.39, .005, .41]
decreasing = [-.41, -.001, .39]
result = anchored_mean([0, 1, 2], increasing, decreasing, 1, 1)
assert result == pytest.approx([-.4, 0, .4])
assert increasing[1] == .005 and decreasing[1] == -.001
with pytest.raises(ValueError, match="observed_training_knot"):
anchored_mean([0, 2], [-.4, .4], [-.4, .4], 1, 1)
with pytest.raises(ValueError, match="inconsistent"):
anchored_mean([0, 1, 2], [-.4, .1, .4], [-.4, .1, .4], 1, 1)
@@ -103,3 +103,27 @@ def test_known_unverified_projection_cannot_publish_even_if_fit_passes(monkeypat
pipeline.finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf='unused',
protected_inputs={},records=[{'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','image_stamp_ns':1}])
assert not (tmp_path/'latest_passed').exists()
def test_frozen_joint_zero_is_never_reselected_from_training_or_holdout(monkeypatch):
rows = build_data(monkeypatch)
zero = dict(next(r for r in rows if 'joint' in r), kind='joint_zero_sample',
sample_phase='joint_zero', image_stamp_ns=10000, direction=None)
result, report = module.resolve_thumb_observations([zero]+rows)
assert result[0] == zero
assert report['training_frames'] == 72 and report['holdout_frames'] == 24
def test_only_declared_fixed_roles_may_use_a_cached_pose(monkeypatch):
from linkerhand_calibration.core.geometry.candidate_selection import resolve_chain_observations
rows = build_data(monkeypatch)
role = module.THUMB_ROLES[0]
for row in rows:
if 'roles' in row:
row['roles'][role].update(locked_reference=True, corners_xy=None)
kwargs = dict(task_key='thumb_mcp_dip_front',
role_pairs={'thumb_mcp': module.THUMB_ROLES[:2], 'thumb_dip': module.THUMB_ROLES[1:]},
solve_pose=module.solve_square_tag_ippe)
with pytest.raises(ValueError, match='cached fixed reference'):
resolve_chain_observations(rows, **kwargs)
assert resolve_chain_observations(rows, fixed_roles=(role,), **kwargs)[1]['status'] == 'resolved'
@@ -39,7 +39,7 @@ def frame(profile, stamp, *, omit=(), shift=0, feedback=True):
corners = {role: projected_square(sizes[role], xyz)+[shift, 0] for role, xyz in roles.items() if role not in omit}
values = tuple(map(float, profile.command.baseline_values))
return CaptureFrame("front", stamp, np.array([[1200, 0, 640], [0, 1200, 480], [0, 0, 1]]),
corners, values if feedback else None, values)
corners, values if feedback else None, values, command_directions=("increasing",)*len(values))
def test_only_ten_unique_post_start_valid_frames_lock_and_mounts_keep_per_tag_size():
@@ -90,3 +90,43 @@ def test_fixed_reference_displacement_requires_ten_confirming_images(drift):
for i in range(9):
assert capture.consume(frame(profile, 20+i, shift=drift))[1] is None
assert capture.consume(frame(profile, 29, shift=drift))[1] is not None
def test_joint_zero_only_requires_its_own_parent_child_tags():
from linkerhand_calibration.core.domain.reference import build_joint_zero_reference
profile, lock, capture = make_capture()
lock.begin_session()
lock.start_locking()
for i in range(10):
capture.consume(frame(profile, i+1))
name = "rh_thumb_cmc_pitch"
spec = profile.motion.joint_zero_references[name]
motion = MotionCommand("joint_zero", spec.command, 1, task_key=spec.task_key,
command_index=0, cycle=0, zero_joints=(name,))
rows = []
for i in range(10):
samples, movement = capture.consume(frame(profile, 20+i, omit=("thumb_dip",)), motion)
assert movement is None
rows.extend(samples)
assert len(rows) == 10 and all(row["kind"] == "joint_zero_sample" for row in rows)
reference = build_joint_zero_reference(profile, name, rows, session_epoch=1, motion_version=2)
assert reference.joint == name and len(reference.samples) == 10
def test_articulated_capture_discards_partial_moving_tags_and_records_cached_base():
profile, lock, capture = make_capture()
profile = replace(profile, measurement=replace(profile.measurement,
candidate_selection_tasks=frozenset({"thumb_pitch_dip_front"})))
capture.profile = profile
lock.begin_session()
lock.start_locking()
for i in range(10):
capture.consume(frame(profile, i+1))
motion = MotionCommand("sweep", profile.command.baseline_values, 1,
"thumb_pitch_dip_front", 0, 0, "decreasing")
assert capture.consume(frame(profile, 20, omit=("thumb_dip",)), motion)[0] == ()
samples, _ = capture.consume(frame(profile, 21, omit=("front_base",)), motion)
candidate = next(r for r in samples if r["kind"] == "pnp_candidate_frame")
assert candidate["roles"]["front_base"]["locked_reference"] is True
assert candidate["roles"]["front_base"]["corners_xy"] is None
assert candidate["roles"]["front_base"]["selected"]
@@ -29,9 +29,16 @@ def rigid(rpy, xyz):
return matrix
def virtual_capture(tmp_path, layout):
def virtual_capture(tmp_path, layout, *, include_baseline=False, profile_transform=None):
base_layout = "l6_right_8" if layout == "virtual_reordered" else layout
profile = load_bundled_hand_profile(base_layout)
# This fixture preserves the legacy v2 acceptance contract. New measured
# behavior is exercised separately using include_baseline and real v3 refs.
profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2))
if include_baseline:
profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=3))
if profile_transform is not None:
profile = profile_transform(profile)
if layout == "virtual_reordered":
profile = replace(profile, key=ProfileKey("VIRTUAL", "right", layout),
namespace="/virtual_calibration",
@@ -99,6 +106,9 @@ def virtual_capture(tmp_path, layout):
for direction in ("increasing", "decreasing"):
codes = (np.linspace(0, 255, 81).round().astype(int) if profile.command.unit == "u8"
else np.linspace(min(task.start_value, task.end_value), max(task.start_value, task.end_value), 81))
if include_baseline:
from linkerhand_calibration.core.domain.sampling import command_nodes
codes = np.unique(np.r_[codes, command_nodes(profile, task), command_nodes(profile, task, holdout=True)])
if direction == "decreasing":
codes = codes[::-1]
for index, code in enumerate(codes):
@@ -78,7 +78,7 @@ artifacts:
corrected_urdf_filename: corrected.urdf
protected_input_fields: [source_urdf_sha256]
publish_corrected_urdf: true
acquisition: {policy_version: unified_engine_v4_dual_mapping}
acquisition: {policy_version: unified_engine_v5_joint_zero}
urdf:
authorized_fields:
finger_joint: [origin.rpy, limit.lower, limit.upper]
@@ -99,7 +99,9 @@ def test_resume_skips_only_complete_units_and_still_prepares_current_task() -> N
session.baseline_complete()
session.reference_locked()
session.resume_checked(
ResumeDecision(True, "verified", completed_units=completed)
ResumeDecision(True, "fixed_reference_verified; pending_joint_zero")
)
assert session.current_unit.task_key == first_task
session.authorize_task_reuse(completed)
assert session.phase == CalibrationPhase.PREPARE
assert session.current_unit.task_key != first_task
@@ -25,6 +25,8 @@ def test_reference_waypoint_cannot_drive_disabled_channel():
def test_reference_waypoints_share_executor_and_return_before_resume(tmp_path):
profile = load_bundled_hand_profile("l6_right_8")
# Legacy v2 sessions retain their all-at-start verification waypoint API.
profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2))
pose = (230., 255., 255., 255., 255., 255.)
waypoint = ReferenceWaypoint("show_tip", pose, {"front": (2,)})
profile = replace(profile, motion=replace(profile.motion, resume_verification_waypoints=(waypoint,)))
@@ -13,6 +13,8 @@ from linkerhand_calibration.runtime.scan_quality import observation_streams
@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "o12_right_16", "g20_right_19"])
def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout):
profile = load_bundled_hand_profile(layout)
from dataclasses import replace
profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=2))
driver = SessionExecution(profile)
driver.session.device_ready()
driver.session.start()