From 69c2da6808b82326171b99d1d1e249582bc3f23c Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Fri, 11 Sep 2026 14:43:44 +0800 Subject: [PATCH] =?UTF-8?q?=E6=A0=87=E5=AE=9A=E6=94=B9=E9=80=A0=E6=96=B9?= =?UTF-8?q?=E6=A1=88=EF=BC=9A=E6=8C=89=E5=85=B3=E8=8A=82=E9=94=81=E5=AE=9A?= =?UTF-8?q?=E9=9B=B6=E4=BD=8D=EF=BC=8C=E8=BE=93=E5=87=BA=E5=AE=9E=E6=B5=8B?= =?UTF-8?q?=20JSON=20=E4=B8=8E=E8=BF=91=E4=BC=BC=20URDF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CALIBRATION_V3_IMPLEMENTATION.md | 122 ++++++++++ src/linkerhand_calibration/README.md | 79 ++++--- .../config/g20_right_product.yaml | 2 +- .../config/l6_right_product.yaml | 2 +- .../config/o12_right_product.yaml | 2 +- .../config/o6_right_product.yaml | 2 +- .../config/profiles/g20_right_19.yaml | 110 ++++++++- .../config/profiles/l6_right_8.yaml | 30 ++- .../config/profiles/o12_right_16.yaml | 85 ++++++- .../config/profiles/o6_right_8.yaml | 30 ++- .../calibrated_joint_state_bridge.py | 2 +- .../models/l6/left_transfer.py | 4 +- .../models/o6/left_transfer.py | 4 +- .../core/domain/__init__.py | 7 + .../core/domain/profile.py | 48 +++- .../core/domain/reference.py | 173 ++++++++++++++ .../core/domain/result.py | 2 + .../core/domain/sampling.py | 17 ++ .../core/fitting/anchored_mapping.py | 22 ++ .../core/fitting/command_mapping.py | 31 ++- .../core/fitting/mimic_approximation.py | 44 ++++ .../core/fitting/motion_fit.py | 21 +- .../core/fitting/observed_motion.py | 5 +- .../core/fitting/rotation_curve.py | 3 +- .../core/fitting/session.py | 37 ++- .../spatial_solver/geometry_helpers.py | 1 + .../core/fitting/spatial_solver/residuals.py | 6 + .../core/fitting/spatial_solver/types.py | 1 + .../core/fitting/spatial_solver/validation.py | 2 + .../core/geometry/candidate_selection.py | 23 +- .../core/urdf/acceptance.py | 30 ++- .../core/urdf/kinematics.py | 4 +- .../core/urdf/measured_acceptance.py | 34 +++ .../linkerhand_calibration/core/urdf/plan.py | 5 + .../core/urdf/result_plan.py | 29 +++ .../core/urdf/tag_acceptance.py | 12 +- .../linkerhand_calibration/profiles/loader.py | 12 +- .../profiles/observations.py | 10 +- .../profiles/validator.py | 28 ++- .../runtime/artifacts/completion.py | 5 +- .../runtime/artifacts/finalization.py | 38 ++- .../runtime/artifacts/publisher.py | 33 ++- .../runtime/artifacts/reader.py | 17 +- .../artifacts/serializers/measured_v3.py | 79 +++++++ .../linkerhand_calibration/runtime/capture.py | 20 +- .../runtime/coordinator.py | 73 +++++- .../runtime/execution.py | 26 ++- .../runtime/joint_resume.py | 78 +++++++ .../runtime/motion_execution.py | 15 +- .../runtime/reporting/progress_zh.py | 2 + .../runtime/reporting/reasons_zh.py | 9 + .../linkerhand_calibration/runtime/runner.py | 4 + .../linkerhand_calibration/runtime/session.py | 16 +- .../runtime/snapshot.py | 15 +- .../linkerhand_calibration/runtime/status.py | 1 + .../linkerhand_calibration/runtime/steady.py | 10 +- .../runtime/trajectory.py | 10 + src/linkerhand_calibration/setup.py | 2 +- .../test/measured_capture_fixture.py | 106 +++++++++ .../test/test_calibration_coordinator.py | 4 +- .../test/test_dual_mapping.py | 10 +- .../test/test_joint_zero_runtime.py | 218 ++++++++++++++++++ .../test/test_measured_finalization.py | 127 ++++++++++ .../test/test_measured_zero_mapping.py | 47 ++++ .../test/test_o12_observation_resolution.py | 24 ++ .../test/test_observation_capture.py | 42 +++- .../test/test_profile_finalization.py | 12 +- .../test/test_profile_loader.py | 2 +- .../test/test_progress_status.py | 4 +- .../test/test_reference_waypoints.py | 2 + .../test/test_session_execution.py | 2 + 71 files changed, 1986 insertions(+), 148 deletions(-) create mode 100644 src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/fitting/anchored_mapping.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py create mode 100644 src/linkerhand_calibration/test/measured_capture_fixture.py create mode 100644 src/linkerhand_calibration/test/test_joint_zero_runtime.py create mode 100644 src/linkerhand_calibration/test/test_measured_finalization.py create mode 100644 src/linkerhand_calibration/test/test_measured_zero_mapping.py diff --git a/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md b/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md new file mode 100644 index 0000000..6bc9ffa --- /dev/null +++ b/src/linkerhand_calibration/CALIBRATION_V3_IMPLEMENTATION.md @@ -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 +``` diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index 996d3a1..d85f553 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -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)。相机图像目前采用主机取帧时间； diff --git a/src/linkerhand_calibration/config/g20_right_product.yaml b/src/linkerhand_calibration/config/g20_right_product.yaml index c1f98bb..ddc30ad 100644 --- a/src/linkerhand_calibration/config/g20_right_product.yaml +++ b/src/linkerhand_calibration/config/g20_right_product.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/l6_right_product.yaml b/src/linkerhand_calibration/config/l6_right_product.yaml index 3ecfddc..a41ee19 100644 --- a/src/linkerhand_calibration/config/l6_right_product.yaml +++ b/src/linkerhand_calibration/config/l6_right_product.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/o12_right_product.yaml b/src/linkerhand_calibration/config/o12_right_product.yaml index 63c2d16..d4911e9 100644 --- a/src/linkerhand_calibration/config/o12_right_product.yaml +++ b/src/linkerhand_calibration/config/o12_right_product.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/o6_right_product.yaml b/src/linkerhand_calibration/config/o6_right_product.yaml index c9edf88..2cd95cf 100644 --- a/src/linkerhand_calibration/config/o6_right_product.yaml +++ b/src/linkerhand_calibration/config/o6_right_product.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/profiles/g20_right_19.yaml b/src/linkerhand_calibration/config/profiles/g20_right_19.yaml index 84eef1d..a20b414 100644 --- a/src/linkerhand_calibration/config/profiles/g20_right_19.yaml +++ b/src/linkerhand_calibration/config/profiles/g20_right_19.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/profiles/l6_right_8.yaml b/src/linkerhand_calibration/config/profiles/l6_right_8.yaml index 675a626..ecdaf46 100644 --- a/src/linkerhand_calibration/config/profiles/l6_right_8.yaml +++ b/src/linkerhand_calibration/config/profiles/l6_right_8.yaml @@ -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 diff --git a/src/linkerhand_calibration/config/profiles/o12_right_16.yaml b/src/linkerhand_calibration/config/profiles/o12_right_16.yaml index 5cf99b8..18b772a 100644 --- a/src/linkerhand_calibration/config/profiles/o12_right_16.yaml +++ b/src/linkerhand_calibration/config/profiles/o12_right_16.yaml @@ -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: diff --git a/src/linkerhand_calibration/config/profiles/o6_right_8.yaml b/src/linkerhand_calibration/config/profiles/o6_right_8.yaml index 778ee50..380b1c0 100644 --- a/src/linkerhand_calibration/config/profiles/o6_right_8.yaml +++ b/src/linkerhand_calibration/config/profiles/o6_right_8.yaml @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py index 6ecb9a1..fdb26dd 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py index a6ab2ab..0449c6c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py @@ -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" ), diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py index d41a4a8..5d921b2 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py @@ -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" ), diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py index cfd381e..8eb504a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py @@ -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", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index 6fa6244..6df822e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py new file mode 100644 index 0000000..3b111b6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/reference.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py index d49772f..55c2956 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py new file mode 100644 index 0000000..a936ff1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sampling.py @@ -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))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/anchored_mapping.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/anchored_mapping.py new file mode 100644 index 0000000..30afd8c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/anchored_mapping.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py index 88c8000..c982f3b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py new file mode 100644 index 0000000..7102745 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/mimic_approximation.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py index 77213e8..32f4c00 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py index 0e25ddf..f2957a5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py index 621a2e3..1f850cf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py @@ -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): diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py index 9d4c50d..2ce4d28 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py @@ -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}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py index da35c05..9443510 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py @@ -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}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py index 66f34f5..f10ec47 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py @@ -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 = ( diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py index 140da8c..25e3207 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py index 42b0bcb..4363cb2 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py @@ -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( diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py index 7b01aec..2766cf8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py @@ -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]: diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py index 01be3ee..10cb4ca 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py @@ -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}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py index 208d26e..e7c65ad 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py @@ -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))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py new file mode 100644 index 0000000..27bf27a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/measured_acceptance.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py index 441fc13..7aefc5b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py @@ -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}" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py index 4a652d8..fa1cf6b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py @@ -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)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py index a5d6d1f..3dc72af 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py @@ -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, diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py index 80d13e5..5bdc7bc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py @@ -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", ())), diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py index 0670d71..cccb03e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py index 5e02ef6..e1fb39f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py @@ -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)}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py index b5b33ad..0ad61b8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py @@ -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") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py index f24af80..e2c196e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py index 8622d47..4bdf6a8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py @@ -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: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py index 298e756..6e1bf92 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py new file mode 100644 index 0000000..3741543 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/measured_v3.py @@ -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} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py index f980ee2..7a38a0c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py @@ -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=""): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py index 3152f4b..def89ae 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -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) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py index 7f29f0b..e4be97d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py @@ -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"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py new file mode 100644 index 0000000..df38b2d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/joint_resume.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py index 9b1975a..1e971ed 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -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"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py index 2c0513c..0a69021 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py @@ -20,6 +20,8 @@ STATE_LABELS = { "ABORTED": "已中止", } PHASE_LABELS = { + "joint_zero": "采集并锁定当前关节零位", + "zero_approach": "准备关节零位观测姿态", "baseline": "安全恢复基准形态", "preflight": "任务运动预检", "prepare": "扫描起点准备", diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py index c1bd9ed..da2023f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -11,6 +11,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 项查表。", "核对扫描范围和端点稳态数据；不得用反馈表、外推或裁剪补齐缺失指令。") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index fde6b92..b7cee7b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -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" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py index 46f3e96..c643c29 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py @@ -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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py index 1ed083f..fd4814d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py @@ -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"), diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py index 0d99792..0d292a6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py @@ -25,6 +25,7 @@ _PHASES = { "baseline": "安全恢复基准", "preflight": "固定通道映射点动", "prepare": "扫描起点准备", "retry_prepare": "同速重扫起点准备", "sweep": "正式扫描", "steady_prepare": "稳态指令采样起点准备", "steady": "稳态指令→rad 采样", + "joint_zero": "采集并冻结关节零位", "zero_approach": "关节零位到达准备", } diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py index 9407f94..aa3abbb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py @@ -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)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py index d83aa0c..da503f9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py @@ -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) diff --git a/src/linkerhand_calibration/setup.py b/src/linkerhand_calibration/setup.py index aa41fa0..6bf6905 100644 --- a/src/linkerhand_calibration/setup.py +++ b/src/linkerhand_calibration/setup.py @@ -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"], diff --git a/src/linkerhand_calibration/test/measured_capture_fixture.py b/src/linkerhand_calibration/test/measured_capture_fixture.py new file mode 100644 index 0000000..9cdca2e --- /dev/null +++ b/src/linkerhand_calibration/test/measured_capture_fixture.py @@ -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 diff --git a/src/linkerhand_calibration/test/test_calibration_coordinator.py b/src/linkerhand_calibration/test/test_calibration_coordinator.py index c135cde..3ba935f 100644 --- a/src/linkerhand_calibration/test/test_calibration_coordinator.py +++ b/src/linkerhand_calibration/test/test_calibration_coordinator.py @@ -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() diff --git a/src/linkerhand_calibration/test/test_dual_mapping.py b/src/linkerhand_calibration/test/test_dual_mapping.py index 495835a..0b63b5d 100644 --- a/src/linkerhand_calibration/test/test_dual_mapping.py +++ b/src/linkerhand_calibration/test/test_dual_mapping.py @@ -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: diff --git a/src/linkerhand_calibration/test/test_joint_zero_runtime.py b/src/linkerhand_calibration/test/test_joint_zero_runtime.py new file mode 100644 index 0000000..c93c7e3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_joint_zero_runtime.py @@ -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() diff --git a/src/linkerhand_calibration/test/test_measured_finalization.py b/src/linkerhand_calibration/test/test_measured_finalization.py new file mode 100644 index 0000000..74ec9b3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_measured_finalization.py @@ -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) diff --git a/src/linkerhand_calibration/test/test_measured_zero_mapping.py b/src/linkerhand_calibration/test/test_measured_zero_mapping.py new file mode 100644 index 0000000..72ecad4 --- /dev/null +++ b/src/linkerhand_calibration/test/test_measured_zero_mapping.py @@ -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) diff --git a/src/linkerhand_calibration/test/test_o12_observation_resolution.py b/src/linkerhand_calibration/test/test_o12_observation_resolution.py index e3e0785..b999eb6 100644 --- a/src/linkerhand_calibration/test/test_o12_observation_resolution.py +++ b/src/linkerhand_calibration/test/test_o12_observation_resolution.py @@ -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' diff --git a/src/linkerhand_calibration/test/test_observation_capture.py b/src/linkerhand_calibration/test/test_observation_capture.py index 4aa4c39..ed1c8a7 100644 --- a/src/linkerhand_calibration/test/test_observation_capture.py +++ b/src/linkerhand_calibration/test/test_observation_capture.py @@ -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"] diff --git a/src/linkerhand_calibration/test/test_profile_finalization.py b/src/linkerhand_calibration/test/test_profile_finalization.py index 684df26..311ebf1 100644 --- a/src/linkerhand_calibration/test/test_profile_finalization.py +++ b/src/linkerhand_calibration/test/test_profile_finalization.py @@ -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): diff --git a/src/linkerhand_calibration/test/test_profile_loader.py b/src/linkerhand_calibration/test/test_profile_loader.py index f3c943f..7a5951e 100644 --- a/src/linkerhand_calibration/test/test_profile_loader.py +++ b/src/linkerhand_calibration/test/test_profile_loader.py @@ -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] diff --git a/src/linkerhand_calibration/test/test_progress_status.py b/src/linkerhand_calibration/test/test_progress_status.py index 6f5ffc4..c9b750c 100644 --- a/src/linkerhand_calibration/test/test_progress_status.py +++ b/src/linkerhand_calibration/test/test_progress_status.py @@ -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 diff --git a/src/linkerhand_calibration/test/test_reference_waypoints.py b/src/linkerhand_calibration/test/test_reference_waypoints.py index 2d21a58..48307a2 100644 --- a/src/linkerhand_calibration/test/test_reference_waypoints.py +++ b/src/linkerhand_calibration/test/test_reference_waypoints.py @@ -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,))) diff --git a/src/linkerhand_calibration/test/test_session_execution.py b/src/linkerhand_calibration/test/test_session_execution.py index 4691521..945c6d5 100644 --- a/src/linkerhand_calibration/test/test_session_execution.py +++ b/src/linkerhand_calibration/test/test_session_execution.py @@ -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()