标定改造方案:按关节锁定零位,输出实测 JSON 与近似 URDF
This commit is contained in:
@@ -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
|
||||
```
|
||||
@@ -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)
|
||||
|
||||
+3
-1
@@ -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"
|
||||
),
|
||||
|
||||
+3
-1
@@ -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}")
|
||||
|
||||
+1
@@ -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}")
|
||||
|
||||
+6
@@ -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:
|
||||
|
||||
+2
@@ -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(
|
||||
|
||||
+14
-9
@@ -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")
|
||||
|
||||
+31
-7
@@ -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)
|
||||
|
||||
+79
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user