From c4ad2b968a776f2ba036f886df3f5ddd1ac0ae3a Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Wed, 9 Sep 2026 15:05:09 +0800 Subject: [PATCH] =?UTF-8?q?O12=E9=87=8D=E6=9E=84=E4=B8=80=E7=89=88?= =?UTF-8?q?=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gui_control/config/constants.py | 9 +- src/gui_control/test/test_l6_config.py | 11 + .../CALIBRATION_FLOW.md | 238 ++ src/linkerhand_calibration/README.md | 176 +- .../config/g20_right_product.yaml | 2 +- .../config/l6_right_product.yaml | 2 +- .../config/l6_three_camera_calibration.yaml | 6 +- .../config/o12_right_product.yaml | 8 +- .../config/o12_three_camera_calibration.yaml | 22 +- .../config/o6_right_product.yaml | 2 +- .../config/o6_three_camera_calibration.yaml | 6 +- .../config/three_camera_calibration.yaml | 19 +- .../linkerhand_calibration/core/__init__.py | 2 + .../core/domain/__init__.py | 2 + .../core/domain/profile.py | 83 +- .../core/geometry/camera.py | 18 + .../core/geometry/rotation.py | 22 + .../core/urdf/__init__.py | 2 + .../linkerhand_calibration/core/urdf/patch.py | 95 + .../models/g20/_adapter.py | 1 - .../linkerhand_calibration/models/g20/node.py | 248 +- .../models/g20/profile.py | 3 +- .../models/g20/runner.py | 7 +- .../models/g20/zero_solver.py | 127 +- .../models/l6/fitting.py | 39 +- .../linkerhand_calibration/models/l6/node.py | 623 +++-- .../models/l6/pipeline.py | 10 + .../models/l6/profile.py | 5 +- .../models/l6/runner.py | 37 +- .../models/o12/artifacts.py | 360 ++- .../models/o12/fitting.py | 567 ++++- .../models/o12/health.py | 133 ++ .../models/o12/kinematics.py | 93 + .../models/o12/motion.py | 57 + .../linkerhand_calibration/models/o12/node.py | 1529 +++++++++++- .../models/o12/observations.py | 139 ++ .../models/o12/pipeline.py | 199 +- .../linkerhand_calibration/models/o12/pnp.py | 76 + .../models/o12/profile.py | 239 +- .../models/o12/quality.py | 115 + .../models/o12/resume.py | 384 +++ .../models/o12/runner.py | 394 +++- .../linkerhand_calibration/models/o12/urdf.py | 257 +- .../linkerhand_calibration/models/o12/zero.py | 180 ++ .../models/o6/fitting.py | 5 - .../models/o6/pipeline.py | 10 + .../models/o6/profile.py | 5 +- .../linkerhand_calibration/models/registry.py | 11 +- .../runtime/__init__.py | 23 +- .../runtime/adapters/__init__.py | 4 + .../runtime/adapters/base.py | 108 + .../linkerhand_calibration/runtime/engine.py | 322 +++ .../linkerhand_calibration/runtime/runner.py | 11 + .../linkerhand_calibration/trajectory.py | 11 +- .../linkerhand_calibration/urdf_comparison.py | 175 ++ src/linkerhand_calibration/setup.py | 8 +- .../test/test_config.py | 10 +- .../test/test_g20_right_product.py | 8 + .../test/test_l6_right_profile.py | 9 +- .../test/test_o12_axis_residual_policy.py | 60 + .../test/test_o12_full_hand_zero.py | 342 +++ .../test/test_o12_observation_resolution.py | 117 + .../test/test_o12_recorded_replay.py | 49 + .../test/test_o12_right_profile.py | 2097 ++++++++++++++++- .../test/test_o12_static_geometry.py | 156 ++ .../test/test_o12_thumb_pnp.py | 124 + .../test/test_o6_right_profile.py | 2 +- .../test/test_rectified_camera_contract.py | 43 + .../test/test_three_camera_retry.py | 109 +- .../test/test_unified_engine.py | 326 +++ .../test/test_urdf_comparison.py | 59 + .../test/test_urdf_patch_engine.py | 35 + ...ibrated_O12_RIGHT_001_20260908_134319.urdf | 1234 ++++++++++ ...ibrated_O12_RIGHT_001_20260908_141642.urdf | 1234 ++++++++++ ...ibrated_O12_RIGHT_001_20260908_170656.urdf | 1234 ++++++++++ ...ibrated_O12_RIGHT_001_20260908_174116.urdf | 1234 ++++++++++ ...ibrated_O12_RIGHT_001_20260908_181314.urdf | 1234 ++++++++++ ...ibrated_O12_RIGHT_001_20260908_183332.urdf | 1234 ++++++++++ ...ibrated_O12_RIGHT_001_20260909_144123.urdf | 1234 ++++++++++ ...RIGHT_001_REVIEW_ONLY_20260908_195044.urdf | 1234 ++++++++++ ...RIGHT_001_REVIEW_ONLY_20260909_105543.urdf | 1234 ++++++++++ ...RIGHT_001_REVIEW_ONLY_20260909_112041.urdf | 1234 ++++++++++ 82 files changed, 22476 insertions(+), 650 deletions(-) create mode 100644 src/linkerhand_calibration/CALIBRATION_FLOW.md create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/geometry/camera.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/resume.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/urdf_comparison.py create mode 100644 src/linkerhand_calibration/test/test_o12_axis_residual_policy.py create mode 100644 src/linkerhand_calibration/test/test_o12_full_hand_zero.py create mode 100644 src/linkerhand_calibration/test/test_o12_observation_resolution.py create mode 100644 src/linkerhand_calibration/test/test_o12_recorded_replay.py create mode 100644 src/linkerhand_calibration/test/test_o12_static_geometry.py create mode 100644 src/linkerhand_calibration/test/test_o12_thumb_pnp.py create mode 100644 src/linkerhand_calibration/test/test_rectified_camera_contract.py create mode 100644 src/linkerhand_calibration/test/test_unified_engine.py create mode 100644 src/linkerhand_calibration/test/test_urdf_comparison.py create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_134319.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_141642.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_170656.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_174116.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_181314.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_183332.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260909_144123.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260908_195044.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_105543.urdf create mode 100644 src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_112041.urdf diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py index ece3355..2dcfef1 100644 --- a/src/gui_control/gui_control/config/constants.py +++ b/src/gui_control/gui_control/config/constants.py @@ -261,7 +261,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = { "叁": [0, 39, 255, 255, 255, 0], "肆": [0, 0, 255, 255, 255, 255], "伍": [255, 255, 255, 255, 255, 255], - "OK": [74, 13, 153, 255, 255, 255], + "OK": [62, 5, 151, 255, 255, 255], "点赞": [255, 255, 0, 0, 0, 0], "握拳": [79, 11, 0, 0, 0, 0], "序列动作1": [250, 250, 250, 250, 250, 250], @@ -277,7 +277,12 @@ _HAND_CONFIGS: Dict[str, HandConfig] = { "拇指压感准备1": [139, 18, 130, 0, 0, 0], "拇指压感测试": [39, 30, 122, 250, 250, 250], "拇指压感准备2": [139, 18, 130, 0, 0, 0] - } + }, + preset_action_overrides={ + "right": { + "OK": [58, 6, 153, 255, 255, 255], + }, + }, ), "O12": HandConfig( # O12 active-angle API order. Keep this independent from the diff --git a/src/gui_control/test/test_l6_config.py b/src/gui_control/test/test_l6_config.py index dd526f9..5452ef4 100644 --- a/src/gui_control/test/test_l6_config.py +++ b/src/gui_control/test/test_l6_config.py @@ -10,3 +10,14 @@ def test_l6_gui_uses_the_sdk_channel_order() -> None: "ring_mcp_pitch", "pinky_mcp_pitch", ] + + +def test_l6_ok_preset_is_hand_specific() -> None: + config = HAND_CONFIGS["L6"] + + assert config.get_preset_actions("left")["OK"] == [ + 62, 5, 151, 255, 255, 255, + ] + assert config.get_preset_actions("right")["OK"] == [ + 58, 6, 153, 255, 255, 255, + ] diff --git a/src/linkerhand_calibration/CALIBRATION_FLOW.md b/src/linkerhand_calibration/CALIBRATION_FLOW.md new file mode 100644 index 0000000..274b4f4 --- /dev/null +++ b/src/linkerhand_calibration/CALIBRATION_FLOW.md @@ -0,0 +1,238 @@ +# LinkerHand 整体标定流程与 URDF 修正原理 + +本文描述 `linkerhand_calibration` 当前的多型号统一架构。具体的通道数、运动单位、 +Tag 布局、任务、零位策略和输出格式由产品 Profile 决定,通用流程不再假定某一种手型。 + +## 1. 整体流程 + +```mermaid +flowchart TD + A[加载产品配置与 Profile] + A --> B[设备、视觉与安全预检] + B --> C[按任务执行三轮训练和一轮留出采集] + C --> D[拟合动态曲线、零位、行程和被动耦合] + D --> E{质量与留出验证通过?} + E -->|数据不足且未重扫| C + E -->|不可恢复| X[保持/暂停,不发布] + E -->|通过| P[形成最终标定结果] + P --> J[生成标定 JSON] + P --> U[从原始 CAD 生成修正后的 URDF] +``` + +对应的通用状态可以概括为: + +```text +配置/Profile → 预检 → 运动与视觉采集 → 参数拟合 → 质量验证 + ├─→ 标定 JSON + └─→ 修正后的 URDF +``` + +图中只展示业务流程和最终产物。实现内部,G20/L6/O6 会额外生成并回读 +`*_urdf_correction_input.json`,用于审计和校验 URDF 修正参数;O12 直接使用已验证的 +拟合结果。该交接细节不改变最终交付物仍是标定 JSON 和修正 URDF。 + +## 2. Profile 负责什么 + +统一入口 `calibrate_hand --config <产品配置>` 先根据产品配置选择 +`MODEL/side/layout/vREVISION` Profile。Profile 是硬件运动和数据解释的边界,声明: + +- SDK 命令顺序、单位(u8 或 rad)、反馈映射和安全 baseline; +- 相机视角、Tag ID、父子连杆角色和外参要求; +- 每项运动任务、起止点、辅助避障姿态和速度; +- 哪些关节实测、迁移、保留 CAD,哪些关节是主动或被动; +- 静态零位、机械端点和 mimic/非线性耦合的求解策略; +- 训练轮、独立留出轮、质量门限、输出 schema 和发布指针。 + +因此通用层只执行“Profile 声明的任务”,不会自行猜测关节顺序、单位、Tag 数量或 +左右手镜像关系。 + +## 3. 标定原理 + +### 3.1 数据采集 + +每项任务只驱动一个目标通道,其余通道保持 baseline 或进入 Profile 规定的避障姿态。 +G20、L6、O6 不再执行逐任务全行程运动预检;O12 只对每个主动通道执行一次不超过 +3° 的映射点动。正式采集同步保存: + +- 请求命令和真实反馈; +- 父、子 AprilTag 的相对旋转与相对平移; +- 相机时间戳、内外参身份、重投影误差及全手状态; +- 扫描方向、轮次、任务和重试编号。 + +请求命令和真实反馈是两个不同的数据域,必须由对应型号的 schema 明确标识,不能混用。 + +### 3.2 视觉几何 + +关节运动由父、子刚性件上 Tag 的相对 SE(3) 轨迹得到。相对观测可以消除相机在世界中的 +绝对位置和固定 Tag 安装变换对动态角度的影响。 + +- 转轴方向由整段相对旋转轨迹拟合; +- 轴线上一点可由刚体旋转关系 `(I-R)p=t` 稳健拟合; +- 接近沿轴观察时,弱可观的单目深度分量会被降级为诊断或投影到图像平面; +- 多视角结果先转换到公共坐标系,再按 Profile 规定用于主测量、交叉检查或零位约束。 + +### 3.3 三类标定结果 + +一次会话通常同时求三类参数: + +1. 动态曲线:控制量/反馈量到关节角的单调映射,并保留或检查正反方向回差。 +2. 静态零位与行程:实物基准姿态相对 CAD 关节坐标系的偏差,以及实测安全范围。 +3. 被动耦合:主动关节与被动关节之间的线性或二次关系。 + +对启用 `isolated_holdout` 的当前产品 Profile,训练轮用于拟合,独立留出轮不参与最终参数 +重拟合,只检验泛化误差。短时 Tag/PnP/同步丢失只丢弃无效帧,运动继续完成。一个方向 +只有在有效同步样本少于 40、行程覆盖不足、少于 32 个分箱或连续盲区超过全行程 1/16 +时,才同速重扫一次。检测率与反馈频率低于理想值只作诊断,只要有效数据足够就不停机。 +拟合或 holdout 失败会拒绝发布,但不自动反复运动。 + +实时运动仅在人工中止/重复控制器、SDK 活动故障或失联、控制模式错误、物理越限、 +明确要求运动后连续两秒无推进,以及固定基准连续 10 帧漂移超过 5 px 时停止。非目标轴 +小幅运动、正常跟随滞后、机构固有耦合和辅助避让轴误差不是停机条件。 + +## 4. URDF 修正原理 + +### 4.1 基本原则 + +修正始终从经过哈希确认的原始 CAD URDF 生成,不在上一份标定 URDF 上叠加,也不覆盖 +源文件。型号适配层只生成声明式 Patch,公共 patch engine 负责字段级修改、禁止覆盖、 +mesh 安全复制和原子写入。 + +允许修改哪些字段由 Profile 和型号 writer 决定,可能包括: + +| 参数 | 作用 | +|---|---| +| `origin.rpy` | 写入可观测且被授权的主动关节静态零偏 | +| `limit.lower/upper` | 把实测行程或端点转换到修正后的关节坐标系 | +| `mimic.multiplier/offset` | 为普通 URDF 使用者提供线性被动联动 | +| MuJoCo equality `polycoef` | 表示 Profile 授权的非线性被动耦合 | + +`origin.xyz`、关节轴、mesh、惯量、连杆长度和拓扑默认保持 CAD;只有型号 Profile 明确 +授权的字段才能变化。 + +### 4.2 静态零位 + +若型号允许修正某主动关节的静态零位,源关节变换为 `T_cad`、源关节轴为 `a`、 +零偏为 `δ`,则: + +```text +T_corrected = T_cad × Rot(a, δ) +``` + +实现上将结果重新表达为 `origin.rpy`。动态曲线描述的是相对该新零位的运动量,所以运行时 +不能再把 `δ` 加到曲线输出中。 + +并非所有型号都修改 origin:如果视觉无法把固定 Tag 安装角与绝对零位可靠分离,Profile +会保留 CAD origin,只发布动态曲线和实测行程。 + +### 4.3 机械端点和限位 + +零位和限位必须作为同一个坐标变换问题处理。Profile 会为不同机构选择经过确认的锚点策略, +例如 `lower_at_start`、`upper_at_end` 或 `cad_range_center`,而不是统一假定命令 0/255 +一定对应某个 CAD 端点。 + +若某物理上限由 CAD 确认,零位移动 `δ` 后,坐标限位也要反向移动,保证: + +```text +静态零偏 + 修正后的坐标端点 = 原 CAD 物理端点 +``` + +其他型号则直接把实测安全行程写成新的 `[lower, upper]`。无论采用哪种策略,发布前都会 +检查运行曲线和被动耦合不越过修正后的 URDF 限位。 + +### 4.4 被动关节和非线性耦合 + +普通 URDF 的 `` 只能表达: + +```text +q_passive = offset + multiplier × q_active +``` + +若实测传动比随行程变化,拟合器可使用二次模型: + +```text +q_passive = a0 + a1·q_active + a2·q_active² +``` + +此时标准 URDF 中保留端点对齐的线性 mimic,保证 RViz 等普通消费者可以合理联动;精确的 +中间行程由运行时标定 JSON/桥接节点提供,支持的型号还会把二次系数写入 MuJoCo equality。 + +## 5. 当前型号差异 + +| 产品 Profile | 单位/范围 | 标定范围 | URDF 修正重点 | 结果 | +|---|---|---|---|---| +| `G20/right/g20_right_19/v1` | 20 路 u8 | 19 Tag、完整右手;主动零位和主动/被动动态曲线 | 主动 origin;部分端点 limit;等价 mimic offset | schema v4,`latest_passed` | +| `L6/right/l6_right_8/v1` | 6 路 u8 | 3 项局部实测,其余三指按已确认同机构迁移 | 主动 origin/行程;线性 mimic;MuJoCo 二次 equality | schema v6,`latest_partial_passed` | +| `O6/right/o6_right_8/v1` | 6 路 u8 | 3 项局部实测,其余三指迁移 | 主动 origin/行程;被动限位;端点线性 mimic | schema v6,`latest_partial_passed` | +| `O12/right/o12_right_16/v1` | 12 路连续 SDK rad | 16 Tag;11 路主动曲线和空间零位;无名指复用小指修正 | 共享 G20 轴方向/相邻轴线相位求解;无名指保留自身平移/范围/mimic | schema v7,`latest_passed` | + +O12 静态零位与行程分开处理:SDK 扫到最大不证明该姿态等于原始 CAD 上限, +禁止使用 `CAD.upper - measured_travel` 推算零位。roll/yaw 使用公共 G20 几何求解器, +由 roll/yaw/pitch 运动轴与小指根部定向轴求解。生产发布使用 +`o12_full_hand_spatial_v3_mount_invariant_phase`:拇指 pitch/MCP、食指/中指 MCP/PIP、小指 MCP +增加相邻轴线位置的相位约束,食指/中指侧摆使用下游 MCP 轴方向。 +三轮训练模型冻结后仅用第四轮验证,不用 SDK/CAD 端点相减生成零位。 +全部 11 个实测主动零位通过后才允许发布,失败时输出 +`spatial_zero_diagnostics.json`,保留逐轴残差和逐关节失败原因,不退回 CAD 后报 PASS。 +O12 显式分离旋转轴线方程的轴向零空间残差;横向残差仍参与几何质量验证。 +侧面相位计算同样先去除轴线点的轴向自由分量,再做相机平面投影;否则重新贴 Tag +造成的轴线参考点改变会被误认为关节零位。该选项由 O12 Profile 显式启用,其他型号 +已验证的默认相位策略本次不变,不表示已完成全型号实测安装不变性验收。 +O12 的 `PHASE_PARENT` 图声明平行机械轴;方向约束覆盖主动 MCP/PIP 以及被动轴。 +不能把独立单目姿态拟合造成的轴向偏差当作机构真实不平行,再通过静态零位补偿。 +通过和失败结果均记录原始/轴向/横向残差,不把不能约束轴线的分量混入拟合。 +动态拟合完成且空间解可用但未通过验证时,自动导出 `review_only/` 下的复核 URDF; +文件名及 manifest 标记 `REVIEW_ONLY`,不输出控制 JSON、不更新发布指针,错误仍向上返回。 +O12 允许保留统计上接近零的修正:训练置信区间必须包含零且半宽不超过 1°, +冻结的零值仍必须通过几何、轮次一致性和独立 holdout 验证。 +这不是跳过未观测关节;该策略显式启用,G20/L6/O6 默认决策不变。 +历史 schema v7 和低层旋转拟合测试仍可能标注 `source_cad_zero_not_measured`, +它们不等于全手空间零位已通过。轴线零位验证也不等于独立指尖接触精度验证; +被动静态零位仍不独立估计,标准 URDF 的线性 mimic 仍是非线性 SDK 联动的近似。 + +无名指迁移的是小指零位的标量修正,按 `R_ring_CAD * Rot(axis_ring, delta_pinky)` +叠加到自身坐标系;不能复制小指的 xyz,也不能将无名指零位遗漏为零。 +反馈曲线在无名指自身限位以内直接复用,到限位才截断,不对整条曲线重新缩放。 +发布同时核对 JSON 静态偏置与实际 URDF origin,轴角 holdout 和曲线通过仅代表 +对应测量通过;被动 SDK 多项式与普通 URDF 的线性 mimic 仍属于不同近似模型。 + +此外仍注册了 G20 左/右 `legacy_11` 兼容 Profile,以及 L6/O6 从右手正式结果生成左手 +迁移产物的 Profile。它们用于兼容或明确的左右手迁移,不代表新增一套通用测量假设。 + +## 6. 发布与产物 + +通过会话通常包含: + +```text +raw_samples.jsonl 原始、可审计采样 +*_calibration.json 运行时曲线和质量信息 +*_urdf_correction_input.json 部分型号的 URDF 参数交接文件 +*_calibrated*.urdf 修正 URDF +meshes/ 会话内可解析的模型资源 +calibration_summary_zh.json 会话范围、迁移来源、质量和哈希摘要 +``` + +发布前会复核源文件身份、输出 schema、URDF 授权字段、曲线限位、被动关节策略、mesh 和 +产物哈希。只有全部通过才更新 `latest_passed` 或 `latest_partial_passed`; +`latest_attempt` 仅表示最近一次尝试,不能作为生产结果。 + +## 7. 代码边界 + +- `core/`:无 ROS、无具体型号的领域契约、几何、拟合接口和 URDF patch engine。 +- `runtime/engine.py`:`unified_engine_v1` 扫描单元、一次同速重扫和统一数据门。 +- `runtime/adapters/`:`SdkAdapter` 契约以及命令/反馈域解析。 +- `models//`:声明式 Profile、SDK I/O 薄封装和兼容旧 schema 的序列化插件。 +- `compat/`:旧配置、旧布局和兼容入口。 +- `config/*_product.yaml`:实物身份、相机、输入文件和哈希。 + +`unified_engine_v1` 不读取旧策略断点;迁移后的第一次运行必须完整重新采集。后续同版本 +断点仍按 Profile、序列号和所有受保护输入哈希校验。 + +全新标定前允许移动底座、重贴 Tag;开始后底座及 Tag 相对连杆安装必须固定。 +断点恢复逻辑和默认行为不变,默认恢复期间安装未变,不增加确认参数。 +G20/O12 安装改变后使用已有 `--no-resume` 开始新采集;L6/O6 产品入口行为不变。 +哈希验证不等于检测物理安装变化。 + +`compare_calibration_urdfs` 对相同 URDF 关节角执行只读 FK 对比,记录连杆原点、方向 +和主动范围。它不加载 SDK、不运行硬件、不参与零位拟合、不改变发布门,也不是 +实机接触精度认证。参考模型不是必须复现的固定参数;同数据回放一致性与独立重采 +精度重复性必须分别验收。 diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index a6950f6..38f175f 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -1,5 +1,47 @@ # LinkerHand 专业标定包 +## 全型号安装与重复标定约定 + +O12 的 `thumb_mcp_dip_front` 使用 ID1/ID2/ID3 联合 IPPE 候选选择,不再逐 Tag +独立决定分支。复用通用组跟踪器,以实测平行转轴方向辅助消歧;不指定 Tag 安装角, +不将 CAD mimic 比例或 SDK 被动公式替换为视觉测量。方向证据不足时退回视觉连续性, +不新增实时停机条件;最终几何质量验证保持不变。其他型号的选解策略及断点逻辑不变。 +在线选择仅作暂定观测。最终求解用前三轮完整轨迹重新比较拇指候选分支,第四轮 +不参与分支评分;结果写入 `pose_selection_diagnostics.json`。不会把关节轨迹强行 +投影成无残差的刚性运动,也不会降低原几何门限。 + +三相机检测来自 `image_rect`,PnP 必须使用 `CameraInfo.P[:3,:3]`,不能使用原始 +`K` 并把畸变设为零。L6/O6/O12 共用取参路径已修正,G20 原本即使用 P。 +新会话保存 `rectified_camera_model`(含原 K/D/R/P);O12 各任务保存角点、候选 +和选解记录 `o12_pnp_candidate_frame`,锁定的基准明确标注 `locked_reference`, +不伪造像素。拇指还保留初始化未选出位姿的帧。 + +旧帧若未记录正确投影来源,最终拟合不会把旧 K 静默当成 P;仍走旧观测路径并 +在诊断中标注 `legacy_projection_unverified`。只保存最终位姿的任务无法可靠重算; +外部相机文件仅可显式用于局部诊断,其混合观测不允许生成整手正式产物。 +当前真实数据修正内参、重选拇指候选后仍有 MCP 残差未通过;不能承诺已解决实机精度。 + +标定前允许移动机械手底座、重新安装 Tag;ID 与所属刚性连杆必须正确,Tag 尺寸和 +平面观测条件必须满足配置。开始后底座固定,Tag 相对各自连杆固定,关节仍正常运动。 +相机相互位置及内参不变时,移动手不要求重标相机外参。 + +断点恢复保持原有默认行为,前提是底座、相机和 Tag 安装未变,不增加确认参数。 +G20/O12 在移动底座或重贴 Tag 后开始全新标定时,使用已有 `--no-resume` 选项, +不混用旧安装的采样。原有哈希/数据兼容性检查保持不变;哈希不能检测物理安装变化。 +L6/O6 产品启动入口行为不变。 + +可用只读工具比较不同结果的模型姿态: + +```bash +ros2 run linkerhand_calibration compare_calibration_urdfs \ + --reference <参考.urdf> --candidate <本次.urdf> --output <新建报告.json> +``` + +它检查相同 **URDF 关节角** 下的连杆原点位置、方向及主动行程,不修改文件、不控制 +硬件、不影响标定拟合或发布。坐标不是原始 SDK 弧度,连杆原点也不是指尖接触点; +离线探测姿态不可直接发送给实机。报告不等于精度 PASS。参考结果只作对照,不作为 +固定零位输入,也不强制新数据拟合成旧参数。 + ## O12 右手 16-Tag 完整标定 O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口,固定订阅 @@ -18,11 +60,101 @@ ros2 run linkerhand_calibration calibrate_hand --config \ runner 会自动加载仓库内 vendor Jazzy overlay,启动 HCAN device 0/channel 0 节点、三相机、AprilTag 与标定节点;READY 后自动开始。启动前会确认 12 路 -POSITION 模式、错误码、温度和反馈,并以不超过 3° 的低速点动执行固定通道预检。 -正式扫描以 20 Hz 发布端点速度为零的弧度余弦轨迹;中止、堵转或质量失败时保持 -当前位置。结果发布到 `calibration_output//latest_passed`,其中 JSON +POSITION 模式、错误码和实时反馈,并以不超过 3° 的低速点动执行固定通道预检。 +这版 O12 固件的温度查询会阻塞后返回空,因此默认不发送该无效请求,而以独立错误 +查询中的 bit1 持续提供过热保护。bit0–bit3 或未知错误位始终立即停止;SDK 明确可能 +由历史超时留下的 bit4 `commu_except`,只有在至少三次相同报告、至少 1.5 秒观察且 +命令触发反馈持续新鲜并达到最低频率后才标记为历史锁存。标定运行中出现新的 bit4 +组合,或反馈流中断超过一秒,仍会立即保持当前位置并停止。 +正式扫描以 50 Hz 发布端点速度为零的平滑限速弧度轨迹;中止、堵转或质量失败时保持 +当前位置。小指完成后,小指与无名指会同步弯至各自安全上限;进入食指标定前, +中指 MCP/PIP 也弯至各自安全上限且中指侧摆保持 0 rad。专用避让航点确认这些轴 +到位后才继续,为中指和食指留出完整空间。SDK 的 MCP→PIP 回读联动、其他非目标轴小幅运动以及避让轴 +跟随滞后全部作为采样/诊断保留,不再套用理论 vendor 包络触发停机。O12 的扫描质量 +由统一引擎按有效同步样本、实测行程、归一化分箱和连续未观测区判断;单 Tag 检测率、 +联合帧率和反馈频率低于理想目标只记告警。短时局部遮挡只丢弃无效帧,不中断运动; +数据不足时当前方向只同速重扫一次。结果发布到 +`calibration_output//latest_passed`,其中 JSON 为 schema v7 弧度 knots,不生成 256 点 u8 表。 +O12 与 G20 共用 Tag 几何拟合、3+1 holdout、URDF 写回和发布门,但输入域不同:G20 +使用 u8,O12 使用连续 SDK 弧度。O12 的 12 路 SDK 坐标包含 tendon/vendor solver +坐标,不能直接当作 19 路 URDF 关节角;必须在完整 SDK 物理范围上由 Tag 相对旋转 +拟合 SDK→URDF 曲线。源 CAD 限位是待修正输出,不能反向截断采集范围。拇指 +roll/yaw 和其余实测主动关节的静态零位复用 G20 空间求解核:非平行轴用方向, +平行弯曲轴用相邻轴线的位置相位。生产发布要求 11 个实测主动零位通过 3+1 验证, +无名指继承小指修正;失败时保存 `spatial_zero_diagnostics.json`,不退回 CAD 报 PASS。 +被动关节的 JSON 仍使用 SDK 非线性公式,标准 URDF 则保留 CAD 端点等价线性 +mimic 近似,两者不能视为任意姿态下完全相同。发布前同时验证曲线、 +静态 origin、URDF 限位和递归 mimic 链;指尖接触精度还需独立组合姿态验收。 + +O12 空间零位策略 `o12_full_hand_spatial_v3_mount_invariant_phase` 正式采用 +复核版的轴向残差分离:求解旋转轴线时只使用可观测的横向方程,同时记录原始、轴向、 +横向残差,不能把轴向误差混作轴线位置误差。每次从本次数据重新求解,不包含某一只手 +的固定修正角度;G20/L6/O6 的默认拟合策略不变。三轮训练、第四轮独立验证及横向几何 +质量门仍保留,不能仅凭模型看起来更像就宣布精度通过。 + +侧面平行关节相位先消除轴线点的轴向自由分量,再投影到图像平面,不能重新使用未经 +处理的两点位移:轴线上的“最近点”随父 Tag 安装原点改变,并不是唯一的物理轴承中心。 +此前这种混用会在略斜的侧面视角下产生安装相关的零位偏差,即使无噪声且四轮重复也 +可能错误通过。回归测试覆盖近轴向机位、移动底座、重新安装父/子 Tag 及两者同时变化。 +这些理想几何测试不替代实测质量验证;单目位姿误差仍可能使数据无法通过。 +O12 的相邻平行轴图同时约束主动和被动弯曲轴的方向,而不只约束被动 DIP;各轴仍 +使用自己的实测旋转行程和轴线位置。配对任务须保持非平行上游轴在相同姿态,不能把 +其他姿态采到的轴方向直接当作当前方向。该约束不适用于 roll/yaw 等非平行轴。 + +如果动态拟合完成、空间零位存在解但验证未通过,会在会话的 `review_only/` 下保存 +文件名带 `REVIEW_ONLY` 的 URDF 和 `review_manifest.json`,方便检查;终端仍明确报告 +失败,不生成可用于控制的标定 JSON,也不更新 `latest_passed`。缺少几何数据、求解 +不可观测或修正超出允许范围时不生成复核模型。验证通过则正常发布,无需人工替换算法。 +标准 URDF 的滑块使用拟合后的关节角,不是原始 SDK 弧度;静态零位已写入 origin, +不要再次加到滑块上。 + +重新完整采集时在原启动命令后加 `--no-resume`;仅验证算法则使用 +`--offline-raw `,不必重采。外参或 Tag 安装改变后,新旧观测不可混用。 + +O12 的命令端点与反馈端点不要求数值相等:完整命令轨迹负责驱动机械全行程,第一轮 +反馈的两个实测端点建立该方向的归一化输入域,后续轮次只需复现首轮实测行程的 90%。 +连续空洞门也只检查这段实测行程内部,不会把反馈比例或零位差形成的命令域末端区间 +误判成 Tag 遮挡。最终曲线和 URDF 端点同样使用该实测输入域,禁止向 SDK 命令端点 +进行未观测外推。 + +四指避让前会锁定无遮挡状态下的正面掌心 ID0 位姿;小指、无名指弯到上限遮住 ID0 +后,中指和食指的正面任务复用该会话固定基准,但 ID12/ID13 等运动 Tag 仍使用实时 +观测。运动 Tag 短时丢失只丢弃对应帧,恢复识别后继续采集,不会中断轨迹或误报映射 +失败;最终仍必须满足统一的有效样本与覆盖率质量门。 + +O12 的主动轴与避让轴共用同一条多轴平滑轨迹。正式扫描期间,避让轴的正常跟随滞后 +只作诊断;进入或退出避让的专用航点则要求侧摆进入 0.03 rad 到位带、每路屈曲反馈 +沿正确方向完成至少 90% 的首轮实测行程并稳定后才继续。避让命令仍发送 SDK 最大值, +但完成判断不再把反馈弧度除以命令弧度;同时屈曲时 vendor solver 的反馈端点可以小于 +单轴命令上限。反馈弧度是连续拟合输入,而 Tag 相对旋转才是目标 URDF 关节角。 +真正的物理越限、活动硬件故障、通信失联 +和要求运动的轴连续两秒无推进仍会停止。 +O12 单独出现的 SDK `commu_except` bit4 只触发连续错误查询并记录诊断;只要完整 +12 路反馈仍然新鲜且目标轴正常推进,就不会把历史/瞬时通信位误判为失联。反馈流 +超时、目标轴无推进,或 bit0--bit3 堵转/过热/过流/电机异常仍会立即安全停止。 + +O12 的 FRONT/TOP 和 FRONT/SIDE 光轴都接近正交,允许最终外参批次 RMS 不超过 +2.0 px,同时继续要求三折旋转稳定性不超过 0.3°、平移稳定性不超过 1.5 mm。 +采集候选时将单相机和候选配对上限设为 2.5 px,以便斜视棋盘进入整批联合拟合; +这两个候选上限不会替代最终的 2.0 px 批次门: + +```bash +ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \ + output_file:=$PWD/config/o12_three_camera_extrinsics.yaml \ + checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \ + maximum_reprojection_rms_px:=2.0 \ + maximum_candidate_pair_reprojection_rms_px:=2.5 \ + maximum_single_camera_reprojection_rms_px:=2.5 +``` + +完整 SDK 范围策略不会复用旧 CAD 截断数据,必须重新完整采集。runner 默认 +寻找最新同版本兼容失败会话,使用 `--no-resume` 可禁用恢复。只有通过 +质量门的连续“任务/轮次/方向”单元会被复用;启动后仍先恢复全手安全基准,并执行 +恢复点所需的映射预检和避让。序列号、profile、schema 或任一受保护输入哈希变化 +时拒绝恢复。断点恢复默认安装未变;安装改变后应使用 `--no-resume` 从头采集。 + 只验证配置、16 张 16 mm Tag、相机/外参、源 URDF 和 SDK 配置哈希而不运动: ```bash @@ -262,23 +394,23 @@ URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll 失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记; 恢复时只读取该标记之后的替代采集,不能把同一尝试编号下重扫前后的稳态点合并。 因此已经在线硬门限验收的任务保持已完成,暂停中的任务从任务开头重采,不会因 -日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量 -独立保留;单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面 -轴线位置若也能明确定位为单轮异常,同样只补扫该轮;补扫会保留任务预检和前次 -采集确定的PnP分支参考,不会因重新初始化切换到另一组平面Tag镜像解。侧面 -校验视角的任务级有效率只记录为诊断;G20右手预检若逐帧识别率低于标称值, +日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面 +主测量和侧面校验测量独立保留。侧面校验视角的任务级有效率只记录为诊断;逐帧 +识别率低于标称值, 但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞,也按完整 轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查,轴线、曲线和模型质量 -门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一 -采集事务:相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用 +门限保持不变。`unified_engine_v1`取消每任务低速全行程预检;相邻方向共享已验证 +端点和任务级PnP参考。采样不足只按原速度重扫当前方向一次,拟合或第四轮留出 +失败立即锁定发布并保持当前位置,不再通过反复运动碰门限。G20右手正式扫描固定使用 产品审定速度,不再根据单次识别密度自动提速,确保不同会话测量的是同一动态 过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查:前三轮 极差默认不得超过0.5°,95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的 -紧密簇、仅另一轮越界,自动补采该轮两个方向;无法明确定位时只重采完整yaw任务, -不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入 +紧密簇、仅另一轮越界,也只输出诊断并停止发布,不自动补采。与`latest_passed` +中上一正式结果相差超过0.75°时另写入 `thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装,但该历史差值 -不直接否决当前会话,也不会用旧结果约束新零位。需要强制从第一个关节 -重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为 +不直接否决当前会话,也不会用旧结果约束新零位。默认恢复兼容断点,前提是安装未变; +使用 `--no-resume` 可从第一个关节重新采集。 +升级前已经分别完成的正面/侧面roll也会合并为 一个完整同步任务断点;只有两边数据都完整时才复用。 命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、 @@ -327,8 +459,7 @@ ros2 launch linkerhand_calibration three_camera_calibration.launch.py \ 侧面PIP/DIP联合任务,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过 各自的观测质量门限。每个PIP任务只驱动一次对应电机,同时用“手掌→中节Tag”实测PIP、 用“中节Tag→末节Tag”实测被动DIP;四个DIP不再由URDF mimic系数生成,并参加完整视觉 -拟合和质量门限。每项正式四轮之前自动低速往返预检 -0/127/255可见性; +拟合和质量门限。任务直接执行四轮双向正式扫描; 基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的 固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0,因此四指正面+侧面同步roll中 @@ -355,23 +486,22 @@ ros2 launch linkerhand_calibration three_camera_calibration.launch.py \ 运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP” 等已评审不变量保持不变,过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。 `parallel_pose_transitions`(默认true)置false可回退旧的逐电机顺序。 -同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出, +同一任务的四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出, 不再每轮重复展开/弯曲辅助手指。跨手指组切换时,下一组避障姿态仍然需要、且 当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机 退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的 -"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺 -不超过8,只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值, -不会因本次预检帧率或识别密度而改变;旧11-Tag布局仍保留自适应速度兼容逻辑。 +"滚转先回中再展开""先滚开再弯曲"顺序保持不变。G20右手四轮正式速度始终使用 +产品配置的固定值,不会因本次帧率或识别密度而改变。 四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理 零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、 四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。 -预检、正式四轮和拟合重扫始终使用速度5。 +正式四轮使用产品审定速度;唯一一次采样重扫保持相同速度。 正式roll的每个方向会在经过127时先到位保持0.5秒,再独立保存至少10帧静止Tag/反馈; 方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再 替代静态保持姿态。 前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。 -每个任务只在低速递减预检起点执行一次8帧PnP静态初始化;预检往返和四轮正式 -扫描连续复用同一帧间分支与任务参考,不再让每一轮独立选择平面Tag解。同一任务第1轮 +每个任务在首个正式方向起点执行PnP静态初始化;四轮正式扫描连续复用同一帧间 +分支与任务参考,不再让每一轮独立选择平面Tag解。同一任务第1轮 已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的 IPPE镜像解。baseline标准接近和全部质量门限保持不变。 电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中 diff --git a/src/linkerhand_calibration/config/g20_right_product.yaml b/src/linkerhand_calibration/config/g20_right_product.yaml index 99ec4e4..936538c 100644 --- a/src/linkerhand_calibration/config/g20_right_product.yaml +++ b/src/linkerhand_calibration/config/g20_right_product.yaml @@ -26,7 +26,7 @@ artifacts: camera_extrinsics: config/g20_three_camera_extrinsics.yaml camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80 calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml - calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3 + calibration_config_sha256: afb323494140c88ab6368a061332fd80724a86831f44505dcfab147234e3c4be tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41 diff --git a/src/linkerhand_calibration/config/l6_right_product.yaml b/src/linkerhand_calibration/config/l6_right_product.yaml index 09a6e29..5229478 100644 --- a/src/linkerhand_calibration/config/l6_right_product.yaml +++ b/src/linkerhand_calibration/config/l6_right_product.yaml @@ -28,7 +28,7 @@ artifacts: camera_extrinsics: config/g20_three_camera_extrinsics.yaml camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80 calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml - calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f + calibration_config_sha256: e25e7ab27f4fd918f7cab70717e070d52a52fff17266a367c136388384ad4baa tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5 diff --git a/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml b/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml index b43f030..ca74874 100644 --- a/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml @@ -61,7 +61,7 @@ l6_calibration: motor_stall_timeout_seconds: 2.0 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 - automatic_sweep_retry_limit: 2 + automatic_sweep_retry_limit: 1 non_target_motion_tolerance_u8: 3.0 - fixed_base_maximum_corner_drift_px: 2.0 - fixed_base_movement_confirmation_frames: 5 + fixed_base_maximum_corner_drift_px: 5.0 + fixed_base_movement_confirmation_frames: 10 diff --git a/src/linkerhand_calibration/config/o12_right_product.yaml b/src/linkerhand_calibration/config/o12_right_product.yaml index 66ebbf7..0e46a23 100644 --- a/src/linkerhand_calibration/config/o12_right_product.yaml +++ b/src/linkerhand_calibration/config/o12_right_product.yaml @@ -12,7 +12,7 @@ sdk: transport: hcan setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml - config_sha256: ca1791c822bf1e99f25db8806397c3f877100c9252fcce6c53a7a6b8cef7694a + config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c cameras: front: @@ -31,10 +31,10 @@ cameras: artifacts: source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb - camera_extrinsics: config/g20_three_camera_extrinsics.yaml - camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80 + camera_extrinsics: config/o12_three_camera_extrinsics.yaml + camera_extrinsics_sha256: 5a515d0706f4e67d5e26bfddb348b519817bd72e885ea9e43997e41016176e53 calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml - calibration_config_sha256: 9defe39a3f4b31c3764cbdcd9ee5b570da70e810215844515e8cc01e56414728 + calibration_config_sha256: 962a16ce3a21fd2d7886caab17c39a9441ff4eb856c9bf8e86c5c303c2291553 tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab diff --git a/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml b/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml index 399745c..fe982ae 100644 --- a/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml @@ -9,39 +9,43 @@ o12_calibration: top_camera_info_topic: /o12_calibration/top/camera/camera_info top_detections_topic: /o12_calibration/top/apriltag/detections - # O12 standard interface is radians. Raw 0..2000 mixed control is forbidden. - command_rate_hz: 20.0 + # O12 standard interface is radians. Scan the complete reviewed SDK range; + # the source CAD limits are outputs to correct, not acquisition limits. + # Raw 0..2000 mixed control is forbidden. + command_rate_hz: 50.0 repetitions: 4 tag_size_m: 0.016 tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] minimum_detection_rate: 0.95 minimum_joint_frame_rate: 0.85 - minimum_feedback_hz: 15.0 + minimum_feedback_hz: 35.0 maximum_state_image_skew_ms: 50.0 minimum_sweep_frames: 40 minimum_state_span_fraction: 0.90 minimum_sweep_bins: 32 maximum_bin_gap: 2 endpoint_tolerance_rad: 0.01 - endpoint_hold_seconds: 1.0 + endpoint_hold_seconds: 0.5 motor_stall_timeout_seconds: 2.0 position_timeout_seconds: 60.0 sweep_timeout_seconds: 180.0 - automatic_sweep_retry_limit: 2 + automatic_sweep_retry_limit: 1 non_target_motion_tolerance_rad: 0.015 maximum_temperature_c: 70 # 当前 O12 固件返回空温度报告。仍发起查询;5秒无结果后依赖已验证的 # joint_error_states bit1 过热保护,并在会话记录中明确标注降级。 temperature_report_required: false temperature_fallback_after_seconds: 5.0 - # 现场确认初始保守速度过慢;正式/点动速度提高到2倍,避让另有硬上限。 - motion_speed_scale: 2.0 + # 快速标定档:各任务按4倍请求,但拇指pitch、侧摆、屈曲及避让均有独立硬上限。 + motion_speed_scale: 4.0 + # 正弦速度加减速时间;中段保持关节限速,避免位置余弦全程低速。 + trajectory_ramp_seconds: 0.4 maximum_hamming: 0 minimum_decision_margin: 30.0 minimum_edge_pixels: 30.0 - fixed_base_maximum_corner_drift_px: 2.0 - fixed_base_movement_confirmation_frames: 5 + fixed_base_maximum_corner_drift_px: 5.0 + fixed_base_movement_confirmation_frames: 10 pnp_maximum_reprojection_error_px: 1.5 pnp_reprojection_tie_px: 1.5 pnp_maximum_pose_jump_deg: 35.0 diff --git a/src/linkerhand_calibration/config/o6_right_product.yaml b/src/linkerhand_calibration/config/o6_right_product.yaml index 46b1284..260c422 100644 --- a/src/linkerhand_calibration/config/o6_right_product.yaml +++ b/src/linkerhand_calibration/config/o6_right_product.yaml @@ -28,7 +28,7 @@ artifacts: camera_extrinsics: config/o6_three_camera_extrinsics.yaml camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4 calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml - calibration_config_sha256: ce20d998a4342dfaacb14568513aa9af5063df48566fabd42180acc8da47e4a6 + calibration_config_sha256: a7fe124a195fa0f491535e96bdc728d25ea3efe298cf905e499917d329e9026f tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e diff --git a/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml index 85cef95..162d644 100644 --- a/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml @@ -58,7 +58,7 @@ o6_calibration: motor_stall_timeout_seconds: 2.0 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 - automatic_sweep_retry_limit: 2 + automatic_sweep_retry_limit: 1 non_target_motion_tolerance_u8: 3.0 - fixed_base_maximum_corner_drift_px: 2.0 - fixed_base_movement_confirmation_frames: 5 + fixed_base_maximum_corner_drift_px: 5.0 + fixed_base_movement_confirmation_frames: 10 diff --git a/src/linkerhand_calibration/config/three_camera_calibration.yaml b/src/linkerhand_calibration/config/three_camera_calibration.yaml index 621d7a2..8881a26 100644 --- a/src/linkerhand_calibration/config/three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/three_camera_calibration.yaml @@ -57,8 +57,8 @@ g20_calibration: pnp_group_maximum_normal_alignment_deg: 15.0 # 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见; # 任一角点相对会话基准漂移超过2 px并连续5帧时,判定标定中基准被移动。 - fixed_base_maximum_corner_drift_px: 2.0 - fixed_base_movement_confirmation_frames: 5 + fixed_base_maximum_corner_drift_px: 5.0 + fixed_base_movement_confirmation_frames: 10 # 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic # 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、 # 缩放或替代被动IP的自身Tag实测曲线。 @@ -110,13 +110,12 @@ g20_calibration: # roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。 baseline_hold_seconds: 0.5 minimum_baseline_hold_frames: 10 - # 19-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。 + # unified_engine_v1 不执行每任务全行程预检;保留参数仅兼容旧配置读取。 task_precheck_hold_seconds: 2.0 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 # 启动宽限1秒后,反馈连续2秒没有至少1个u8的进展,按机械卡滞立即暂停; - # 这类故障不进入遮挡/超时的三次自动重扫。 - # 低速5也应持续产生反馈进展;5秒无进展即停,减少机构持续顶死时间。 + # 这类硬故障不自动重试。 motor_stall_timeout_seconds: 2.0 motor_stall_startup_grace_seconds: 1.0 motor_stall_minimum_progress_u8: 1.0 @@ -126,18 +125,18 @@ g20_calibration: minimum_sweep_bins: 32 maximum_bin_gap: 16 # 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。 - automatic_sweep_retry_limit: 2 + automatic_sweep_retry_limit: 1 # 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。 - automatic_fit_retry_limit: 2 - automatic_motion_retry_limit: 2 + automatic_fit_retry_limit: 0 + automatic_motion_retry_limit: 0 # 留空为正式标定;设为pinky/ring/middle/index时只采该指正面+侧面roll, # 即使正面baseline回差失败也继续完成侧面对照,并永久锁定本会话URDF发布。 cross_view_roll_diagnostic_finger: "" # 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。 provisional_warning_ratio: 1.25 retry_minimum_speed: 3 - retry_speed_scales: [0.8, 0.6] - retry_endpoint_hold_seconds: [0.75, 1.0] + retry_speed_scales: [1.0] + retry_endpoint_hold_seconds: [0.5] trajectory_maximum_plane_rms_m: 0.004 trajectory_maximum_radial_rms_m: 0.004 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py index 5f99926..b6b43d6 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py @@ -1,6 +1,7 @@ """Hardware- and model-independent calibration kernel.""" from .domain import ( + AcquisitionPolicy, ArtifactPolicy, CalibrationProfile, CommandLayout, @@ -40,6 +41,7 @@ from .geometry import ( ) __all__ = [ + "AcquisitionPolicy", "ArtifactPolicy", "CalibrationProfile", "CommandLayout", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py index a085d23..dd75d79 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py @@ -1,6 +1,7 @@ """Calibration domain types.""" from .profile import ( + AcquisitionPolicy, ArtifactPolicy, CalibrationProfile, CommandLayout, @@ -21,6 +22,7 @@ from .profile import ( from .sample import SampleRecord __all__ = [ + "AcquisitionPolicy", "ArtifactPolicy", "CalibrationProfile", "CommandLayout", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index 42ff94c..e0d26aa 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -66,6 +66,13 @@ class CommandLayout: baseline: tuple[float, ...] = () lower_bounds: tuple[float, ...] = () upper_bounds: tuple[float, ...] = () + # Position feedback is an observation, not a command. Servo tracking, + # encoder quantisation, and the vendor's zero offset may put a valid + # observation just outside the safe command envelope. Physical-angle + # profiles can therefore register a separate feedback envelope; legacy + # profiles default to the command envelope. + feedback_lower_bounds: tuple[float, ...] = () + feedback_upper_bounds: tuple[float, ...] = () feedback_by_index: bool = False @property @@ -93,6 +100,22 @@ class CommandLayout: else (255.0,) * self.command_count ) + @property + def minimum_feedback_values(self) -> tuple[float, ...]: + return ( + tuple(float(value) for value in self.feedback_lower_bounds) + if self.feedback_lower_bounds + else self.minimum_values + ) + + @property + def maximum_feedback_values(self) -> tuple[float, ...]: + return ( + tuple(float(value) for value in self.feedback_upper_bounds) + if self.feedback_upper_bounds + else self.maximum_values + ) + def normalize(self, index: int, value: float) -> float: lower = self.minimum_values[int(index)] upper = self.maximum_values[int(index)] @@ -237,6 +260,24 @@ class QualityPolicy: isolated_holdout: bool = False +@dataclass(frozen=True) +class AcquisitionPolicy: + """Cross-model runtime safety and retained-data acceptance contract.""" + + policy_version: str = "unified_engine_v1" + mapping_probe_maximum_rad: float = 0.0 + automatic_rescan_limit: int = 1 + minimum_valid_samples: int = 40 + minimum_bins: int = 32 + maximum_unobserved_fraction: float = 1.0 / 16.0 + legacy_minimum_span_01: float = 240.0 / 255.0 + physical_first_cycle_minimum_span_01: float = 0.85 + physical_repeat_minimum_fraction: float = 0.90 + stall_timeout_seconds: float = 2.0 + fixed_reference_maximum_drift_px: float = 5.0 + fixed_reference_confirmation_frames: int = 10 + + @dataclass(frozen=True) class ScopePolicy: calibrate_joints: Mapping[str, frozenset[str]] @@ -273,6 +314,7 @@ class CalibrationProfile: quality: QualityPolicy scope: ScopePolicy artifacts: ArtifactPolicy + acquisition: AcquisitionPolicy = field(default_factory=AcquisitionPolicy) # Per-URDF-joint provenance used by partial calibration artifacts. # Known values are: measured_static_dynamic, measured_dynamic_cad_static, # transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal, @@ -312,6 +354,27 @@ def validate_profile(profile: CalibrationProfile) -> None: ) ): errors.append("command bounds or baseline values are invalid") + if ( + len(command.minimum_feedback_values) != command.command_count + or len(command.maximum_feedback_values) != command.command_count + ): + errors.append("feedback bounds must align with command names") + elif any( + not math.isfinite(feedback_lower) + or not math.isfinite(feedback_upper) + or feedback_lower >= feedback_upper + or feedback_lower > command_lower + or feedback_upper < command_upper + for feedback_lower, feedback_upper, command_lower, command_upper in zip( + command.minimum_feedback_values, + command.maximum_feedback_values, + command.minimum_values, + command.maximum_values, + ) + ): + errors.append( + "feedback bounds must be finite and contain the command domain" + ) indices = set(range(command.command_count)) if not set(command.disabled_indices).issubset(indices): errors.append("disabled command index is out of range") @@ -430,7 +493,12 @@ def validate_profile(profile: CalibrationProfile) -> None: ): errors.append("coupling model target has no source mapping") if not set(zero.coupling_model_by_joint.values()).issubset( - {"linear_mimic", "quadratic_runtime"} + { + "linear_mimic", + "quadratic_runtime", + "direction_aware_knots", + "vendor_o12_polynomial", + } ): errors.append("coupling model policy is unsupported") @@ -457,6 +525,19 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"{label} filename must not contain a directory") if not profile.namespace.startswith("/"): errors.append("runtime namespace must be absolute") + acquisition = profile.acquisition + if acquisition.policy_version != "unified_engine_v1": + errors.append("unsupported acquisition policy version") + if acquisition.automatic_rescan_limit != 1: + errors.append("unified engine requires exactly one automatic rescan") + if acquisition.minimum_valid_samples < 40 or acquisition.minimum_bins < 32: + errors.append("acquisition sample and bin minima are too small") + if not 0.0 < acquisition.maximum_unobserved_fraction <= 1.0 / 16.0: + errors.append("maximum unobserved travel must not exceed 1/16") + if command.unit == "rad" and not 0.0 < acquisition.mapping_probe_maximum_rad <= math.radians(3.0): + errors.append("physical-angle profiles require a <=3 degree mapping probe") + if command.unit == "u8" and acquisition.mapping_probe_maximum_rad != 0.0: + errors.append("legacy profiles must not request a radian mapping probe") if profile.joint_coverage: valid_coverage = { "measured_static_dynamic", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/camera.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/camera.py new file mode 100644 index 0000000..ea84795 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/camera.py @@ -0,0 +1,18 @@ +"""Projection contract for detections on image_proc's rectified images.""" +import numpy as np + + +def rectified_camera_matrix(projection): + """CameraInfo.P, not raw K, is the intrinsic matrix of image_rect. + + Never silently fall back to K: that fits a different pixel coordinate + system and can produce smooth but biased tag poses. + """ + values = np.asarray(projection, dtype=float) + if values.size != 12: + raise ValueError('rectified detections require a 3x4 CameraInfo.P') + matrix = values.reshape(3, 4)[:, :3].copy() + if (not np.all(np.isfinite(matrix)) or matrix[0, 0] <= 0 + or matrix[1, 1] <= 0 or not np.allclose(matrix[2], [0, 0, 1])): + raise ValueError('invalid rectified CameraInfo.P') + return matrix diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py index 400cdd8..4fd14a8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py @@ -152,6 +152,28 @@ def fit_rotation_axis( projections = matrix @ axis low = projections[command_values <= 16] high = projections[command_values >= 239] + # Physical-angle adapters normalize their measured feedback into the + # legacy 0..255 fitting coordinate. Tracking lag and mechanical travel + # mean those records need not reach the historical <=16 endpoint even + # when the observed physical stroke is complete. SVD axis sign is + # arbitrary, so fall back to the ends of the *observed* command domain + # instead of allowing that arbitrary sign to reverse an otherwise valid + # trajectory. Byte-feedback products that observe the canonical end + # bands retain their existing behaviour. + if not low.size or not high.size: + observed = command_values[useful] + lower = int(np.min(observed)) + upper = int(np.max(observed)) + span = upper - lower + if span <= 0: + raise ValueError("commands do not span a rotation trajectory") + band = max(1, int(math.ceil(0.1 * span))) + low = projections[ + useful & (command_values <= lower + band) + ] + high = projections[ + useful & (command_values >= upper - band) + ] if low.size and high.size and float(np.median(low)) < float(np.median(high)): axis = -axis return axis / np.linalg.norm(axis) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py index 19aabbb..46c1413 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py @@ -7,6 +7,7 @@ from .patch import ( UrdfPatchSet, apply_urdf_patch_text, materialize_relative_mesh_assets, + validate_urdf_mimic_ranges, write_urdf_patches, ) @@ -18,5 +19,6 @@ __all__ = [ "apply_urdf_patch_text", "build_correction_plan", "materialize_relative_mesh_assets", + "validate_urdf_mimic_ranges", "write_urdf_patches", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py index c505dc1..48b5e9a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py @@ -9,6 +9,7 @@ publishing a new file. from __future__ import annotations from dataclasses import dataclass, field +import math import os from pathlib import Path import re @@ -191,6 +192,99 @@ def _files_have_identical_contents(left: Path, right: Path) -> bool: return True +def _mimic_reachable_ranges( + root: ET.Element, +) -> tuple[dict[str, tuple[float, float]], dict[str, float]]: + """Resolve full mimic chains and report limit excess for each joint.""" + nodes = { + str(node.get("name")): node + for node in root.findall("joint") + if node.get("type") in {"revolute", "prismatic"} + } + resolved: dict[str, tuple[float, float]] = {} + resolving: set[str] = set() + + def resolve(name: str) -> tuple[float, float]: + if name in resolved: + return resolved[name] + if name in resolving: + raise ValueError(f"URDF mimic cycle contains joint: {name}") + node = nodes.get(name) + if node is None: + raise ValueError(f"URDF mimic source joint does not exist: {name}") + limit = node.find("limit") + if limit is None: + raise ValueError(f"bounded URDF joint has no limit: {name}") + lower = float(limit.get("lower", "nan")) + upper = float(limit.get("upper", "nan")) + if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper: + raise ValueError(f"URDF joint has invalid limits: {name}") + resolving.add(name) + mimic = node.find("mimic") + if mimic is None: + reachable = (lower, upper) + else: + source = str(mimic.get("joint", "")) + multiplier = float(mimic.get("multiplier", "1")) + offset = float(mimic.get("offset", "0")) + if not math.isfinite(multiplier) or not math.isfinite(offset): + raise ValueError(f"URDF joint has invalid mimic values: {name}") + source_range = resolve(source) + endpoints = ( + offset + multiplier * source_range[0], + offset + multiplier * source_range[1], + ) + reachable = (min(endpoints), max(endpoints)) + resolving.remove(name) + resolved[name] = reachable + return reachable + + for joint_name in nodes: + resolve(joint_name) + excess: dict[str, float] = {} + for name, reachable in resolved.items(): + node = nodes[name] + if node.find("mimic") is None: + continue + limit = node.find("limit") + assert limit is not None + lower = float(limit.get("lower", "nan")) + upper = float(limit.get("upper", "nan")) + excess[name] = max(0.0, lower - reachable[0], reachable[1] - upper) + return resolved, excess + + +def validate_urdf_mimic_ranges( + urdf: str | Path, + *, + reference_urdf: str | Path | None = None, + tolerance: float = 1.0e-9, +) -> Mapping[str, tuple[float, float]]: + """Reject new physical-limit violations anywhere in a mimic chain. + + ``reference_urdf`` permits only an already-present CAD rounding excess of + the same joint. Calibration may never enlarge that excess. + """ + root = ET.parse(Path(urdf)).getroot() + ranges, excess = _mimic_reachable_ranges(root) + reference_excess: Mapping[str, float] = {} + if reference_urdf is not None: + _, reference_excess = _mimic_reachable_ranges( + ET.parse(Path(reference_urdf)).getroot() + ) + failures = { + name: value + for name, value in excess.items() + if value > float(reference_excess.get(name, 0.0)) + float(tolerance) + } + if failures: + details = ",".join( + f"{name}={value:.9f}rad" for name, value in sorted(failures.items()) + ) + raise ValueError(f"URDF mimic reachable range exceeds limits: {details}") + return ranges + + def materialize_relative_mesh_assets( *, source: Path, output: Path, urdf_root: ET.Element ) -> tuple[Path, ...]: @@ -311,5 +405,6 @@ __all__ = [ "UrdfPatchSet", "apply_urdf_patch_text", "materialize_relative_mesh_assets", + "validate_urdf_mimic_ranges", "write_urdf_patches", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py index f75e23d..e544e89 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py @@ -33,7 +33,6 @@ _HARD_THRESHOLD_KEYS = frozenset( "maximum_reprojection_error_px", "maximum_axis_cycle_difference_rad", "maximum_pose_line_rms_m", - "maximum_hysteresis_rad", "maximum_validation_error_rad", } ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py index 2c4bae8..571888f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py @@ -40,6 +40,8 @@ from ...core import ( robust_rotation_summary, ) from ...core.urdf import build_correction_plan +from ...runtime import ACQUISITION_POLICY_VERSION, CalibrationEngine +from ...runtime.adapters import ProfileSdkAdapter from ...extrinsics import ( ThreeCameraExtrinsics, camera_info_fingerprint, @@ -2628,8 +2630,8 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("pnp_group_initialization_frames", 8) self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0) self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0) - self.declare_parameter("fixed_base_maximum_corner_drift_px", 2.0) - self.declare_parameter("fixed_base_movement_confirmation_frames", 5) + self.declare_parameter("fixed_base_maximum_corner_drift_px", 5.0) + self.declare_parameter("fixed_base_movement_confirmation_frames", 10) self.declare_parameter("thumb_ip_pnp_coupling_multiplier", 1.03) self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0) self.declare_parameter( @@ -2686,7 +2688,7 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("task_precheck_hold_seconds", 2.0) self.declare_parameter("position_timeout_seconds", 30.0) self.declare_parameter("sweep_timeout_seconds", 90.0) - self.declare_parameter("motor_stall_timeout_seconds", 5.0) + self.declare_parameter("motor_stall_timeout_seconds", 2.0) self.declare_parameter("motor_stall_startup_grace_seconds", 1.0) self.declare_parameter("motor_stall_minimum_progress_u8", 1.0) self.declare_parameter("invalid_timeout_seconds", 3.0) @@ -2694,15 +2696,15 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("minimum_state_span_u8", 240.0) self.declare_parameter("minimum_sweep_bins", 32) self.declare_parameter("maximum_bin_gap", 16) - self.declare_parameter("automatic_sweep_retry_limit", 3) - self.declare_parameter("automatic_fit_retry_limit", 2) - self.declare_parameter("automatic_motion_retry_limit", 2) + self.declare_parameter("automatic_sweep_retry_limit", 1) + self.declare_parameter("automatic_fit_retry_limit", 0) + self.declare_parameter("automatic_motion_retry_limit", 0) self.declare_parameter("cross_view_roll_diagnostic_finger", "") self.declare_parameter("provisional_warning_ratio", 1.25) self.declare_parameter("retry_minimum_speed", 3) - self.declare_parameter("retry_speed_scales", [0.8, 0.6, 0.5]) + self.declare_parameter("retry_speed_scales", [1.0]) self.declare_parameter( - "retry_endpoint_hold_seconds", [0.75, 1.0, 1.25] + "retry_endpoint_hold_seconds", [0.5] ) self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) @@ -2779,6 +2781,8 @@ class G20ThreeCameraCalibrationNode(Node): self.profile = product_contract.profile self.zero_profile = product_contract.zero_profile self.calibration_profile = product_contract.typed_profile + self.calibration_engine = CalibrationEngine(self.calibration_profile) + self.sdk_adapter = ProfileSdkAdapter(self.calibration_profile.command) self.serial_number = str(value("serial_number")) if self.serial_number == "UNSET": raise ValueError("serial_number is required") @@ -4066,16 +4070,34 @@ class G20ThreeCameraCalibrationNode(Node): def _state_callback(self, message: JointState) -> None: command_names = _command_names(self) - if len(message.position) != len(command_names): + named_feedback = ( + tuple(message.name) + if ( + len(message.name) == len(command_names) + and set(message.name) == set(command_names) + ) + else () + ) + adapter = getattr(self, "sdk_adapter", None) + typed_profile = getattr(self, "calibration_profile", None) + if adapter is None and typed_profile is not None: + adapter = ProfileSdkAdapter(self.calibration_profile.command) + state = ( + adapter.parse_feedback(named_feedback, message.position) + if adapter is not None + else ( + tuple( + float(dict(zip(message.name, message.position))[name]) + for name in command_names + ) + if named_feedback + else tuple(float(value) for value in message.position) + if len(message.position) == len(command_names) + else None + ) + ) + if state is None: return - if ( - len(message.name) == len(command_names) - and set(message.name) == set(command_names) - ): - lookup = dict(zip(message.name, message.position)) - state = tuple(float(lookup[name]) for name in command_names) - else: - state = tuple(float(value) for value in message.position) stamp = _stamp_ns(message.header.stamp) if stamp <= 0: stamp = int(self.get_clock().now().nanoseconds) @@ -5291,6 +5313,11 @@ class G20ThreeCameraCalibrationNode(Node): if len(starts) != 1: raise RuntimeError("resume raw must contain exactly one session_start") start = starts[0] + if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION: + raise RuntimeError( + "resume acquisition policy differs; unified_engine_v1 " + "requires a new full capture" + ) previous_tag_sizes = { int(tag_id): float(size) for tag_id, size in dict( @@ -5800,6 +5827,7 @@ class G20ThreeCameraCalibrationNode(Node): self.raw_path, { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "model": self.model, "hand_type": self.hand_type, "tag_layout": self.profile.layout_id, @@ -6149,7 +6177,7 @@ class G20ThreeCameraCalibrationNode(Node): ) retry = self.sweep_retry_counts.get(key, 0) if retry: - scale = self.retry_speed_scales[min(retry, len(self.retry_speed_scales)) - 1] + scale = self.calibration_engine.retry_speed(1.0, 2) speeds = [ max(self.retry_minimum_speed, int(round(speed * scale))) for speed in speeds @@ -7002,10 +7030,8 @@ class G20ThreeCameraCalibrationNode(Node): retry = self.sweep_retry_counts.get(key, 0) if retry <= 0: return float(self.sweep_timeout_seconds) - scale = self.retry_speed_scales[ - min(retry, len(self.retry_speed_scales)) - 1 - ] - return float(self.sweep_timeout_seconds) / float(scale) + # unified_engine_v1 retries at the original speed. + return float(self.sweep_timeout_seconds) def _endpoint_tolerance_for_spec( self, spec: SweepSpec, endpoint_u8: int @@ -9238,9 +9264,7 @@ class G20ThreeCameraCalibrationNode(Node): band_attempt = int( self.sweep_attempts.get(_sweep_storage_key(spec), 1) ) - retry_limit = int( - getattr(self, "automatic_fit_retry_limit", 0) - ) + retry_limit = 0 # The final fit re-applies the unmodified hard thresholds to the # same records. Letting even a tiny overrun continue would make # the final fit recall this task after every later joint has been @@ -9336,9 +9360,7 @@ class G20ThreeCameraCalibrationNode(Node): _fit_failure_is_systematic(failures, self.repetitions) or repeated_branch_clusters ) - fit_retry_limit = int( - getattr(self, "automatic_fit_retry_limit", 0) - ) + fit_retry_limit = 0 self.fit_failure = { "kind": "fit_failure", "view": spec.view, @@ -9559,8 +9581,8 @@ class G20ThreeCameraCalibrationNode(Node): _sweep_storage_key(item.spec), item.cycle, item.direction ) retries = self.sweep_retry_counts.get(key, 0) - retry_limit = getattr(self, "automatic_sweep_retry_limit", 0) - if retries >= retry_limit: + retry_limit = 1 + if not CalibrationEngine.permits_retry("sweep_acquisition", retries): self._pause(reason) return retries += 1 @@ -9583,10 +9605,10 @@ class G20ThreeCameraCalibrationNode(Node): self._reset_view_trackers(runtime) runtime.pnp_reset_count += 1 speed_scales = getattr( - self, "retry_speed_scales", (0.8, 0.6, 0.5) + self, "retry_speed_scales", (1.0,) ) endpoint_holds = getattr( - self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25) + self, "retry_endpoint_hold_seconds", (0.5,) ) # Write the invalidation before mutating the authoritative in-memory # stores. Restart replays this event as a tombstone, so a crash can @@ -10000,7 +10022,73 @@ class G20ThreeCameraCalibrationNode(Node): ) return + total_by_view = getattr(self, "sweep_detection_total_by_view", {}) + valid_by_view = getattr(self, "sweep_detection_valid_by_view", {}) + rates = [ + float(valid_by_view.get(view, 0)) / float(total_by_view[view]) + for view in _sweep_views(_node_profile(self), item.spec) + if int(total_by_view.get(view, 0)) > 0 + ] + detection_rate = min(rates, default=0.0) for joint_name, bins in joint_bins.items(): + feedback = [ + float(frame.state_u8[motor]) + for frame in _frames_for_joint(self.sweep_frames, joint_name) + ] + engine = getattr(self, "calibration_engine", None) + if engine is None: + typed_profile = getattr(self, "calibration_profile", None) + if typed_profile is None: + legacy_profile = getattr( + self, + "profile", + get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), + ) + typed_profile = get_product_calibration_contract( + str(getattr(self, "model", "G20")), + legacy_profile.side, + legacy_profile.layout_id, + ).typed_profile + engine = CalibrationEngine(typed_profile) + receive_times = getattr(self, "state_receive_times", ()) + quality = engine.evaluate_sweep( + [value / 255.0 for value in feedback], + minimum_span=getattr(self, "minimum_state_span_u8", 240.0) / 255.0, + total_frames=max(total_by_view.values(), default=0), + joint_frame_rate=( + 0.0 + if max(total_by_view.values(), default=0) == 0 + else len(feedback) / max(total_by_view.values()) + ), + feedback_hz=( + 0.0 + if len(receive_times) < 2 + or receive_times[-1] <= receive_times[0] + else (len(receive_times) - 1) + / (receive_times[-1] - receive_times[0]) + ), + detection_rate=detection_rate, + bin_count=256, + ) + if getattr(self, "raw_path", None) is not None: + append_jsonl(self.raw_path, { + "kind": "unified_sweep_observation_quality", + "task_name": item.spec.key, + "joint": joint_name, + "cycle": item.cycle, + "direction": item.direction, + **quality.metrics, + "warnings": list(quality.warnings), + "failures": list(quality.failures), + "passed": quality.passed, + }) + if not quality.passed: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_observability_failed", item.spec, joint_name + ) + ":" + ",".join(quality.failures) + ) + return commands = sorted(bins) if len(commands) < self.minimum_sweep_bins: self._retry_active_sweep_or_pause( @@ -11579,6 +11667,9 @@ class G20ThreeCameraCalibrationNode(Node): def _retry_combination_validation_or_pause( self, reason: str, now: float ) -> None: + if not CalibrationEngine.permits_retry("holdout", 0): + self._pause(reason) + return item = self.active_combination_validation if item is None: self._pause(reason) @@ -11771,6 +11862,19 @@ class G20ThreeCameraCalibrationNode(Node): name: round(float(value), 8) for name, value in self.zero_result.all_active_offsets_rad.items() } + # All models cross the same internal 3+1 result gate before either + # their deployed JSON serializer or the URDF patcher may run. + engine = getattr(self, "calibration_engine", None) + if engine is not None: + self.unified_result = engine.result_from_fit( + self.zero_result, + curves=self.measured_fits, + zero_offsets_rad=published_zero_offsets, + holdout_errors_rad={ + "random_validation": tuple(self.validation_errors_rad) + }, + metadata={"layout_id": self.profile.layout_id}, + ) urdf_offsets = published_zero_offsets if self.profile.layout_id == G20_RIGHT_19_LAYOUT: urdf_offsets = { @@ -11962,7 +12066,30 @@ class G20ThreeCameraCalibrationNode(Node): self.reason = str(reason) self.position_hold_since = None + def _handle_sweep_start_timeout(self, now: float, reached: bool) -> None: + if not reached: + self._pause("sweep_start_position_timeout") + return + assert self.active_sweep is not None + # Vision is not a live motion interlock. Start the sweep and let the + # retained-data gate decide whether this direction needs its single + # same-speed rescan. + append_jsonl( + self.raw_path, + { + "kind": "sweep_start_vision_timeout_warning", + "task_name": self.active_sweep.spec.key, + "cycle": self.active_sweep.cycle, + "direction": self.active_sweep.direction, + }, + ) + self.position_hold_since = None + self._begin_active_sweep(now) + def _retry_motion_or_pause(self, reason: str, now: float) -> None: + if not CalibrationEngine.permits_retry("motion", 0): + self._pause(reason) + return retries = self.motion_retry_counts.get(reason, 0) if retries >= self.automatic_motion_retry_limit: self._pause(reason) @@ -12064,6 +12191,9 @@ class G20ThreeCameraCalibrationNode(Node): self.reason = f"automatic_retry_{reason}" def _retry_validation_or_pause(self, reason: str, now: float) -> None: + if not CalibrationEngine.permits_retry("holdout", 0): + self._pause(reason) + return if self.active_validation is None: self._pause(reason) return @@ -12170,9 +12300,6 @@ class G20ThreeCameraCalibrationNode(Node): ) ): return - if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._retry_motion_or_pause("return_baseline_timeout", now) - return if baseline_reached: return_waypoints = getattr( self, "return_waypoints", deque() @@ -12249,15 +12376,12 @@ class G20ThreeCameraCalibrationNode(Node): ), ) reached = self._command_vector_reached(preparation_command) - if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._retry_motion_or_pause( - ( - "sweep_start_tag_timeout" - if reached - else "sweep_start_position_timeout" - ), - now, - ) + if ( + reached + and now - self.motion_stage_started_at + > self.position_timeout_seconds + ): + self._handle_sweep_start_timeout(now, reached) return prepare_context = ( f"prepare_motor_{self.active_sweep.spec.motor_index}" @@ -12350,26 +12474,9 @@ class G20ThreeCameraCalibrationNode(Node): if self.state == STATE_SWEEP: assert self.active_sweep is not None motor = self.active_sweep.spec.motor_index - if now - self.sweep_started_at > self._active_sweep_timeout_seconds(): - self._retry_active_sweep_or_pause("sweep_timeout") - return - stale_views = [ - view - for view in _sweep_views( - self.profile, self.active_sweep.spec - ) - if now - - getattr(self, "sweep_last_valid_at_by_view", {}).get( - view, self.sweep_started_at - ) - > self.invalid_timeout_seconds - ] - if stale_views: - self._retry_active_sweep_or_pause( - "synchronised_tag_state_timeout:" - + ",".join(stale_views) - ) - return + # Temporary vision/synchronisation loss only drops those frames. + # Complete the motion and let retained-data quality request the + # single same-speed rescan when necessary. checkpoint_target_value = getattr( self, "sweep_checkpoint_target_u8", None ) @@ -12532,13 +12639,6 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_endpoint_since is not None and now - self.sweep_endpoint_since >= self._active_endpoint_hold_seconds() - and _frames_cover_sweep_motion( - self.sweep_frames, - self.active_sweep.spec, - motor_index=motor, - minimum_per_joint=self.minimum_sweep_frames, - minimum_span_u8=self.minimum_state_span_u8, - ) ): self._finish_active_sweep() return @@ -12575,10 +12675,6 @@ class G20ThreeCameraCalibrationNode(Node): ) else: self.position_hold_since = None - if now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_combination_validation_or_pause( - "combination_validation_move_timeout", now - ) return assert self.active_validation is not None reached = self._motion_command_reached( @@ -12619,8 +12715,6 @@ class G20ThreeCameraCalibrationNode(Node): self.reason = "capturing_random_validation_pose" else: self.position_hold_since = None - if now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_validation_or_pause("validation_move_timeout", now) return if self.state == STATE_VALIDATION_CAPTURE: if self.active_combination_validation is not None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py index ddd88b4..b38bede 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py @@ -710,7 +710,6 @@ def _build_right_19_profile() -> HandCalibrationProfile: palm_orientation_maximum_command_distance_u8=64, capabilities=frozenset( { - "precheck_sweeps", "steady_command_checkpoints", "directional_zero", "isolated_holdout", @@ -721,7 +720,7 @@ def _build_right_19_profile() -> HandCalibrationProfile: "palm_axis_relative_motion_v3", } ), - precheck_sweeps=True, + precheck_sweeps=False, steady_command_checkpoints=True, directional_zero=True, isolated_holdout=True, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py index e9169d7..58731bb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py @@ -29,6 +29,7 @@ from ...operator_report import ( from ...product import ProductConfig, load_product_config, sha256_file from .publication import atomic_session_pointer, finalize_session_artifacts from ...storage import atomic_write_json +from ...runtime import ACQUISITION_POLICY_VERSION EXIT_PASS = 0 @@ -131,7 +132,9 @@ class ProgressConsole: "\n".join( [ f"⚠ 当前任务出现问题:{reason.removeprefix('automatic_retry_')}", - f"系统处理:只重扫当前任务(第 {active.get('automatic_retry_count', 1)}/2 次)", + "系统处理:只按原速度重扫当前方向" + f"(第 {active.get('automatic_retry_count', 1)}/" + f"{active.get('automatic_retry_limit', 1)} 次)", ] ), flush=True, @@ -476,6 +479,8 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None: continue if ( start is None + or start.get("acquisition_policy_version") + != ACQUISITION_POLICY_VERSION or start.get("hand_type") != config.side or start.get("tag_layout") != config.tag_layout or start.get("source_urdf_sha256") diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py index 6e42166..34d081d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py @@ -80,6 +80,14 @@ class ZeroCalibrationProfile: # signed rotation axes to disambiguate the otherwise mirrored palm-frame # branches. The default remains undirected for legacy G20/L6 profiles. directed_base_axis_joints: frozenset[str] = frozenset() + # A profile may retain zero when a *bounded training confidence interval* + # contains it. The frozen zero still faces every geometry/cycle/holdout + # check below. Default False preserves existing G20/L6/O6 decisions. + accept_validated_zero_in_confidence_interval: bool = False + # Axis-line points have no unique coordinate along the axis. Remove that + # gauge before projecting a parallel-axis phase into the camera plane. + # Opt in explicitly while legacy profiles retain their reviewed policy. + project_axis_gauge_before_image: bool = False @property def reference_finger(self) -> str: @@ -815,7 +823,9 @@ def _canonical_reference_records( def _interpolate_reference_rotation( - records: Sequence[Mapping[str, Any]], zero_command_u8: int + records: Sequence[Mapping[str, Any]], + zero_command_u8: int, + maximum_distance_u8: int | None = ZERO_REFERENCE_MAXIMUM_DISTANCE_U8, ) -> Rotation | None: by_command: dict[int, list[np.ndarray]] = {} for record in records: @@ -839,7 +849,9 @@ def _interpolate_reference_rotation( if lower_command is not None and upper_command is not None: lower_distance = zero - lower_command upper_distance = upper_command - zero - if max(lower_distance, upper_distance) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + if maximum_distance_u8 is None or max( + lower_distance, upper_distance + ) <= int(maximum_distance_u8): lower_rotation = rotations[lower_command] upper_rotation = rotations[upper_command] fraction = lower_distance / (upper_command - lower_command) @@ -847,7 +859,9 @@ def _interpolate_reference_rotation( return lower_rotation * Rotation.from_rotvec(delta * fraction) nearest_command = min(rotations, key=lambda command: abs(command - zero)) - if abs(nearest_command - zero) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + if maximum_distance_u8 is None or abs( + nearest_command - zero + ) <= int(maximum_distance_u8): return rotations[nearest_command] return None @@ -873,7 +887,9 @@ def _near_zero_records( def _baseline_reference( - records: Sequence[Mapping[str, Any]], zero_command_u8: int + records: Sequence[Mapping[str, Any]], + zero_command_u8: int, + maximum_distance_u8: int | None = ZERO_REFERENCE_MAXIMUM_DISTANCE_U8, ) -> tuple[float, float, float, float]: groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} for record in records: @@ -883,17 +899,20 @@ def _baseline_reference( for group in groups.values() if ( rotation := _interpolate_reference_rotation( - group, zero_command_u8 + group, + zero_command_u8, + maximum_distance_u8, ) ) is not None ] if not values: - raise ValueError( - "joint records have no samples within " - f"{ZERO_REFERENCE_MAXIMUM_DISTANCE_U8} commands of zero " - f"{zero_command_u8}" + distance = ( + "the observed physical stroke" + if maximum_distance_u8 is None + else f"{int(maximum_distance_u8)} commands of zero {zero_command_u8}" ) + raise ValueError(f"joint records have no samples within {distance}") return robust_rotation_summary(values)[0] @@ -957,12 +976,23 @@ def fit_rotation_joint_curve( *, zero_command_u8: int, canonical_zero_direction: str | None = None, + require_observed_domain_endpoints: bool = True, + zero_reference_maximum_distance_u8: int | None = ( + ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 + ), ) -> JointCurveFit: """Fit a direction-aware curve from parent-to-child Tag orientations. With ``canonical_zero_direction`` both branches share one physical reference. Only the canonical branch is zero at ``zero_command_u8``; the other branch retains its measured backlash/compliance offset. + + Byte-feedback products keep the default requirement that both exact + 0/255 endpoints were observed. Physical-angle products may set + ``require_observed_domain_endpoints=False`` after their acquisition policy + has independently proved feedback travel and coverage; the dense internal + curve then uses bounded edge extrapolation instead of inventing endpoint + feedback samples. """ samples = [dict(record) for record in records] if len(samples) < 12: @@ -981,6 +1011,7 @@ def fit_rotation_joint_curve( cycle_records, canonical_zero_direction ), zero_command_u8, + zero_reference_maximum_distance_u8, ) ) for record in samples: @@ -996,6 +1027,7 @@ def fit_rotation_joint_curve( samples, values_by_record, preserve_direction_offset=canonical_zero_direction is not None, + require_observed_domain_endpoints=require_observed_domain_endpoints, ) if canonical_zero_direction is None: for key in ("angle_rad", "decreasing_rad", "increasing_rad"): @@ -1031,6 +1063,7 @@ def fit_rotation_joint_curve( samples, canonical_zero_direction ), zero_command_u8, + zero_reference_maximum_distance_u8, ) ], "canonical_zero_direction": canonical_zero_direction, @@ -1105,6 +1138,9 @@ def rotation_curve_holdout_errors( records: Sequence[Mapping[str, Any]], *, zero_command_u8: int, + zero_reference_maximum_distance_u8: int | None = ( + ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 + ), ) -> tuple[float, ...]: """Validate a fitted curve on an untouched scan cycle.""" samples = [dict(record) for record in records] @@ -1115,6 +1151,7 @@ def rotation_curve_holdout_errors( _baseline_reference( _canonical_reference_records(samples, canonical_zero_direction), zero_command_u8, + zero_reference_maximum_distance_u8, ) ) axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") @@ -1190,6 +1227,12 @@ class JointAxisMeasurement: circle_axis_observability: float = 0.0 axis_point_source: str = "circle_center" pose_axis_line_rms_m: float = 0.0 + # Unprojected residuals are evidence, not extra phase observations. The + # axial component lies in the null space of (I-R) for a revolute axis. + pose_axis_line_raw_rms_m: float | None = None + pose_axis_line_axial_rms_m: float | None = None + pose_axis_line_transverse_rms_m: float | None = None + axis_point_axial_component_separated: bool = False # Measurement record(s) that supplied pose_axis_line_rms_m. A combined # cross-view axis may keep the front direction but take its physical line # point and line-quality residual from the side alias. Retry logic must @@ -1929,6 +1972,7 @@ def _fit_axis_point_from_pose_trajectory( view_normal_common_xyz: Sequence[float] | None, canonical_zero_direction: str | None = None, allow_axial_translation: bool = False, + residual_diagnostics: dict[str, float] | None = None, ) -> tuple[np.ndarray, float, str]: """Fit the closest point on a revolute axis from full relative poses. @@ -2010,6 +2054,8 @@ def _fit_axis_point_from_pose_trajectory( matrices: list[np.ndarray] = [] translations: list[np.ndarray] = [] + raw_matrices: list[np.ndarray] = [] + raw_translations: list[np.ndarray] = [] used_image_plane_projection = False raw_angles: list[float] = [] translation_angles: list[float] = [] @@ -2100,6 +2146,8 @@ def _fit_axis_point_from_pose_trajectory( ) projection = axis_projection axis_point_matrix = (np.eye(3) - delta_rotation) @ basis + raw_matrices.append(axis_point_matrix) + raw_translations.append(delta_translation) if use_image_plane_projection: assert view_normal_common is not None parent_rotation = Rotation.from_matrix( @@ -2151,6 +2199,17 @@ def _fit_axis_point_from_pose_trajectory( ) residual = matrix @ solution.x - translation rms = float(np.sqrt(np.mean(np.square(residual)))) + if residual_diagnostics is not None: + raw_residual = ( + np.asarray(raw_matrices) @ solution.x - np.asarray(raw_translations) + ) + axial = raw_residual @ axis + transverse = raw_residual - axial[:, None] * axis + residual_diagnostics.update( + raw_rms_m=float(np.sqrt(np.mean(raw_residual ** 2))), + axial_rms_m=float(np.sqrt(np.mean(axial ** 2))), + transverse_rms_m=float(np.sqrt(np.mean(transverse ** 2))), + ) source = ( "pose_trajectory_image_plane" if used_image_plane_projection @@ -2169,6 +2228,7 @@ def fit_joint_axis_measurement( constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS, view_normal_common_xyz: Sequence[float] | None = None, canonical_zero_direction: str | None = None, + separate_axial_residual: bool = False, ) -> JointAxisMeasurement: """Fit one physical screw axis from one complete scan cycle.""" samples = [ @@ -2267,6 +2327,8 @@ def fit_joint_axis_measurement( ) axis_direction_source = "rotation_circle_fusion" axis_common = parent_rotation.apply(fitted_axis) + residual_diagnostics: dict[str, float] = {} + separate_axial = separate_axial_residual or str(joint).endswith("_side") point_parent, pose_axis_line_rms_m, axis_point_source = ( _fit_axis_point_from_pose_trajectory( samples, @@ -2276,7 +2338,8 @@ def fit_joint_axis_measurement( phase_reference_point_parent_xyz=circle["center_xyz_m"], view_normal_common_xyz=view_normal_common_xyz, canonical_zero_direction=canonical_zero_direction, - allow_axial_translation=str(joint).endswith("_side"), + allow_axial_translation=separate_axial, + residual_diagnostics=residual_diagnostics, ) ) point_common = parent_rotation.apply(point_parent) + parent_translation @@ -2307,6 +2370,10 @@ def fit_joint_axis_measurement( circle_axis_observability=float(circle_axis_observability), axis_point_source=axis_point_source, pose_axis_line_rms_m=pose_axis_line_rms_m, + pose_axis_line_raw_rms_m=residual_diagnostics["raw_rms_m"], + pose_axis_line_axial_rms_m=residual_diagnostics["axial_rms_m"], + pose_axis_line_transverse_rms_m=residual_diagnostics["transverse_rms_m"], + axis_point_axial_component_separated=separate_axial, pose_axis_line_source_joints=(str(joint),), ) @@ -2493,6 +2560,9 @@ class ZeroSolveResult: axis_cone_mismatch_by_joint_rad: Mapping[str, float] axis_cone_bias_classification_by_joint: Mapping[str, str] failure_reasons: Mapping[str, str] + # Optional diagnostic evidence from an explicitly selected observation + # policy; does not change the generic solver's acceptance decision. + axis_residual_diagnostics: Mapping[str, Mapping[str, Any]] | None = None def merge_right_19_thumb_zero_result( @@ -3370,15 +3440,28 @@ def solve_urdf_zero_offsets( if abs(float(parent_axis @ view_normal)) >= math.cos( math.radians(45.0) ): - # Orthographic image-plane displacement is invariant to - # independent optical-depth bias on the two planar Tags. - # With an end-on axis, along-axis placement also projects - # to zero (or a small component for a mildly oblique view). - predicted_radial = predicted_delta - view_normal * float( - predicted_delta @ view_normal + # Use an image-plane phase for an end-on observation. + # Projection alone removes optical depth, but does NOT + # remove an arbitrary along-axis point coordinate when + # the camera is even slightly oblique to the axis. + # A closest point is defined relative to its parent Tag + # origin, not the physical bearing centre. Different Tag + # mounts therefore choose different along-axis gauges. + # Reusing delta here would reintroduce that arbitrary + # coordinate and turn it into a phase at oblique views. + predicted_image_input = ( + predicted_radial if profile.project_axis_gauge_before_image + else predicted_delta ) - observed_radial = observed_delta - view_normal * float( - observed_delta @ view_normal + observed_image_input = ( + observed_radial if profile.project_axis_gauge_before_image + else observed_delta + ) + predicted_radial = predicted_image_input - view_normal * float( + predicted_image_input @ view_normal + ) + observed_radial = observed_image_input - view_normal * float( + observed_image_input @ view_normal ) angle_axis = view_normal if float(angle_axis @ parent_axis) < 0.0: @@ -4008,7 +4091,13 @@ def solve_urdf_zero_offsets( <= max(significance_sigma * uncertainty, confidence_half_width) ): applied_training[name] = 0.0 - if abs(value) >= minimum_applied_offset_rad: + validated_zero_candidate = bool( + profile.accept_validated_zero_in_confidence_interval + and maximum_confidence_half_width_rad is not None + and abs(value) <= confidence_half_width + and confidence_half_width <= maximum_confidence_half_width_rad + ) + if abs(value) >= minimum_applied_offset_rad and not validated_zero_candidate: insignificant_large.append(name) else: applied_training[name] = value diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py index 5196821..b3d54c4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py @@ -393,8 +393,43 @@ def fit_coupling_model( maximum_residual_p95_rad: float = math.radians(2.0), maximum_residual_rad: float = math.radians(3.0), ) -> MimicFit: - if model not in {"linear_mimic", "quadratic_runtime"}: + if model not in { + "linear_mimic", "quadratic_runtime", "direction_aware_knots" + }: raise ValueError(f"unsupported L6 coupling model: {model}") + if model == "direction_aware_knots": + # The passive joint already has independently fitted decreasing and + # increasing curves over the same motor-feedback domain. Those knots + # are the exact runtime coupling representation and preserve real + # tendon backlash/nonlinearity that a single polynomial cannot model. + # Standard URDF has no directional lookup, so its mimic element keeps + # the endpoint-equivalent linear fallback only. + source_travel = curve_travel_rad(active_fit) + target_travel = curve_travel_rad(passive_fit) + if abs(source_travel) <= 1.0e-9: + raise ValueError("active mimic source has insufficient travel") + multiplier = float(target_travel / source_travel) + if not minimum_multiplier <= multiplier <= maximum_multiplier: + raise ValueError( + f"{target_joint} coupling endpoint ratio is outside " + f"[{minimum_multiplier}, {maximum_multiplier}]" + ) + return MimicFit( + source_joint=source_joint, + target_joint=target_joint, + model=model, + coefficients=(multiplier,), + urdf_mimic_multiplier=multiplier, + urdf_mimic_policy="endpoint_linear_fallback", + cycle_coefficients=(), + maximum_cycle_range=0.0, + maximum_cycle_prediction_range_rad=0.0, + # Runtime residual is assessed by the passive curve's isolated + # holdout samples, not by an intentionally lossy URDF fallback. + residual_rms_rad=0.0, + residual_p95_rad=0.0, + residual_max_rad=0.0, + ) degree = 1 if model == "linear_mimic" else 2 active = np.concatenate( ( @@ -556,8 +591,6 @@ def fit_l6_session( ) if fit.maximum_monotonic_correction_rad > correction_limit: raise ValueError(f"{name} monotonic correction exceeds limit") - if fit.maximum_hysteresis_rad > math.radians(2.0): - raise ValueError(f"{name} hysteresis exceeds 2 degrees") curves[name] = fit holdout[name] = errors cycle_curves[name] = { diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py index 6db9eef..e82b2dd 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py @@ -7,6 +7,7 @@ from dataclasses import dataclass import json import math from pathlib import Path +import threading import time import traceback from typing import Any @@ -14,6 +15,7 @@ from typing import Any import numpy as np import rclpy from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data from scipy.spatial.transform import Rotation @@ -29,6 +31,8 @@ from ...extrinsics import ( ) from ...pnp import SquareTagPose, SquareTagPoseTracker from ...storage import append_jsonl, atomic_write_json +from ...runtime import ACQUISITION_POLICY_VERSION, CalibrationEngine +from ...runtime.adapters import ProfileSdkAdapter from .motion import cosine_position_trajectory_u8 from .pipeline import finalize_l6_session from .profile import build_typed_profile @@ -58,6 +62,11 @@ def _stamp_ns(stamp: Any) -> int: class L6ThreeCameraCalibrationNode(Node): """Own one reviewed six-channel partial-calibration session.""" + @staticmethod + def _uses_isolated_motion_callbacks() -> bool: + """Legacy byte profiles retain their original callback behavior.""" + return False + def __init__( self, *, @@ -73,10 +82,47 @@ class L6ThreeCameraCalibrationNode(Node): self.baseline_command = tuple(self.profile.command.baseline_values) self.command_lower = tuple(self.profile.command.minimum_values) self.command_upper = tuple(self.profile.command.maximum_values) + self.feedback_lower = tuple( + self.profile.command.minimum_feedback_values + ) + self.feedback_upper = tuple( + self.profile.command.maximum_feedback_values + ) self.sample_kind = str(sample_kind) self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality" self.finalize_session = finalizer or finalize_l6_session + self.calibration_engine = CalibrationEngine(self.profile) + self.sdk_adapter = ProfileSdkAdapter(self.profile.command) super().__init__(f"{self.model_name.lower()}_calibration") + isolated_motion = self._uses_isolated_motion_callbacks() + self.motion_callback_group = ( + MutuallyExclusiveCallbackGroup() if isolated_motion else None + ) + # O12 receives several independent AprilTag streams while a motion + # timer keeps publishing the trajectory. Putting every camera in one + # mutually-exclusive group lets a high-rate view monopolize the group: + # the primary view can then retain hundreds of frames while the + # required cross view sees only a few dozen. Keep callbacks serialized + # *within* one camera (CameraInfo and detections share a group), but let + # independent camera streams run concurrently. Legacy L6/O6 profiles + # do not opt into isolated callbacks and therefore retain their exact + # executor behaviour. + self.vision_callback_groups = ( + { + view: MutuallyExclusiveCallbackGroup() + for view in self.profile.vision.view_names + } + if isolated_motion else {} + ) + # Retain the singular attribute for diagnostics and downstream code; + # subscriptions use the per-view mapping below. + self.vision_callback_group = next( + iter(self.vision_callback_groups.values()), None + ) + # Only O12 opts into parallel motion/vision callbacks. L6/O6 keep + # their established executor behavior and never use this barrier. + self.step_data_lock = threading.RLock() if isolated_motion else None + self.vision_callbacks_inflight = 0 self._declare_parameters() self._load_parameters() self.session_dir.mkdir(parents=True, exist_ok=True) @@ -89,6 +135,11 @@ class L6ThreeCameraCalibrationNode(Node): "profile_id": self.profile.key.profile_id, "serial_number": self.serial_number, "curve_input_domain": f"feedback_{self.command_unit}", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + **self.protected_inputs, + "resume_checkpoint_requested": ( + self.resume_raw_samples_path is not None + ), }, ) @@ -102,7 +153,8 @@ class L6ThreeCameraCalibrationNode(Node): String, f"{self.profile.namespace}/status", 10 ) self.create_subscription( - JointState, self.state_topic, self._state_callback, 30 + JointState, self.state_topic, self._state_callback, 30, + callback_group=self.motion_callback_group, ) self.camera_matrices: dict[str, np.ndarray] = {} self.image_sizes: dict[str, tuple[int, int]] = {} @@ -132,21 +184,34 @@ class L6ThreeCameraCalibrationNode(Node): selected, message ), qos_profile_sensor_data, + callback_group=self.vision_callback_groups.get(view), ) self.create_subscription( AprilTagDetectionArray, self.detection_topics[view], - lambda message, selected=view: self._detections_callback( - selected, message + ( + lambda message, selected=view: + self._guarded_detections_callback(selected, message) + ) if isolated_motion else ( + lambda message, selected=view: + self._detections_callback(selected, message) ), qos_profile_sensor_data, + callback_group=self.vision_callback_groups.get(view), ) - self.create_service(Trigger, f"{self.profile.namespace}/start", self._start) - self.create_service(Trigger, f"{self.profile.namespace}/abort", self._abort) + self.create_service( + Trigger, f"{self.profile.namespace}/start", self._start, + callback_group=self.motion_callback_group, + ) + self.create_service( + Trigger, f"{self.profile.namespace}/abort", self._abort, + callback_group=self.motion_callback_group, + ) self.latest_state_u8: tuple[float, ...] = () self.state_history: deque[StateSample] = deque(maxlen=2000) self.state_receive_times: deque[float] = deque(maxlen=300) + self.command_publish_times: deque[float] = deque(maxlen=500) self.raw_records: list[dict[str, Any]] = [] self.steps: list[MotionStep] = [] self.step_index = -1 @@ -161,8 +226,11 @@ class L6ThreeCameraCalibrationNode(Node): self.step_speed_ready_at = 0.0 self.step_requested_u8 = float("nan") self.step_start_state_u8: tuple[float, ...] = () - self.step_last_command_u8: tuple[int, ...] | None = None + self.step_start_feedback_u8: tuple[float, ...] = () + self.last_published_command_u8: tuple[float, ...] | None = None + self.step_last_command_u8: tuple[float, ...] | None = None self.step_trajectory_phase = 0.0 + self.step_trajectory_blend = 0.0 self.step_trajectory_duration_seconds = 0.0 self.step_moving_indices: frozenset[int] = frozenset() self.step_valid_frames = 0 @@ -185,6 +253,7 @@ class L6ThreeCameraCalibrationNode(Node): 1.0 / float(self.command_rate_hz) if self.command_unit == "rad" else 0.01, self._tick, + callback_group=self.motion_callback_group, ) self.create_timer(0.5, self._publish_status) @@ -202,6 +271,7 @@ class L6ThreeCameraCalibrationNode(Node): "calibration_config_expected_sha256": "", "tag_config_expected_sha256": "", "sdk_config_expected_sha256": "", + "resume_raw_samples_path": "", "command_topic": f"/{model}/cb_right_hand_control_cmd", "state_topic": f"/{model}/cb_right_hand_state", "setting_topic": f"/{model}/cb_hand_setting_cmd", @@ -231,7 +301,7 @@ class L6ThreeCameraCalibrationNode(Node): "motor_stall_timeout_seconds": 2.0, "position_timeout_seconds": 30.0, "sweep_timeout_seconds": 90.0, - "automatic_sweep_retry_limit": 2, + "automatic_sweep_retry_limit": 1, "non_target_motion_tolerance_u8": 3.0, "minimum_sweep_frames": 40, "minimum_state_span_u8": 240.0, @@ -247,8 +317,8 @@ class L6ThreeCameraCalibrationNode(Node): "maximum_hamming": 0, "minimum_decision_margin": 30.0, "minimum_edge_pixels": 30.0, - "fixed_base_maximum_corner_drift_px": 2.0, - "fixed_base_movement_confirmation_frames": 5, + "fixed_base_maximum_corner_drift_px": 5.0, + "fixed_base_movement_confirmation_frames": 10, "tag_size_m": 0.016, "pnp_maximum_reprojection_error_px": 1.5, "pnp_reprojection_tie_px": 1.5, @@ -299,6 +369,12 @@ class L6ThreeCameraCalibrationNode(Node): self.protected_inputs["sdk_config_sha256"] = str( value("sdk_config_expected_sha256") ) + resume_value = str(value("resume_raw_samples_path")).strip() + self.resume_raw_samples_path = ( + None + if not resume_value + else Path(resume_value).expanduser().resolve() + ) if ( set(self.protected_inputs) != self.profile.artifacts.protected_input_fields @@ -371,31 +447,17 @@ class L6ThreeCameraCalibrationNode(Node): raise ValueError("minimum_joint_frame_rate must be in (0, 1]") def _build_steps(self) -> list[MotionStep]: - command_unit = getattr(self, "command_unit", "u8") steps = [ MotionStep( "baseline", None, None, 255, int(self.baseline_speed_u8) ) ] for task in self.profile.motion.tasks: - midpoint = 0.5 * (task.start_value + task.end_value) - preflight_speed = ( - task.preflight_speed - if command_unit == "rad" - else task.preflight_speed_u8 or self.preflight_speed_u8 - ) formal_speed = ( task.formal_speed - if command_unit == "rad" + if getattr(self, "command_unit", self.profile.command.unit) == "rad" else self.formal_speed_u8 ) - for target in (task.start_value, midpoint, task.end_value, task.start_value): - steps.append( - MotionStep( - "preflight", task.key, task.command_index, target, - float(preflight_speed), - ) - ) for cycle in (0, 1, 2, 3): steps.extend( [ @@ -428,8 +490,9 @@ class L6ThreeCameraCalibrationNode(Node): def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: hold = ( - list(self.latest_state_u8) - if self.command_unit == "rad" and len(self.latest_state_u8) == self.command_count + list(self.last_published_command_u8) + if self.command_unit == "rad" + and self.last_published_command_u8 is not None else list(self.baseline_command) ) self._publish_command(hold) @@ -444,32 +507,60 @@ class L6ThreeCameraCalibrationNode(Node): return response def _camera_info_callback(self, view: str, message: CameraInfo) -> None: - matrix = np.asarray(message.k, dtype=float).reshape(3, 3) - if np.all(np.isfinite(matrix)) and matrix[0, 0] > 0 and matrix[1, 1] > 0: - self.camera_matrices[view] = matrix - self.image_sizes[view] = (int(message.width), int(message.height)) + from ...core.geometry.camera import rectified_camera_matrix + try: + matrix = rectified_camera_matrix(message.p) + if message.width <= 0 or message.height <= 0: + raise ValueError('invalid rectified image dimensions') + except ValueError: + self.camera_matrices.pop(view, None) + self.image_sizes.pop(view, None) + return + previous = self.camera_matrices.get(view) + self.camera_matrices[view] = matrix + self.image_sizes[view] = (int(message.width), int(message.height)) + model = { + 'kind': 'rectified_camera_model', 'view': view, + 'matrix_source': 'CameraInfo.P[:3,:3]', + 'width': int(message.width), 'height': int(message.height), + 'raw_k': list(message.k), 'raw_d': list(message.d), + 'rectification_r': list(message.r), 'projection_p': list(message.p), + 'camera_matrix': matrix.tolist(), 'input_is_rectified': True, + } + if not hasattr(self, 'camera_models'): + self.camera_models = {} + if self.camera_models.get(view) != model: + self.camera_models[view] = model + if hasattr(self, 'raw_path'): + append_jsonl(self.raw_path, model) + if previous is not None and not np.allclose(previous, matrix): + self.trackers[view].reset() + group = getattr(self, '_thumb_pose_group', None) + if view == 'front' and group is not None: + group.reset() def _state_callback(self, message: JointState) -> None: - if len(message.position) != self.command_count: + adapter = getattr(self, "sdk_adapter", ProfileSdkAdapter(self.profile.command)) + state = adapter.parse_feedback(message.name, message.position) + if state is None: return - if message.name and not self.profile.command.feedback_by_index: - by_name = dict(zip((str(name) for name in message.name), message.position)) - for alias, canonical in self.profile.command.feedback_name_aliases.items(): - if alias in by_name and canonical not in by_name: - by_name[canonical] = by_name[alias] - if any(name not in by_name for name in self.command_names): - return - state = tuple(float(by_name[name]) for name in self.command_names) - else: - state = tuple(float(value) for value in message.position) - if not all(math.isfinite(value) for value in state): - return - if any( - value < self.command_lower[index] - 1.0e-6 - or value > self.command_upper[index] + 1.0e-6 + violation = next(( + (index, value) for index, value in enumerate(state) - ): - self._pause("feedback_outside_registered_command_domain") + if value < self.feedback_lower[index] + or value > self.feedback_upper[index] + ), None) + if violation is not None: + index, value = violation + # Preserve the offending observation for the operator diagnostic, + # while the pause path continues to hold the last safe command. + self.latest_state_u8 = state + self._pause( + "feedback_outside_registered_feedback_domain:" + f"channel={self.command_names[index]}:value={value:.9f}:" + f"lower={self.feedback_lower[index]:.9f}:" + f"upper={self.feedback_upper[index]:.9f}" + ) return stamp = _stamp_ns(message.header.stamp) if stamp <= 0: @@ -478,22 +569,9 @@ class L6ThreeCameraCalibrationNode(Node): self.state_history.append(StateSample(stamp, state)) self.latest_state_u8 = state self.state_receive_times.append(time.monotonic()) - step = self._current_step() - if step is not None and step.task_key is not None: - for index, actual in enumerate(state): - if index == step.command_index or index in self.step_moving_indices: - continue - expected = ( - self.step_last_command_u8[index] - if self.step_last_command_u8 is not None - else self.baseline_command[index] - ) - tolerance = float(self.non_target_motion_tolerance_u8) - if abs(actual - expected) > tolerance: - self._pause( - f"non_target_motor_moved:channel={index}:feedback={actual:.2f}" - ) - return + # Non-target motion is retained in every sample for diagnostics and + # coupling fitting. It is not a runtime stop condition: tendon hands + # legitimately back-drive neighbouring SDK coordinates. def _task(self, key: str): return next(task for task in self.profile.motion.tasks if task.key == key) @@ -620,6 +698,7 @@ class L6ThreeCameraCalibrationNode(Node): step = self._current_step() recording_this_view = bool( step is not None + and self.step_command_sent and step.recording and self._task(str(step.task_key)).view == view ) @@ -715,11 +794,48 @@ class L6ThreeCameraCalibrationNode(Node): f"fixed_base_tag_moved:{view}:drift_px={drift:.3f}" ) return - if not all(role in corners and good.get(role, False) for role in roles): + # A model may freeze a fixed palm reference before a collision- + # clearance pose intentionally occludes that Tag. The default is an + # empty mapping, so legacy L6/O6 behaviour is byte-for-byte unchanged; + # O12 supplies only its front palm pose for the affected finger tasks. + locked_reference_hook = getattr( + self, "_locked_reference_poses_for_capture", None + ) + locked_reference_poses = ( + dict(locked_reference_hook(view, step) or {}) + if locked_reference_hook is not None + else {} + ) + locked_reference_poses = { + str(role): pose + for role, pose in locked_reference_poses.items() + if str(role) in roles + } + live_roles = tuple( + role for role in roles if role not in locked_reference_poses + ) + if not all( + role in corners and good.get(role, False) + for role in live_roles + ): return stamp = _stamp_ns(message.header.stamp) - selected: dict[str, SquareTagPose] = {} - for role in roles: + selected: dict[str, SquareTagPose] = dict(locked_reference_poses) + # Optional model-specific articulated branch selector. Legacy models + # keep the exact independent selection below when the hook is absent. + pose_hook = getattr(self, "_select_articulated_capture_poses", None) + joint_selection = ( + pose_hook(view, step, live_roles, corners, stamp) + if pose_hook is not None else None + ) + if joint_selection is not None: + poses, reason = joint_selection + if poses is None: + if recording_this_view: + self._count_step_rejection(f"pnp:group:{reason}") + return + selected.update(poses) + for role in (() if joint_selection is not None else live_roles): pose, pnp_reason = self.trackers[view].estimate( role, corners[role], @@ -734,6 +850,10 @@ class L6ThreeCameraCalibrationNode(Node): ) return selected[role] = pose + evidence_hook = getattr(self, "_record_capture_pose_evidence", None) + if evidence_hook is not None: + evidence_hook(view, step, roles, corners, selected, stamp, + locked_roles=tuple(locked_reference_poses)) if recording_this_view: self.step_pnp_valid_frames += 1 self.last_view_valid_at[view] = time.monotonic() @@ -754,12 +874,16 @@ class L6ThreeCameraCalibrationNode(Node): self.step_state_sync_frames += 1 task = self._task(step.task_key) feedback = float(state_u8[task.command_index]) - lower = self.command_lower[task.command_index] - upper = self.command_upper[task.command_index] + lower = self.feedback_lower[task.command_index] + upper = self.feedback_upper[task.command_index] if not lower <= feedback <= upper: - self._count_step_rejection("feedback:outside_registered_domain") + self._count_step_rejection("feedback:outside_registered_feedback_domain") return - progress = self.profile.command.normalize(task.command_index, feedback) + progress = float(np.clip( + self.profile.command.normalize(task.command_index, feedback), + 0.0, + 1.0, + )) for joint in task.joints: measurement = self.profile.measurement.measurements[joint] parent = selected[str(measurement.parent_role)] @@ -840,14 +964,34 @@ class L6ThreeCameraCalibrationNode(Node): }) self.raw_records.append(record) append_jsonl(self.raw_path, record) + # This is a frame counter, not a joint-record counter. A frame can + # emit active and passive joint records from the same synchronized + # observation and must still contribute exactly once to the rate. self.step_valid_frames += 1 + def _guarded_detections_callback( + self, view: str, message: AprilTagDetectionArray + ) -> None: + with self.step_data_lock: + self.vision_callbacks_inflight += 1 + try: + self._detections_callback(view, message) + finally: + with self.step_data_lock: + self.vision_callbacks_inflight -= 1 + def _feedback_hz(self) -> float: if len(self.state_receive_times) < 2: return 0.0 elapsed = self.state_receive_times[-1] - self.state_receive_times[0] return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed + def _command_hz(self) -> float: + if len(self.command_publish_times) < 2: + return 0.0 + elapsed = self.command_publish_times[-1] - self.command_publish_times[0] + return 0.0 if elapsed <= 0 else (len(self.command_publish_times) - 1) / elapsed + def _publish_torque(self) -> None: if not self.commands_enabled or self.command_unit != "u8": return @@ -892,6 +1036,8 @@ class L6ThreeCameraCalibrationNode(Node): ) message.position = bounded self.command_publisher.publish(message) + self.last_published_command_u8 = tuple(bounded) + self.command_publish_times.append(time.monotonic()) def _target_command(self, step: MotionStep) -> tuple[float, ...]: if step.target_command is not None: @@ -907,6 +1053,15 @@ class L6ThreeCameraCalibrationNode(Node): return tuple(target) def _begin_step(self, step: MotionStep) -> None: + if not self._uses_isolated_motion_callbacks(): + self._begin_step_without_vision_callback(step) + return + assert self.step_data_lock is not None + with self.step_data_lock: + if not self.vision_callbacks_inflight: + self._begin_step_without_vision_callback(step) + + def _begin_step_without_vision_callback(self, step: MotionStep) -> None: now = time.monotonic() if self.commanded_speed != step.speed_u8: self._publish_speed(step.speed_u8) @@ -917,7 +1072,18 @@ class L6ThreeCameraCalibrationNode(Node): return if len(self.latest_state_u8) != self.command_count: return - self.step_start_state_u8 = tuple(float(value) for value in self.latest_state_u8) + self.step_start_feedback_u8 = tuple( + float(value) for value in self.latest_state_u8 + ) + # A radian feedback value is an observation to calibrate, not the last + # command that was sent. O12 can report a stable non-zero feedback at + # command zero, so trajectories must remain entirely in command space. + self.step_start_state_u8 = ( + tuple(self.last_published_command_u8) + if self.command_unit == "rad" + and self.last_published_command_u8 is not None + else self.step_start_feedback_u8 + ) self.step_started_at = now self.step_last_progress_at = now self.step_last_feedback = ( @@ -940,17 +1106,21 @@ class L6ThreeCameraCalibrationNode(Node): index for index, error in enumerate(errors) if error > float(self.non_target_motion_tolerance_u8) ) - self.step_last_distance_u8 = ( + command_distance = ( float(sum(errors)) if step.command_index is None else float(errors[step.command_index]) ) - self.step_initial_distance_u8 = self.step_last_distance_u8 + self.step_last_distance_u8 = ( + 0.0 if self.command_unit == "rad" else command_distance + ) + self.step_initial_distance_u8 = command_distance maximum_distance = max(errors) if self.command_unit == "rad": - self.step_trajectory_duration_seconds = ( - 0.0 if maximum_distance <= 0.0 else - math.pi * maximum_distance / (2.0 * float(step.speed_u8)) + _, _, self.step_trajectory_duration_seconds = ( + self._radian_trajectory_fraction( + maximum_distance, 0.0, float(step.speed_u8) + ) ) else: _, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8( @@ -958,6 +1128,7 @@ class L6ThreeCameraCalibrationNode(Node): float(self.command_trajectory_full_range_seconds), ) self.step_trajectory_phase = 0.0 + self.step_trajectory_blend = 0.0 self.step_last_command_u8 = None self.step_hold_since = None roles: tuple[str, ...] = () @@ -971,6 +1142,16 @@ class L6ThreeCameraCalibrationNode(Node): f"direction={step.direction}:target={step.target_u8}:attempt={step.attempt}" ) + def _radian_trajectory_fraction( + self, distance: float, elapsed: float, maximum_speed: float + ) -> tuple[float, float, float]: + """Return blend, phase and duration for the default cosine profile.""" + if distance <= 0.0: + return 1.0, 1.0, 0.0 + duration = math.pi * float(distance) / (2.0 * float(maximum_speed)) + phase = min(1.0, max(0.0, float(elapsed) / duration)) + return 0.5 - 0.5 * math.cos(math.pi * phase), phase, duration + def _advance_step_trajectory(self, step: MotionStep, now: float) -> None: if hasattr(self, "_target_command"): target_values = self._target_command(step) @@ -981,15 +1162,24 @@ class L6ThreeCameraCalibrationNode(Node): target[step.command_index] = float(step.target_u8) target_values = tuple(target) elapsed = max(0.0, float(now) - self.step_started_at) - duration = float( - getattr( - self, - "step_trajectory_duration_seconds", - self.command_trajectory_full_range_seconds, + if getattr(self, "command_unit", "u8") == "rad": + maximum_distance = max( + abs(target - start) + for start, target in zip(self.step_start_state_u8, target_values) ) - ) - phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration) - blend = 0.5 - 0.5 * math.cos(math.pi * phase) + blend, phase, duration = self._radian_trajectory_fraction( + maximum_distance, elapsed, float(step.speed_u8) + ) + else: + duration = float( + getattr( + self, + "step_trajectory_duration_seconds", + self.command_trajectory_full_range_seconds, + ) + ) + phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration) + blend = 0.5 - 0.5 * math.cos(math.pi * phase) values = [ start + (target - start) * blend for start, target in zip(self.step_start_state_u8, target_values) @@ -1001,12 +1191,21 @@ class L6ThreeCameraCalibrationNode(Node): for index, value in enumerate(values) ) self.step_trajectory_phase = phase + self.step_trajectory_blend = blend self.step_requested_u8 = ( float(np.mean(values)) if step.command_index is None else float(values[step.command_index]) ) - if command != self.step_last_command_u8: + # O12 joint feedback is request-driven: every radian JointState command + # triggers both the control write and a fresh readback. Keep streaming + # the unchanged endpoint during holds so the next step never starts + # from a stale feedback sample. Legacy byte profiles retain their + # existing duplicate suppression. + if ( + getattr(self, "command_unit", "u8") == "rad" + or command != self.step_last_command_u8 + ): self._publish_command(list(command)) self.step_last_command_u8 = command @@ -1030,9 +1229,24 @@ class L6ThreeCameraCalibrationNode(Node): gap = max((right - left for left, right in zip(bins, bins[1:])), default=256) maximum_gap = float(self.maximum_bin_gap) else: + normalized_bin_count = int( + getattr(self, "normalized_sweep_bin_count", 32) + ) + if normalized_bin_count < 32: + raise ValueError("normalized sweep bin count must be at least 32") normalized = sorted( set( - min(31, max(0, int(self.profile.command.normalize(task.command_index, value) * 32.0))) + min( + normalized_bin_count - 1, + max( + 0, + int( + self.profile.command.normalize( + task.command_index, value + ) * normalized_bin_count + ), + ), + ) for value in feedback ) ) @@ -1041,40 +1255,41 @@ class L6ThreeCameraCalibrationNode(Node): float(np.ptp([self.profile.command.normalize(task.command_index, value) for value in feedback])) if feedback.size else 0.0 ) - required_span = float(self.minimum_state_span_u8) - gap = max((right - left for left, right in zip(bins, bins[1:])), default=32) + span_policy = getattr( + self, "_required_radian_feedback_span_fraction", None + ) + required_span = ( + float(self.minimum_state_span_u8) + if span_policy is None + else float(span_policy(task, step, feedback)) + ) + gap = max( + (right - left for left, right in zip(bins, bins[1:])), + default=normalized_bin_count, + ) maximum_gap = float(self.maximum_bin_gap) observation = self._step_observation_metrics() detection_rate = float(observation["tag_detection_rate"]) joint_frame_rate = float(observation["joint_frame_rate"]) - failures = [] - if len(rows) < int(self.minimum_sweep_frames): - failures.append(f"frames={len(rows)}") - if feedback.size == 0 or span < required_span: - failures.append("feedback_span") - if len(bins) < int(self.minimum_sweep_bins): - failures.append(f"bins={len(bins)}") - if gap > maximum_gap: - failures.append(f"maximum_gap={gap}") - if detection_rate < float(self.minimum_detection_rate): - role_rates = observation["tag_detection_rate_by_role"] - worst_role = min(role_rates, key=role_rates.get, default="unknown") - tag_id = next( - ( - tag.tag_id - for view in self.profile.vision.views - for tag in view.tags - if tag.role == worst_role - ), - "?", - ) - failures.append( - f"tag_rate[ID{tag_id}/{worst_role}]={detection_rate:.3f}" - ) - if joint_frame_rate < float(self.minimum_joint_frame_rate): - failures.append(f"joint_frame_rate={joint_frame_rate:.3f}") - if self._feedback_hz() < float(self.minimum_feedback_hz): - failures.append(f"feedback_hz={self._feedback_hz():.2f}") + progress = ( + [self.profile.command.normalize(task.command_index, value) for value in feedback] + if command_unit == "rad" + else [float(value) / 255.0 for value in feedback] + ) + engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile)) + decision = engine.evaluate_sweep( + progress, + minimum_span=(required_span if command_unit == "rad" else required_span / 255.0), + total_frames=self.step_total_frames, + joint_frame_rate=joint_frame_rate, + feedback_hz=self._feedback_hz(), + detection_rate=detection_rate, + bin_count=( + int(getattr(self, "normalized_sweep_bin_count", 256)) + if command_unit == "rad" else 256 + ), + ) + failures = list(decision.failures) append_jsonl( self.raw_path, { @@ -1088,8 +1303,12 @@ class L6ThreeCameraCalibrationNode(Node): "valid_frames": len(rows), "total_frames": self.step_total_frames, "feedback_bins": len(bins), + "feedback_span": round(span, 9), + "required_feedback_span": round(required_span, 9), "maximum_bin_gap": gap, **observation, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "warnings": list(decision.warnings), "failures": failures, }, ) @@ -1099,16 +1318,22 @@ class L6ThreeCameraCalibrationNode(Node): def _retry_step(self, step: MotionStep, reason: str) -> bool: key = (str(step.task_key), int(step.cycle), str(step.direction)) retries = self.retry_counts.get(key, 0) - if retries >= int(self.automatic_sweep_retry_limit): + if not CalibrationEngine.permits_retry("sweep_acquisition", retries): return False attempt = retries + 2 self.retry_counts[key] = retries + 1 task = self._task(str(step.task_key)) start = task.start_value if step.direction == "decreasing" else task.end_value - retry_speed = float(step.speed_u8) * (0.75 if attempt == 2 else 0.5 / 0.75) + engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile)) + retry_speed = engine.retry_speed(step.speed_u8, attempt) + # Preserve model-specific motion metadata on engine-generated retry + # steps. O12 extends MotionStep with clearance/probe fields; replacing + # it with the legacy L6 class makes the next common timer tick lose + # that contract and can terminate the node at the retry boundary. + step_type = type(step) replacement = [ - MotionStep("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt), - MotionStep("sweep", step.task_key, step.command_index, step.target_u8, retry_speed, step.cycle, step.direction, attempt), + step_type("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt), + step_type("sweep", step.task_key, step.command_index, step.target_u8, retry_speed, step.cycle, step.direction, attempt), ] self.steps[self.step_index + 1:self.step_index + 1] = replacement append_jsonl( @@ -1126,6 +1351,15 @@ class L6ThreeCameraCalibrationNode(Node): return True def _finish_step(self, step: MotionStep) -> None: + if not self._uses_isolated_motion_callbacks(): + self._finish_step_without_vision_callback(step) + return + assert self.step_data_lock is not None + with self.step_data_lock: + if not self.vision_callbacks_inflight: + self._finish_step_without_vision_callback(step) + + def _finish_step_without_vision_callback(self, step: MotionStep) -> None: if step.recording: try: self._qualify_recording_step(step) @@ -1138,6 +1372,77 @@ class L6ThreeCameraCalibrationNode(Node): if self.step_index >= len(self.steps): self._finalize() + def _radian_feedback_travel(self, step: MotionStep) -> float: + """Measure motion in feedback space without comparing it to commands.""" + if ( + len(self.latest_state_u8) != self.command_count + or len(self.step_start_feedback_u8) != self.command_count + ): + return 0.0 + if step.command_index is not None: + index = int(step.command_index) + return abs( + float(self.latest_state_u8[index]) + - float(self.step_start_feedback_u8[index]) + ) + return float(sum( + abs( + float(self.latest_state_u8[index]) + - float(self.step_start_feedback_u8[index]) + ) + for index in self.step_moving_indices + )) + + def _minimum_radian_feedback_travel(self, step: MotionStep) -> float: + """Return the safety evidence required before a non-recording move ends.""" + command_distance = float(self.step_initial_distance_u8) + if command_distance <= 0.005 or step.recording: + return 0.0 + if step.phase == "preflight": + # The model-specific visual hook performs the stronger axis and + # direction check after this electrical movement evidence. + return min(0.004, 0.25 * command_distance) + # Baseline/prepare/clearance/return must make most of their requested + # move, but no absolute command-vs-feedback equality is assumed. + return 0.60 * command_distance + + def _tick_radian_motion(self, step: MotionStep, now: float) -> None: + """Advance a radian step with command/feedback domains kept separate.""" + travel = self._radian_feedback_travel(step) + if travel >= float(self.step_last_distance_u8) + 0.001: + self.step_last_distance_u8 = travel + self.step_last_progress_at = now + self.step_hold_since = None + + command_distance = float(self.step_initial_distance_u8) + commanded_travel = command_distance * self.step_trajectory_blend + if self.step_trajectory_phase < 1.0: + # Only call it a stall while the command trajectory is demanding + # meaningful motion and feedback has provided almost none. + if ( + commanded_travel > 0.02 + and travel < 0.10 * commanded_travel + and now - self.step_last_progress_at + > float(self.motor_stall_timeout_seconds) + ): + self._pause(f"mechanical_stall:{self.reason}") + return + + minimum_travel = self._minimum_radian_feedback_travel(step) + if travel + 0.001 < minimum_travel: + if ( + now - self.step_last_progress_at + > float(self.motor_stall_timeout_seconds) + ): + self._pause(f"mechanical_stall:{self.reason}") + return + + if self.step_hold_since is None: + self.step_hold_since = now + return + if now - self.step_hold_since >= float(self.endpoint_hold_seconds): + self._finish_step(step) + def _tick(self) -> None: if self.state in {"PASSED", "PAUSED", "ABORTED"}: return @@ -1168,12 +1473,14 @@ class L6ThreeCameraCalibrationNode(Node): return now = time.monotonic() self._advance_step_trajectory(step, now) - timeout = self.sweep_timeout_seconds if step.recording else self.position_timeout_seconds - if now - self.step_started_at > float(timeout): - self._pause(f"motion_timeout:{self.reason}") - return + # Absolute duration is diagnostic only. A slow but continuously + # progressing joint is valid; the two-second no-progress watchdog is + # the motion safety gate. if len(self.latest_state_u8) != self.command_count: return + if self.command_unit == "rad": + self._tick_radian_motion(step, now) + return target_state = list(self._target_command(step)) endpoint_errors = [ abs(value - target_state[index]) @@ -1220,10 +1527,6 @@ class L6ThreeCameraCalibrationNode(Node): if self.step_trajectory_phase < 1.0: self.step_hold_since = None return - task_view = None if step.task_key is None else self._task(step.task_key).view - if task_view is not None and now - self.last_view_valid_at.get(task_view, 0.0) > 0.25: - self.step_hold_since = None - return if self.step_hold_since is None: self.step_hold_since = now return @@ -1273,9 +1576,14 @@ class L6ThreeCameraCalibrationNode(Node): return self.state = "PAUSED" self.reason = str(reason) - # Holding the latest position is safer than issuing an automatic move - # after a stall, unexpected motor motion, or moved palm reference. - if len(self.latest_state_u8) == self.command_count: + # In radian mode, hold the last command rather than feeding an + # uncalibrated feedback value back into the command domain. + if ( + self.command_unit == "rad" + and self.last_published_command_u8 is not None + ): + self._publish_command(list(self.last_published_command_u8)) + elif len(self.latest_state_u8) == self.command_count: self._publish_command( [ int(np.clip(round(value), 0, 255)) @@ -1283,7 +1591,32 @@ class L6ThreeCameraCalibrationNode(Node): for value in self.latest_state_u8 ] ) - append_jsonl(self.raw_path, {"kind": "paused", "reason": self.reason}) + step = self._current_step() + target = [] if step is None else list(self._target_command(step)) + errors = ( + [] + if len(target) != len(self.latest_state_u8) + else [ + abs(float(actual) - float(expected)) + for actual, expected in zip(self.latest_state_u8, target) + ] + ) + append_jsonl(self.raw_path, { + "kind": "paused", + "reason": self.reason, + "command_unit": self.command_unit, + f"latest_state_{self.command_unit}": list(self.latest_state_u8), + f"target_state_{self.command_unit}": target, + f"channel_errors_{self.command_unit}": errors, + "maximum_error_channel": ( + None + if not errors + else self.command_names[int(np.argmax(errors))] + ), + f"maximum_error_{self.command_unit}": ( + None if not errors else max(errors) + ), + }) def _status(self) -> dict[str, Any]: step = self._current_step() @@ -1305,16 +1638,19 @@ class L6ThreeCameraCalibrationNode(Node): ] step_fraction = 0.0 if step is not None and math.isfinite(feedback): - current_distance = ( - float(sum(channel_errors)) - if step.command_index is None - else channel_errors[step.command_index] - ) - initial_distance = self.step_initial_distance_u8 - if math.isfinite(initial_distance) and initial_distance > 0.0: - step_fraction = 1.0 - current_distance / initial_distance - elif current_distance <= float(self.endpoint_tolerance_u8): - step_fraction = 1.0 + if self.command_unit == "rad": + step_fraction = self.step_trajectory_blend + else: + current_distance = ( + float(sum(channel_errors)) + if step.command_index is None + else channel_errors[step.command_index] + ) + initial_distance = self.step_initial_distance_u8 + if math.isfinite(initial_distance) and initial_distance > 0.0: + step_fraction = 1.0 - current_distance / initial_distance + elif current_distance <= float(self.endpoint_tolerance_u8): + step_fraction = 1.0 step_fraction = float(np.clip(step_fraction, 0.0, 1.0)) elif self.state == "PASSED": step_fraction = 1.0 @@ -1393,6 +1729,7 @@ class L6ThreeCameraCalibrationNode(Node): self.fixed_base_maximum_corner_drift_px ), "feedback_hz": round(self._feedback_hz(), 3), + "command_publish_hz": round(self._command_hz(), 3), "camera_info_views": sorted(self.camera_matrices), "state_publisher_count": self.count_publishers(self.state_topic), "command_publisher_count": self.count_publishers(self.command_topic), diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py index fd67428..e66d348 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py @@ -8,6 +8,8 @@ import math from pathlib import Path from typing import Any, Mapping, Sequence +from ...runtime.engine import CalibrationEngine + from .artifacts import ( artifact_hashes, atomic_write_json, @@ -23,6 +25,7 @@ from .profile import ( MEASURED_PASSIVE_JOINTS, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, ) from .urdf import L6UrdfCorrection, write_l6_corrected_urdf @@ -148,6 +151,13 @@ def finalize_l6_session( accepted_records_by_joint(records), require_thumb_axis_zero=True, ) + CalibrationEngine(build_typed_profile()).result_from_fit( + result, + transfers={ + **TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + **TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + }, + ) # Validate the complete runtime schema before materializing any corrected # URDF. A fit/schema rejection therefore leaves only the node's failure # diagnostic and the immutable raw samples. diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py index 10d2770..580acdd 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py @@ -272,8 +272,8 @@ def build_typed_profile() -> CalibrationProfile: ), motion=MotionPolicy( tasks=tasks, - precheck_sweeps=True, - steady_command_checkpoints=True, + precheck_sweeps=False, + steady_command_checkpoints=False, speed_parameters={ "preflight_u8": 1, "formal_u8": 1, @@ -313,7 +313,6 @@ def build_typed_profile() -> CalibrationProfile: { "minimum_detection_rate", "maximum_state_image_skew_ms", - "maximum_hysteresis_rad", "maximum_validation_error_rad", "maximum_mimic_residual_rad", } diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py index 043626f..8fcda1e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py @@ -33,6 +33,7 @@ _STATE_LABELS = { "WAIT_DEVICES": "等待六通道反馈和三相机内参", "READY": "设备就绪", "RUNNING": "标定中", + "FINALIZING": "拟合、验证并生成 URDF", "PASSED": "通过", "PAUSED": "已暂停", "ABORTED": "已中止", @@ -47,6 +48,7 @@ _PHASE_LABELS = { "preflight": "任务运动预检", "prepare": "扫描起点准备", "retry_prepare": "自动重扫起点准备", + "resume_prepare": "断点恢复起点准备", "sweep": "正式扫描", "clearance": "手指避让", "clearance_outer": "小指/无名指避让", @@ -111,12 +113,6 @@ def _l6_reason_zh( "查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate;先处理遮挡或" "反馈采样问题,再重新开始。程序已禁止发布本次结果。", ) - if reason.startswith("non_target_motor_moved:"): - return ( - "MOTION-NONTARGET-304", - "扫描期间检测到非目标电机离开保持位置。", - "停止其他控制节点并检查机械耦合或反馈通道顺序,确认后重新开始。", - ) if reason.startswith("multiple_state_publishers:"): count = status.get("state_publisher_count", "?") return ( @@ -275,28 +271,38 @@ def render_six_channel_progress_zh( ) lines.append( f"运动:峰值 {float(speed_rad_s):.3f} rad/s;" - f"本段 {trajectory_seconds:.1f} 秒余弦轨迹" + f"本段 {trajectory_seconds:.1f} 秒平滑限速轨迹" ) - latest_state = status.get("latest_state_u8", []) + latest_state = status.get(f"latest_state_{command_unit}", []) command_names = status.get("command_names", []) if ( isinstance(latest_state, (list, tuple)) and isinstance(command_names, (list, tuple)) - and len(latest_state) == len(command_names) == 6 + and len(latest_state) == len(command_names) + and len(command_names) in {6, 12} and (phase == "baseline" or state in {"PAUSED", "ABORTED"}) ): + digits = 3 if command_unit == "rad" else 1 + suffix = " rad" if command_unit == "rad" else "" feedback_text = ", ".join( - f"{name}={float(value):.1f}" + f"{name}={float(value):.{digits}f}{suffix}" for name, value in zip(command_names, latest_state) ) maximum_error_channel = status.get("maximum_error_channel") - maximum_error = status.get("maximum_error_u8") + maximum_error = status.get(f"maximum_error_{command_unit}") error_text = ( "未知" if maximum_error_channel is None or maximum_error is None - else f"{maximum_error_channel}={float(maximum_error):.1f}" + else ( + f"{maximum_error_channel}=" + f"{float(maximum_error):.{digits}f}{suffix}" + ) + ) + channel_label = "六路" if len(command_names) == 6 else "十二路" + error_label = "最大命令/反馈差" if command_unit == "rad" else "最大偏差" + lines.append( + f"{channel_label}反馈:{feedback_text} {error_label}:{error_text}" ) - lines.append(f"六路反馈:{feedback_text} 最大偏差:{error_text}") if state in {"PAUSED", "ABORTED"}: _code, problem, suggestion = reason_renderer(status) lines.extend((f"原因:{problem}", f"建议:{suggestion}")) @@ -367,6 +373,7 @@ def _launch_command( record_bag: bool, commands_enabled: bool, sdk_startup_speed_u8: int = 1, + resume_from: Path | None = None, ) -> list[str]: arguments = { "model": config.model, @@ -393,6 +400,10 @@ def _launch_command( "commands_enabled": str(commands_enabled).lower(), "record_bag": str(record_bag).lower(), } + if resume_from is not None: + arguments["resume_raw_samples_path"] = str( + resume_from / "raw_samples.jsonl" + ) # HCAN/ZLG vendor products do not have a Linux SocketCAN interface. # Omitting the launch override also avoids the invalid token # ``can_interface:=`` when the reviewed product value is intentionally empty. diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py index b5edc00..301570f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py @@ -2,14 +2,21 @@ from __future__ import annotations +from dataclasses import asdict import math from pathlib import Path from typing import Any, Mapping, Sequence import xml.etree.ElementTree as ET import numpy as np +from scipy.spatial.transform import Rotation from .fitting import O12FitResult, curve_input_knots_rad, curve_values_at_rad +from .kinematics import ( + PASSIVE_POLYNOMIAL_BY_JOINT, + PASSIVE_SDK_SOURCE_BY_JOINT, + vendor_passive_curve, +) from .profile import ( ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, @@ -22,10 +29,14 @@ from .profile import ( SAFE_UPPER_RAD, SDK_LOWER_RAD, SDK_TO_URDF_JOINT, + SDK_TO_URDF_SIGN, SDK_UPPER_RAD, + STATIC_ZERO_EXCLUDED_JOINTS, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, build_typed_profile, ) +from .urdf import o12_endpoint_mimic_contract +from .zero import SPATIAL_ZERO_POLICY ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS) @@ -102,48 +113,107 @@ def build_o12_runtime_payload( passed: bool, ) -> dict[str, Any]: profile = build_typed_profile() + if result.full_hand_zero_result is not None and not result.full_hand_zero_result.passed: + raise ValueError("O12 full-hand spatial zero validation has not passed") + for name in STATIC_ZERO_EXCLUDED_JOINTS: + if ( + not math.isclose( + float(result.zero_offsets_rad.get(name, math.nan)), + 0.0, + rel_tol=0.0, + abs_tol=1.0e-12, + ) + or result.zero_method_by_joint.get(name) + != "source_cad_zero_profile_excluded" + ): + raise ValueError( + f"O12 {name} must retain immutable source-CAD static zero" + ) source = _source_joint_metadata(source_urdf) + endpoint_mimics = o12_endpoint_mimic_contract(source_urdf, result) curves: dict[str, tuple[np.ndarray, np.ndarray, np.ndarray, list[float]]] = {} joints: dict[str, dict[str, Any]] = {} for name in ACTIVE_JOINTS: donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) fit = result.curves[donor] - inputs = list(curve_input_knots_rad(donor)) + feedback_domain = result.feedback_domains_rad[donor] + inputs = list(curve_input_knots_rad( + donor, feedback_domain_rad=feedback_domain + )) + motor = COMMAND_INDEX_BY_JOINT[name] + sign = float(SDK_TO_URDF_SIGN[motor]) decreasing = _zeroed( - curve_values_at_rad(donor, fit, inputs, "decreasing_rad"), inputs + curve_values_at_rad( + donor, fit, inputs, "decreasing_rad", feedback_domain + ), inputs ) increasing = _zeroed( - curve_values_at_rad(donor, fit, inputs, "increasing_rad"), inputs + curve_values_at_rad( + donor, fit, inputs, "increasing_rad", feedback_domain + ), inputs ) angle = 0.5 * (decreasing + increasing) + # fit_rotation_joint_curve has an arbitrary SVD axis sign. Apply the + # reviewed SDK-to-CAD direction contract after fitting so all curves + # use the target URDF convention deterministically. + observed_direction = float(np.sign(angle[-1] - angle[0])) + if observed_direction and observed_direction != sign: + angle = -angle + decreasing = -decreasing + increasing = -increasing + if donor != name: + cad_lower = float(source[name]["lower"]) + cad_upper = float(source[name]["upper"]) + # Same feedback means the same transferred correction until the + # ring's own CAD/mimic range saturates. Rescaling all donor values + # would invent a different gain for this unobserved finger. + angle = np.clip(angle, cad_lower, cad_upper) + decreasing = np.clip(decreasing, cad_lower, cad_upper) + increasing = np.clip(increasing, cad_lower, cad_upper) curves[name] = (angle, decreasing, increasing, inputs) - motor = COMMAND_INDEX_BY_JOINT[name] joint: dict[str, Any] = { "urdf_joint": name, "sdk_channel": COMMAND_NAMES[motor], "motor_index": motor, "passive": False, - "calibration_status": profile.joint_coverage[name], + "calibration_status": ( + "transferred_static_dynamic" if donor != name + else "measured_dynamic_cad_static" + if str(result.zero_method_by_joint[name]).startswith("source_cad_zero") + else "measured_static_dynamic" + ), "curve_input_knots_rad": _round(inputs), "angle_rad": _round(angle), "decreasing_rad": _round(decreasing), "increasing_rad": _round(increasing), "zero_feedback_rad": 0.0, - "urdf_sign_adapter": ( - "sdk_negative_to_urdf_positive" - if SDK_UPPER_RAD[motor] <= 0.0 and SDK_LOWER_RAD[motor] < 0.0 - else "identity" + "measured_feedback_domain_rad": [ + round(float(value), 10) for value in feedback_domain + ], + "static_urdf_origin_offset_rad": round( + float(result.zero_offsets_rad[name]), 10 + ), + "static_zero_method": str(result.zero_method_by_joint[name]), + "sdk_to_urdf_direction": sign, + "urdf_sign_adapter": "negate" if sign < 0.0 else "identity", + "runtime_mapping_source": "tag_rotation_over_sdk_feedback_rad", + "visual_arc_diagnostic_rad": round( + float(result.visual_arc_diagnostics_rad[donor]), 10 ), "raw_increasing_curve_branch": ( "decreasing" - if _task_for_joint(donor).end_value > _task_for_joint(donor).start_value + if _task_for_joint(donor).end_value + > _task_for_joint(donor).start_value else "increasing" ), } if donor != name: joint["transferred_from_joint"] = donor - joint["transfer_policy"] = "pinky_feedback_correction_on_ring_cad" + joint["transfer_policy"] = ( + "pinky_curve_clamped_to_ring_cad_range" + ) + joint["static_zero_transfer_policy"] = "donor_offset_on_own_cad_frame" joints[name] = joint for name in PASSIVE_JOINTS: @@ -152,20 +222,33 @@ def build_o12_runtime_payload( motor = _motor_index(source_name) if name in MEASURED_PASSIVE_JOINTS: fit = result.curves[name] - inputs = list(curve_input_knots_rad(name)) - decreasing = _zeroed( - curve_values_at_rad(name, fit, inputs, "decreasing_rad"), inputs - ) + float(metadata["offset"]) - increasing = _zeroed( - curve_values_at_rad(name, fit, inputs, "increasing_rad"), inputs - ) + float(metadata["offset"]) - angle = 0.5 * (decreasing + increasing) - coupling = result.mimic_fits[name] - coefficients = [float(metadata["offset"]), *coupling.coefficients] + feedback_domain = result.feedback_domains_rad[name] + inputs = list(curve_input_knots_rad( + name, feedback_domain_rad=feedback_domain + )) + sdk_source = PASSIVE_SDK_SOURCE_BY_JOINT[name] + sdk_motor = _motor_index(sdk_source) + expected_direction = float(SDK_TO_URDF_SIGN[sdk_motor]) + vendor_values = np.asarray( + vendor_passive_curve( + name, + inputs, + sdk_to_urdf_sign=expected_direction, + ), + dtype=float, + ) + angle = vendor_values + decreasing = vendor_values.copy() + increasing = vendor_values.copy() + observed = result.mimic_fits[name] + coefficients = [ + expected_direction * float(value) + for value in PASSIVE_POLYNOMIAL_BY_JOINT[name] + ] coefficients.extend([0.0] * (6 - len(coefficients))) - coupling_model = coupling.model - multiplier = coupling.urdf_mimic_multiplier - policy = coupling.urdf_mimic_policy + coupling_model = "vendor_o12_polynomial" + multiplier = endpoint_mimics[name] + policy = "source_cad_closed_endpoint" status = profile.joint_coverage[name] else: source_curve, source_dec, source_inc, inputs = curves[source_name] @@ -179,14 +262,14 @@ def build_o12_runtime_payload( policy = "cad_nominal_preserved" status = profile.joint_coverage[name] curves[name] = (angle, decreasing, increasing, list(inputs)) - joints[name] = { + joint = { "urdf_joint": name, "sdk_channel": COMMAND_NAMES[motor], "motor_index": motor, "passive": True, "source_joint": source_name, "calibration_status": status, - "static_zero_policy": "cad_preserved_tag_mount_ambiguous", + "static_zero_policy": "cad_mechanical_endpoint", "curve_input_knots_rad": _round(inputs), "angle_rad": _round(angle), "decreasing_rad": _round(decreasing), @@ -195,12 +278,37 @@ def build_o12_runtime_payload( "coupling_coefficients": [round(float(v), 10) for v in coefficients], "mimic_multiplier": round(float(multiplier), 10), "urdf_mimic_policy": policy, + "runtime_mapping_source": ( + "o12_vendor_solver_over_sdk_feedback_rad" + if name in MEASURED_PASSIVE_JOINTS + else "source_urdf_cad_mimic" + ), "raw_increasing_curve_branch": ( "decreasing" - if _task_for_joint(name).end_value > _task_for_joint(name).start_value + if _task_for_joint(name).end_value + > _task_for_joint(name).start_value else "increasing" ), } + if name in MEASURED_PASSIVE_JOINTS: + joint.update({ + "measured_feedback_domain_rad": [ + round(float(value), 10) for value in feedback_domain + ], + "visual_observation_role": "independent_dynamic_validation", + "vendor_sdk_source_joint": sdk_source, + "observed_coupling_model": observed.model, + "observed_mimic_multiplier": round( + float(observed.urdf_mimic_multiplier), 10 + ), + "observed_residual_rms_rad": round( + float(observed.residual_rms_rad), 10 + ), + "observed_visual_arc_rad": round( + float(result.visual_arc_diagnostics_rad[name]), 10 + ), + }) + joints[name] = joint errors = np.abs(np.concatenate([ np.asarray(values, dtype=float) @@ -213,7 +321,7 @@ def build_o12_runtime_payload( "model": "O12", "side": "right", "serial_number": str(serial_number), - "calibration_scope": "full", + "calibration_scope": "full_dynamic_except_thumb_mcp_static_zero", "publication_pointer": "latest_passed", "angle_unit": "rad", "curve_input_domain": "feedback_rad", @@ -243,8 +351,61 @@ def build_o12_runtime_payload( "validation_mae_rad": round(float(np.mean(errors)), 10), "validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10), "validation_max_rad": round(float(np.max(errors)), 10), + "roll_cross_view": { + joint: { + key: round(float(value), 10) + for key, value in metrics.items() + } + for joint, metrics in result.cross_view_roll_metrics.items() + }, + "thumb_root_spatial_zero": ( + None + if result.thumb_root_zero_result is None + else { + "offsets_rad": { + name: round(float(value), 10) + for name, value in result.thumb_root_zero_result.direct_offsets_rad.items() + }, + "validation_mae_rad": round(float(np.mean(np.abs( + result.thumb_root_zero_result.validation_errors_rad + ))), 10), + "validation_p95_rad": round(float(np.percentile(np.abs( + result.thumb_root_zero_result.validation_errors_rad + ), 95.0)), 10), + "validation_max_rad": round(float(np.max(np.abs( + result.thumb_root_zero_result.validation_errors_rad + ))), 10), + "axis_line_rms_m_diagnostic": round(float( + result.thumb_root_zero_result.axis_line_rms_m + ), 10), + "method": "roll_yaw_pitch_axes_with_pinky_palm_orientation", + "validation_scope": "root_axis_angles_not_full_chain_position", + } + ), "ring_transfer_source": "pinky_mcp_pitch", + "full_hand_spatial_zero": ( + None if result.full_hand_zero_result is None else { + **asdict(result.full_hand_zero_result), + "policy": SPATIAL_ZERO_POLICY, + "validation_scope": "active_zero_axis_geometry_not_fingertip_contact", + } + ), + "cad_static_joints_not_measured": [ + name for name, method in result.zero_method_by_joint.items() + if str(method).startswith("source_cad_zero") + ], + "static_zero_exclusions": { + name: { + "static_origin_source": "immutable_source_cad", + "reason": "downstream_curved_shell_tag_not_absolute_axis_line_datum", + "dynamic_curve_and_holdout_required": True, + } + for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS) + }, "ring_cad_geometry_and_mimic_preserved": True, + "active_angle_source": "tag_rotation_over_sdk_feedback_rad", + "passive_runtime_source": "o12_vendor_solver_polynomial", + "visual_measurement_role": "active_curve_and_passive_validation", }, } validate_o12_runtime_payload(payload) @@ -306,8 +467,149 @@ def validate_o12_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError(f"{name} has invalid O12 motor binding") if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}: raise ValueError(f"{name} has invalid O12 direction binding") + for name in ACTIVE_JOINTS: + joint = joints[name] + motor = COMMAND_INDEX_BY_JOINT[name] + sign = float(SDK_TO_URDF_SIGN[motor]) + offset = float(joint.get("static_urdf_origin_offset_rad", math.nan)) + if not math.isfinite(offset) or abs(offset) > math.radians(20.0): + raise ValueError(f"{name} has invalid O12 static origin offset") + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + differences = np.diff(np.asarray(joint[field], dtype=float)) + if np.any(sign * differences < -1.0e-7): + raise ValueError( + f"{name}.{field} disagrees with fixed O12 SDK/URDF direction" + ) + for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): + if not math.isclose( + float(joints[target]["static_urdf_origin_offset_rad"]), + float(joints[donor]["static_urdf_origin_offset_rad"]), + rel_tol=0.0, abs_tol=1.0e-9, + ): + raise ValueError(f"{target} static zero differs from transfer donor {donor}") + for name in PASSIVE_JOINTS: + joint = joints[name] + source_joint = str(joint.get("source_joint", "")) + if source_joint != MIMIC_SOURCE_BY_JOINT[name]: + raise ValueError(f"{name} has invalid O12 mimic source") + multiplier = float(joint.get("mimic_multiplier", math.nan)) + coefficients = np.asarray( + joint.get("coupling_coefficients", ()), dtype=float + ) + if coefficients.shape != (6,) or not np.all(np.isfinite(coefficients)): + raise ValueError(f"{name} has invalid O12 CAD mimic coefficients") + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"{name} has invalid O12 mimic multiplier") + if name not in MEASURED_PASSIVE_JOINTS: + offset = float(coefficients[0]) + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + expected_values = offset + multiplier * np.asarray( + joints[source_joint][field], dtype=float + ) + if not np.allclose( + np.asarray(joint[field], dtype=float), + expected_values, + rtol=0.0, + atol=2.0e-9, + ): + raise ValueError( + f"{name}.{field} differs from the source CAD mimic chain" + ) if not bool(payload["quality"].get("passed")): raise ValueError("failed O12 calibration cannot be published") -__all__ = ["ALL_REVOLUTE_JOINTS", "build_o12_runtime_payload", "validate_o12_runtime_payload"] +def validate_o12_runtime_payload_against_urdf( + payload: Mapping[str, Any], urdf: str | Path, + *, source_urdf: str | Path | None = None, +) -> None: + """Check curve bounds/mimics and, with original CAD, actual static frames. + + This verifies the artifact contract, not full-hand physical accuracy. + Passive runtime polynomials and linear URDF mimic are distinct models. + """ + validate_o12_runtime_payload(payload) + metadata = _source_joint_metadata(urdf) + joints = payload["joints"] + for name, joint in joints.items(): + expected = metadata[name] + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(joint[field], dtype=float) + allowance = 0.0 + if bool(joint.get("passive")): + source_name = str(expected["source_joint"]) + if source_name != str(joint["source_joint"]): + raise ValueError( + f"{name} corrected URDF mimic source differs from payload" + ) + if not math.isclose( + float(expected["multiplier"]), + float(joint["mimic_multiplier"]), + rel_tol=0.0, + abs_tol=2.0e-9, + ): + raise ValueError( + f"{name} corrected URDF mimic differs from payload" + ) + if name not in MEASURED_PASSIVE_JOINTS: + source_values = np.asarray( + joints[source_name][field], dtype=float + ) + cad_values = ( + float(expected["offset"]) + + float(expected["multiplier"]) * source_values + ) + if not np.allclose( + values, cad_values, rtol=0.0, atol=2.0e-9 + ): + raise ValueError( + f"{name}.{field} differs from preserved CAD mimic" + ) + allowance = max( + 0.0, + float(np.max(cad_values)) - float(expected["upper"]), + float(expected["lower"]) - float(np.min(cad_values)), + ) + if ( + float(np.min(values)) + < float(expected["lower"]) - allowance - 2.0e-9 + ): + raise ValueError(f"{name}.{field} is below the URDF physical limit") + if ( + float(np.max(values)) + > float(expected["upper"]) + allowance + 2.0e-9 + ): + raise ValueError(f"{name}.{field} is above the URDF physical limit") + if source_urdf is not None: + original = ET.parse(source_urdf).getroot() + corrected = ET.parse(urdf).getroot() + for name in ACTIVE_JOINTS: + before = original.find(f"joint[@name='{name}']") + after = corrected.find(f"joint[@name='{name}']") + if before is None or after is None: + raise ValueError(f"{name} missing from static-frame validation") + b_origin, a_origin = before.find("origin"), after.find("origin") + if b_origin is None or a_origin is None: + raise ValueError(f"{name} missing static origin") + if b_origin.get("xyz") != a_origin.get("xyz"): + raise ValueError(f"{name} static origin translation changed") + axis_node = before.find("axis") + if axis_node is None: + raise ValueError(f"{name} missing CAD axis") + axis = np.asarray([float(v) for v in axis_node.get("xyz").split()]) + axis /= np.linalg.norm(axis) + b_rot = Rotation.from_euler("xyz", [float(v) for v in b_origin.get("rpy", "0 0 0").split()]) + a_rot = Rotation.from_euler("xyz", [float(v) for v in a_origin.get("rpy", "0 0 0").split()]) + expected_rot = b_rot * Rotation.from_rotvec( + axis * float(joints[name]["static_urdf_origin_offset_rad"]) + ) + if (expected_rot.inv() * a_rot).magnitude() > 1.0e-8: + raise ValueError(f"{name} static origin disagrees with payload") + + +__all__ = [ + "ALL_REVOLUTE_JOINTS", + "build_o12_runtime_payload", + "validate_o12_runtime_payload", + "validate_o12_runtime_payload_against_urdf", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py index 0586a85..6271f2d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py @@ -2,26 +2,44 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, replace import math from pathlib import Path from typing import Any, Mapping, Sequence import numpy as np -from ..g20.profile import JointCurveFit -from ..g20.zero_solver import fit_rotation_joint_curve, rotation_curve_holdout_errors +from ..g20.profile import ( + HandCalibrationProfile, + JointCurveFit, + JointSpec, + cross_view_roll_diagnostic_metrics, +) +from ..g20.zero_solver import ( + ZeroCalibrationProfile, + ZeroSolveResult, + fit_joint_axis_measurement, + fit_rotation_joint_curve, + rotation_curve_holdout_errors, + solve_urdf_zero_offsets, + with_depth_free_axis_projection, +) from ..l6.fitting import MimicFit, fit_coupling_model from .profile import ( CALIBRATED_ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, + COMMAND_NAMES, + GEOMETRIC_ZERO_JOINTS, + ROOT_GEOMETRIC_ZERO_JOINTS, MEASURED_PASSIVE_JOINTS, MIMIC_SOURCE_BY_JOINT, + SDK_TO_URDF_JOINT, + SDK_TO_URDF_SIGN, + STATIC_ZERO_EXCLUDED_JOINTS, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, build_typed_profile, ) - @dataclass(frozen=True) class O12FitResult: curves: Mapping[str, JointCurveFit] @@ -29,6 +47,86 @@ class O12FitResult: travels_rad: Mapping[str, float] mimic_fits: Mapping[str, MimicFit] holdout_errors_rad: Mapping[str, tuple[float, ...]] + cross_view_roll_metrics: Mapping[str, Mapping[str, float]] + visual_arc_diagnostics_rad: Mapping[str, float] + feedback_domains_rad: Mapping[str, tuple[float, float]] + zero_method_by_joint: Mapping[str, str] + thumb_root_zero_result: ZeroSolveResult | None + full_hand_zero_result: ZeroSolveResult | None = None + + +O12_THUMB_ROOT_AXIS_JOINTS: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "pinky_mcp_pitch", +) + + +def _thumb_root_zero_profile() -> ZeroCalibrationProfile: + """Declare the spatial graph that observes O12 thumb roll/yaw zeros. + + A relative Tag rotation observes roll travel but not its constant phase. + The downstream yaw screw axis rotates with that phase. The independently + observed pinky MCP axis fixes palm orientation, leaving roll as the only + fitted roll coordinate. The pitch axis in turn observes yaw phase. + Parallel pitch/MCP axes do not identify their own phase from direction; + those origins retain CAD values instead of inferring zeros from travel. + """ + specs = { + name: JointSpec( + name, + COMMAND_INDEX_BY_JOINT[name], + True, + None, + None, + None, + pose_axis_line_required=False, + ) + for name in O12_THUMB_ROOT_AXIS_JOINTS + } + hand = HandCalibrationProfile( + side="right", + reference_finger="pinky", + view_tags={}, + preflight_view_roles={}, + joint_specs=specs, + sweep_specs=(), + image_trajectory_joints=frozenset(), + roll_clearance_commands={}, + thumb_pitch_clearance_commands={}, + layout_id="o12_right_16", + model="O12", + command_names=COMMAND_NAMES, + baseline_command=(255,) * len(COMMAND_NAMES), + directional_zero=True, + isolated_holdout=True, + # As in the G20 multi-view solver, a repeatable planar-PnP cone bias + # is diagnostic rather than a zero-phase failure. Acceptance still + # requires the per-cycle cone magnitude to be stable, the fitted axis + # lines to pass, and the independent holdout to improve. + stable_cross_view_cone_bias=True, + ) + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=("thumb_cmc_roll", "thumb_cmc_yaw"), + axis_joints=O12_THUMB_ROOT_AXIS_JOINTS, + inherited_zero_joints={}, + inherited_static_zero_joints={}, + constrained_circle_joints=frozenset(O12_THUMB_ROOT_AXIS_JOINTS), + root_anchor_joints=frozenset({"thumb_cmc_roll"}), + axis_parent_joint={"thumb_cmc_yaw": "thumb_cmc_roll", "thumb_cmc_pitch": "thumb_cmc_yaw"}, + phase_parent_joint={}, + offset_observer_joint={"thumb_cmc_roll": "thumb_cmc_yaw", "thumb_cmc_yaw": "thumb_cmc_pitch"}, + same_view_axis_pair_by_offset={}, + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + base_pose_strategy="thumb_serial", + orientation_anchor_joint="pinky_mcp_pitch", + directed_base_axis_joints=frozenset({ + "thumb_cmc_roll", "pinky_mcp_pitch", + }), + ) def _task_by_joint() -> dict[str, Any]: @@ -39,51 +137,168 @@ def _task_by_joint() -> dict[str, Any]: } -def feedback_rad_to_curve_index(joint: str, feedback_rad: float) -> float: - """Map a physical feedback angle to the fitter's normalized 255..0 axis.""" +def feedback_rad_to_curve_index( + joint: str, + feedback_rad: float, + feedback_domain_rad: tuple[float, float] | None = None, +) -> float: + """Map measured feedback to the fitter's normalized 255..0 axis.""" task = _task_by_joint()[str(joint)] - denominator = task.end_value - task.start_value + if feedback_domain_rad is None: + start, end = float(task.start_value), float(task.end_value) + else: + lower, upper = (float(value) for value in feedback_domain_rad) + if task.end_value >= task.start_value: + start, end = lower, upper + else: + start, end = upper, lower + denominator = end - start if abs(denominator) <= 1.0e-12: raise ValueError(f"O12 task {task.key} has a degenerate range") - phase = (float(feedback_rad) - task.start_value) / denominator + phase = (float(feedback_rad) - start) / denominator return 255.0 * (1.0 - float(np.clip(phase, 0.0, 1.0))) -def curve_input_knots_rad(joint: str, count: int = 65) -> tuple[float, ...]: +def curve_input_knots_rad( + joint: str, + count: int = 65, + feedback_domain_rad: tuple[float, float] | None = None, +) -> tuple[float, ...]: task = _task_by_joint()[str(joint)] + if feedback_domain_rad is None: + lower, upper = sorted((task.start_value, task.end_value)) + else: + lower, upper = sorted(float(value) for value in feedback_domain_rad) + # Zero is the open/centred runtime command. Include it in the public + # knot domain while clamping the tiny unobserved offset to the measured + # endpoint instead of extrapolating a full SDK command range. + lower, upper = min(0.0, lower), max(0.0, upper) return tuple( float(value) for value in np.linspace( - min(task.start_value, task.end_value), - max(task.start_value, task.end_value), + lower, + upper, int(count), ) ) def curve_values_at_rad( - joint: str, fit: JointCurveFit, inputs_rad: Sequence[float], branch: str + joint: str, + fit: JointCurveFit, + inputs_rad: Sequence[float], + branch: str, + feedback_domain_rad: tuple[float, float] | None = None, ) -> tuple[float, ...]: values = np.asarray(getattr(fit, branch), dtype=float) indices = np.arange(256, dtype=float) return tuple( - float(np.interp(feedback_rad_to_curve_index(joint, value), indices, values)) + float(np.interp( + feedback_rad_to_curve_index(joint, value, feedback_domain_rad), + indices, + values, + )) for value in inputs_rad ) def _virtual_records( - joint: str, rows: Sequence[Mapping[str, Any]] + joint: str, + rows: Sequence[Mapping[str, Any]], + feedback_domain_rad: tuple[float, float] | None = None, + feedback_domains_rad: Mapping[str, tuple[float, float]] | None = None, ) -> list[dict[str, Any]]: result = [] for source in rows: row = dict(source) - curve_index = feedback_rad_to_curve_index(joint, float(row["feedback_rad"])) + curve_index = feedback_rad_to_curve_index( + joint, float(row["feedback_rad"]), feedback_domain_rad + ) row["command_u8"] = int(np.clip(round(curve_index), 0, 255)) + if "state_rad" in row: + state_rad = tuple(float(value) for value in row["state_rad"]) + if len(state_rad) != len(SDK_TO_URDF_JOINT): + raise ValueError("O12 state_rad has the wrong channel count") + state_u8 = [] + for channel, value in enumerate(state_rad): + state_joint = SDK_TO_URDF_JOINT[channel] + state_joint = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get( + state_joint, state_joint + ) + state_u8.append( + feedback_rad_to_curve_index( + state_joint, + value, + (feedback_domains_rad or {}).get(state_joint), + ) + ) + row["state_u8"] = state_u8 result.append(row) return result +def _measured_feedback_domain( + joint: str, rows: Sequence[Mapping[str, Any]] +) -> tuple[float, float]: + """Return the full-stroke feedback interval established by cycle zero.""" + values = [ + float(row["feedback_rad"]) + for row in rows + if int(row.get("cycle", -1)) == 0 + and math.isfinite(float(row.get("feedback_rad", math.nan))) + ] + if not values or max(values) - min(values) <= 1.0e-9: + raise ValueError(f"{joint} cycle 0 has no measurable feedback travel") + return float(min(values)), float(max(values)) + + +def _validate_cycle_repeatability( + joint: str, + rows: Sequence[Mapping[str, Any]], + feedback_domain_rad: tuple[float, float], +) -> None: + """Require later cycles to repeat measured travel, not command scale.""" + reference = feedback_domain_rad[1] - feedback_domain_rad[0] + for cycle in (1, 2, 3): + values = [ + float(row["feedback_rad"]) + for row in rows + if int(row.get("cycle", -1)) == cycle + and math.isfinite(float(row.get("feedback_rad", math.nan))) + ] + span = max(values) - min(values) if values else 0.0 + if span < 0.90 * reference: + raise ValueError( + f"{joint} cycle {cycle} repeats only " + f"{span / reference:.3f} of cycle-zero measured travel" + ) + + +def _uniform_curve_holdout_errors( + rows: Sequence[Mapping[str, Any]], errors: Sequence[float] +) -> tuple[float, ...]: + """Give every observed feedback bin equal holdout weight. + + O12 motor feedback can saturate before a passive tendon joint has fully + settled. A camera then records many frames at one identical feedback bin. + The curve fitter already uses one median per bin; validation must use the + same curve-space measure instead of letting endpoint dwell duration + multiply the P95 weight of that single coordinate. + """ + if len(rows) != len(errors): + raise ValueError("O12 holdout rows and errors have different lengths") + grouped: dict[tuple[str, int], list[float]] = {} + for row, error in zip(rows, errors): + grouped.setdefault( + (str(row["direction"]), int(row["command_u8"])), [] + ).append(float(error)) + if not grouped: + raise ValueError("O12 holdout has no observed feedback bins") + return tuple( + float(np.median(grouped[key])) for key in sorted(grouped) + ) + + def _travel(fit: JointCurveFit) -> float: return 0.5 * ( float(fit.decreasing_rad[0] - fit.decreasing_rad[255]) @@ -91,11 +306,134 @@ def _travel(fit: JointCurveFit) -> float: ) +def measured_curve_bounds( + name: str, + fit: JointCurveFit, + feedback_domain_rad: tuple[float, float], + *, + sign: float, +) -> tuple[float, float]: + """Return the zero-referenced public URDF range of one O12 curve.""" + inputs = curve_input_knots_rad(name, feedback_domain_rad=feedback_domain_rad) + branches = [] + for branch in ("decreasing_rad", "increasing_rad"): + values = np.asarray( + curve_values_at_rad(name, fit, inputs, branch, feedback_domain_rad), + dtype=float, + ) + values -= float(np.interp(0.0, np.asarray(inputs, dtype=float), values)) + branches.append(values) + average = 0.5 * (branches[0] + branches[1]) + direction = float(np.sign(average[-1] - average[0])) + if direction not in {0.0, float(sign)}: + branches = [-values for values in branches] + average = -average + values = np.concatenate([average, *branches]) + return float(np.min(values)), float(np.max(values)) + + +def _has_complete_thumb_root_geometry( + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], +) -> bool: + required = { + "relative_translation_xyz_m", + "parent_pose_common", + "child_pose_common", + "view_normal_common_xyz", + "camera_center_common_xyz_m", + "state_rad", + } + return all( + rows and all(required.issubset(row) for row in rows) + for name in O12_THUMB_ROOT_AXIS_JOINTS + for rows in (records_by_joint.get(name, ()),) + ) + + +def _fit_thumb_root_zero( + source_urdf: str | Path, + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + curves: Mapping[str, JointCurveFit], + feedback_domains_rad: Mapping[str, tuple[float, float]], +) -> ZeroSolveResult: + """Fit the O12 root thumb phases with the shared G20 geometry kernel.""" + profile = _thumb_root_zero_profile() + measurements = [] + for cycle in range(4): + for name in O12_THUMB_ROOT_AXIS_JOINTS: + rows = _virtual_records( + name, + records_by_joint[name], + feedback_domains_rad[name], + feedback_domains_rad, + ) + measurement = fit_joint_axis_measurement( + name, + rows, + cycle=cycle, + zero_command_u8=255, + constrained_circle_joints=profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + ) + measurements.append(with_depth_free_axis_projection( + measurement, + rows[0]["camera_center_common_xyz_m"], + )) + + result = solve_urdf_zero_offsets( + source_urdf=source_urdf, + measurements=measurements, + curves=curves, + motor_by_joint={ + name: COMMAND_INDEX_BY_JOINT[name] + for name in O12_THUMB_ROOT_AXIS_JOINTS + }, + training_cycles=(0, 1, 2), + validation_cycle=3, + maximum_offset_rad=math.radians(20.0), + finger_maximum_offset_rad=math.radians(20.0), + joint_maximum_offset_rad={ + name: math.radians(20.0) for name in ROOT_GEOMETRIC_ZERO_JOINTS + }, + maximum_cycle_difference_rad=math.radians(0.75), + minimum_applied_offset_rad=math.radians(0.1), + maximum_validation_mae_rad=math.radians(1.0), + maximum_validation_p95_rad=math.radians(2.0), + maximum_validation_error_rad=math.radians(3.0), + maximum_confidence_half_width_rad=math.radians(1.0), + # Axis-line displacement is retained as a diagnostic. Roll phase is + # determined by axis directions, so monocular depth/Tag placement is + # not allowed to reject an otherwise repeatable angular solution. + maximum_pose_axis_line_rms_m=0.0015, + maximum_systematic_axis_cone_bias_rad=math.radians(15.0), + hand_type="right", + tag_layout="o12_right_16", + zero_profile=profile, + ) + if not result.passed: + details = ",".join( + f"{name}={reason}" + for name, reason in sorted(result.failure_reasons.items()) + ) + raise ValueError("O12 thumb root spatial zero solve failed:" + details) + return result + + def fit_o12_session( source_urdf: str | Path, records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + *, + cross_view_records_by_joint: Mapping[ + str, Sequence[Mapping[str, Any]] + ] | None = None, + require_cross_view: bool = False, + require_thumb_root_spatial_zero: bool = False, + require_full_hand_spatial_zero: bool = False, ) -> O12FitResult: - del source_urdf # topology is validated by the immutable product loader + # SDK feedback supplies the continuous radian input domain; Tag relative + # rotation supplies the corresponding physical URDF coordinate and the + # shared G20 geometry kernel solves observable static thumb phase. expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS if set(records_by_joint) != expected: missing = sorted(expected - set(records_by_joint)) @@ -104,16 +442,40 @@ def fit_o12_session( curves: dict[str, JointCurveFit] = {} holdout: dict[str, tuple[float, ...]] = {} cycle_curves: dict[str, dict[int, JointCurveFit]] = {} + feedback_domains = { + joint: _measured_feedback_domain(joint, records_by_joint[joint]) + for joint in expected + } + virtual_by_joint = { + joint: _virtual_records( + joint, + records_by_joint[joint], + feedback_domains[joint], + feedback_domains, + ) + for joint in expected + } for joint in sorted(expected): - rows = _virtual_records(joint, records_by_joint[joint]) + rows = virtual_by_joint[joint] + _validate_cycle_repeatability(joint, rows, feedback_domains[joint]) training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}] validation = [row for row in rows if int(row["cycle"]) == 3] if not training or not validation: raise ValueError(f"{joint} is missing training or holdout records") fit = fit_rotation_joint_curve( - training, zero_command_u8=255, canonical_zero_direction="decreasing" + training, + zero_command_u8=255, + canonical_zero_direction="decreasing", + require_observed_domain_endpoints=False, + zero_reference_maximum_distance_u8=None, ) - errors = rotation_curve_holdout_errors(fit, validation, zero_command_u8=255) + frame_errors = rotation_curve_holdout_errors( + fit, + validation, + zero_command_u8=255, + zero_reference_maximum_distance_u8=None, + ) + errors = _uniform_curve_holdout_errors(validation, frame_errors) absolute = np.abs(np.asarray(errors, dtype=float)) if ( float(np.mean(absolute)) > math.radians(1.0) @@ -121,8 +483,6 @@ def fit_o12_session( or float(np.max(absolute)) > math.radians(3.0) ): raise ValueError(f"{joint} isolated holdout failed") - if fit.maximum_hysteresis_rad > math.radians(2.0): - raise ValueError(f"{joint} hysteresis exceeds 2 degrees") curves[joint] = fit holdout[joint] = tuple(float(value) for value in errors) cycle_curves[joint] = { @@ -130,6 +490,8 @@ def fit_o12_session( [row for row in training if int(row["cycle"]) == cycle], zero_command_u8=255, canonical_zero_direction="decreasing", + require_observed_domain_endpoints=False, + zero_reference_maximum_distance_u8=None, ) for cycle in (0, 1, 2) } @@ -151,31 +513,176 @@ def fit_o12_session( target, curves[source], curves[target], - model="quadratic_runtime", + # Retain the Tag-derived coupling as an independent diagnostic. + # Deployment uses the profile-declared vendor O12 polynomial. + model="direction_aware_knots", cycle_curve_pairs=cycle_pairs, minimum_multiplier=0.5, maximum_multiplier=2.2, ) + cross_view_rows = { + str(name): tuple(values) + for name, values in (cross_view_records_by_joint or {}).items() + } + required_roll = {"middle_mcp_roll", "index_mcp_roll"} + if require_cross_view and set(cross_view_rows) != required_roll: + raise ValueError( + "O12 roll cross-view records are incomplete: " + f"missing={sorted(required_roll - set(cross_view_rows))} " + f"extra={sorted(set(cross_view_rows) - required_roll)}" + ) + cross_view_metrics: dict[str, dict[str, float]] = {} + for joint, source_rows in sorted(cross_view_rows.items()): + if joint not in required_roll: + raise ValueError(f"unexpected O12 roll cross-view joint: {joint}") + rows = _virtual_records( + joint, + source_rows, + feedback_domains[joint], + feedback_domains, + ) + training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}] + validation = [row for row in rows if int(row["cycle"]) == 3] + if not training or not validation: + raise ValueError(f"{joint} side view lacks training or holdout records") + secondary = fit_rotation_joint_curve( + training, + zero_command_u8=255, + canonical_zero_direction="decreasing", + require_observed_domain_endpoints=False, + zero_reference_maximum_distance_u8=None, + ) + frame_errors = rotation_curve_holdout_errors( + secondary, + validation, + zero_command_u8=255, + zero_reference_maximum_distance_u8=None, + ) + errors = _uniform_curve_holdout_errors(validation, frame_errors) + absolute = np.abs(np.asarray(errors, dtype=float)) + if ( + float(np.mean(absolute)) > math.radians(1.0) + or float(np.percentile(absolute, 95.0)) > math.radians(2.0) + or float(np.max(absolute)) > math.radians(3.0) + ): + raise ValueError(f"{joint} side-view isolated holdout failed") + metrics = cross_view_roll_diagnostic_metrics(curves[joint], secondary) + if metrics.get("direction_disagrees", 0.0): + raise ValueError(f"{joint} cross-view roll direction disagrees") + metrics.update({ + "holdout_mae_rad": float(np.mean(absolute)), + "holdout_p95_rad": float(np.percentile(absolute, 95.0)), + "holdout_max_rad": float(np.max(absolute)), + "maximum_hysteresis_rad": float( + secondary.maximum_hysteresis_rad + ), + "sample_count": float(len(rows)), + }) + cross_view_metrics[joint] = metrics + + visual_arcs = {joint: _travel(curves[joint]) for joint in expected} travels = { - joint: _travel(curves[joint]) for joint in CALIBRATED_ACTIVE_JOINTS + joint: visual_arcs[joint] for joint in CALIBRATED_ACTIVE_JOINTS } for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): travels[target] = travels[donor] - # O12 active feedback is already a physical angle with open/centred zero. - # Calibration publishes the visual transfer curves and measured travel; - # it must not invent a static offset from an arbitrary Tag mounting angle. - offsets = {joint: 0.0 for joint in travels} - return O12FitResult( + offsets = {name: 0.0 for name in SDK_TO_URDF_JOINT} + zero_methods = {name: "source_cad_zero_not_measured" for name in offsets} + for name in STATIC_ZERO_EXCLUDED_JOINTS: + zero_methods[name] = "source_cad_zero_profile_excluded" + if ( + require_thumb_root_spatial_zero + and not _has_complete_thumb_root_geometry(records_by_joint) + ): + raise ValueError( + "O12 thumb root absolute zero requires common-frame roll, yaw, pitch " + "and palm-orientation axis trajectories" + ) + thumb_root_zero_result = None + full_hand_zero_result = None + spatial_error = None + if require_full_hand_spatial_zero: + from .zero import AXIS_JOINTS, O12SpatialZeroError, motor_index, solve_full_hand_zero + required = {"relative_translation_xyz_m", "parent_pose_common", "child_pose_common", + "view_normal_common_xyz", "camera_center_common_xyz_m", "state_rad"} + missing = [name for name in AXIS_JOINTS if not records_by_joint.get(name) + or any(not required.issubset(row) for row in records_by_joint[name])] + if missing: + raise O12SpatialZeroError( + "O12 full-hand spatial zero requires pose observations:" + ",".join(missing), + {"passed": False, "stage": "missing_geometry", "joints": missing}, + ) + # G20's geometry kernel uses a normalized 256-entry coordinate, not + # literal u8 motor commands. Rebase that private curve at SDK feedback + # zero and apply the same sign contract as the public radian mapper. + solver_curves = {} + for name in AXIS_JOINTS: + fit = curves[name] + domain = feedback_domains[name] + baseline = feedback_rad_to_curve_index(name, 0., domain) + lo = feedback_rad_to_curve_index(name, domain[0], domain) + hi = feedback_rad_to_curve_index(name, domain[1], domain) + values = np.asarray(fit.angle_rad, dtype=float) + direction = np.sign(np.interp(hi, np.arange(256), values) + - np.interp(lo, np.arange(256), values)) + multiplier = 1. if direction == SDK_TO_URDF_SIGN[motor_index(name)] else -1. + fields = {} + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(getattr(fit, field), dtype=float) + fields[field] = tuple(multiplier * (values - np.interp(baseline, np.arange(256), values))) + solver_curves[name] = replace(fit, **fields) + try: + full_hand_zero_result = solve_full_hand_zero( + source_urdf, + {name: _virtual_records(name, records_by_joint[name], feedback_domains[name], feedback_domains) + for name in AXIS_JOINTS}, + solver_curves, + ) + except O12SpatialZeroError as error: + if "result" not in error.diagnostics: + raise # Missing/unobservable geometry has no review estimate. + full_hand_zero_result = ZeroSolveResult(**error.diagnostics["result"]) + spatial_error = error + for name in GEOMETRIC_ZERO_JOINTS: + offsets[name] = float(full_hand_zero_result.direct_offsets_rad[name]) + zero_methods[name] = "urdf_serial_axis_geometry" + elif _has_complete_thumb_root_geometry(records_by_joint): + thumb_root_zero_result = _fit_thumb_root_zero( + source_urdf, + records_by_joint, + curves, + feedback_domains, + ) + for name in ROOT_GEOMETRIC_ZERO_JOINTS: + offsets[name] = float(thumb_root_zero_result.direct_offsets_rad[name]) + zero_methods[name] = "urdf_serial_axis_geometry" + # Transfer the scalar correction, never the donor's origin transform. + # The writer composes it with the ring's own CAD joint frame. + for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): + offsets[target] = offsets[donor] + zero_methods[target] = "transferred_static_zero_on_own_cad" + result = O12FitResult( curves=curves, zero_offsets_rad=offsets, travels_rad=travels, mimic_fits=mimic_fits, holdout_errors_rad=holdout, + cross_view_roll_metrics=cross_view_metrics, + visual_arc_diagnostics_rad=visual_arcs, + feedback_domains_rad=feedback_domains, + zero_method_by_joint=zero_methods, + full_hand_zero_result=full_hand_zero_result, + thumb_root_zero_result=thumb_root_zero_result, ) + if spatial_error is not None: + spatial_error.review_fit = result + raise spatial_error + return result __all__ = [ - "O12FitResult", "curve_input_knots_rad", "curve_values_at_rad", - "feedback_rad_to_curve_index", "fit_o12_session", + "O12FitResult", "O12_THUMB_ROOT_AXIS_JOINTS", + "curve_input_knots_rad", "curve_values_at_rad", + "feedback_rad_to_curve_index", "fit_o12_session", "measured_curve_bounds", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py new file mode 100644 index 0000000..646c740 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py @@ -0,0 +1,133 @@ +"""O12-specific interpretation of the vendor motor error report. + +The Pro2025 SDK documents ``commu_except`` as a possibly historical flag. It +must therefore be interpreted together with fresh command-triggered feedback; +the other four flags are active motor faults and are never downgraded. +""" + +from __future__ import annotations + +from dataclasses import dataclass + + +ERROR_STALLED = 1 << 0 +ERROR_OVERHEAT = 1 << 1 +ERROR_OVER_CURRENT = 1 << 2 +ERROR_MOTOR_EXCEPTION = 1 << 3 +ERROR_COMMUNICATION_EXCEPTION = 1 << 4 +ACTIVE_MOTOR_FAULT_MASK = ( + ERROR_STALLED + | ERROR_OVERHEAT + | ERROR_OVER_CURRENT + | ERROR_MOTOR_EXCEPTION +) +DOCUMENTED_ERROR_MASK = ACTIVE_MOTOR_FAULT_MASK | ERROR_COMMUNICATION_EXCEPTION + +ERROR_BIT_NAMES = ( + "stalled", + "overheat", + "over_current", + "motor_except", + "commu_except", +) + +HISTORICAL_COMMUNICATION_MINIMUM_REPORTS = 3 +HISTORICAL_COMMUNICATION_CONFIRM_SECONDS = 1.5 +HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS = 0.25 + + +@dataclass(frozen=True) +class O12ErrorAssessment: + codes: tuple[int, ...] + valid: bool + active_fault_channels: tuple[int, ...] + communication_channels: tuple[int, ...] + unknown_bit_channels: tuple[int, ...] + + @property + def clear(self) -> bool: + return self.valid and not any(self.codes) + + @property + def communication_only(self) -> bool: + return bool( + self.valid + and self.communication_channels + and not self.active_fault_channels + and not self.unknown_bit_channels + ) + + +def assess_o12_error_report(values) -> O12ErrorAssessment: + """Decode one complete 12-motor O12 report without hiding any bit.""" + try: + codes = tuple(int(value) for value in values) + except (TypeError, ValueError): + codes = () + valid = len(codes) == 12 and all(0 <= value <= 0xFFFF for value in codes) + if not valid: + return O12ErrorAssessment(codes, False, (), (), ()) + return O12ErrorAssessment( + codes=codes, + valid=True, + active_fault_channels=tuple( + index for index, value in enumerate(codes) + if value & ACTIVE_MOTOR_FAULT_MASK + ), + communication_channels=tuple( + index for index, value in enumerate(codes) + if value & ERROR_COMMUNICATION_EXCEPTION + ), + unknown_bit_channels=tuple( + index for index, value in enumerate(codes) + if value & ~DOCUMENTED_ERROR_MASK + ), + ) + + +def decoded_faults(codes: tuple[int, ...]) -> tuple[dict[str, object], ...]: + return tuple( + { + "motor_index": index, + "code": int(value), + "flags": tuple( + name for bit, name in enumerate(ERROR_BIT_NAMES) + if int(value) & (1 << bit) + ), + } + for index, value in enumerate(codes) + if value + ) + + +def historical_communication_latch_confirmed( + *, + assessment: O12ErrorAssessment, + matching_report_count: int, + observation_seconds: float, + feedback_hz: float, + feedback_age_seconds: float, + minimum_feedback_hz: float, +) -> bool: + """Require repeated reports and a fresh full-rate control feedback stream.""" + return bool( + assessment.communication_only + and matching_report_count >= HISTORICAL_COMMUNICATION_MINIMUM_REPORTS + and observation_seconds >= HISTORICAL_COMMUNICATION_CONFIRM_SECONDS + and feedback_hz >= minimum_feedback_hz + and 0.0 <= feedback_age_seconds <= HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS + ) + + +__all__ = [ + "ACTIVE_MOTOR_FAULT_MASK", + "ERROR_BIT_NAMES", + "ERROR_COMMUNICATION_EXCEPTION", + "HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS", + "HISTORICAL_COMMUNICATION_CONFIRM_SECONDS", + "HISTORICAL_COMMUNICATION_MINIMUM_REPORTS", + "O12ErrorAssessment", + "assess_o12_error_report", + "decoded_faults", + "historical_communication_latch_confirmed", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py new file mode 100644 index 0000000..3ea8b95 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py @@ -0,0 +1,93 @@ +"""O12 vendor kinematics used by calibration and URDF publication. + +The O12 SDK exposes twelve active joint coordinates but the hand URDF has +nineteen revolute joints. The seven passive coordinates are part of the +vendor solver contract; they are not independent quantities that may be +replaced by an arbitrary linear fit of a planar AprilTag pose. + +The coefficients below are the public ``OmniHandPro2025Solver`` coefficients +from the protected SDK input. Keeping this small, deterministic evaluator in +the calibration package makes online and offline publication identical even +when the binary Python wheel is only available inside the vendor overlay. +""" + +from __future__ import annotations + +import math +from typing import Mapping, Sequence + + +# Polynomial order is constant, x, x**2, x**3, x**4. The input is the SDK +# active-joint feedback in radians. Right-thumb MCP/PIP feedback is negative, +# hence its passive DIP result is negative too; ``sdk_to_urdf_sign`` converts +# both to the positive target-URDF convention. +PASSIVE_POLYNOMIAL_BY_JOINT: Mapping[str, tuple[float, ...]] = { + "thumb_dip": (0.0, 0.6359, -0.3539, -0.3066, -0.1240), + "index_dip": (0.0, 1.0630, 0.08942, 0.1845, -0.2169), + "middle_dip": (0.0, 1.1490, -0.2581, 0.6033, -0.3371), + "ring_pip": (0.0, 0.7869, 0.3884, -0.4545, 0.1578), + "ring_dip": (0.0, 0.8990, 0.3138, -0.1728, -0.03666), + "pinky_pip": (0.0, 0.7869, 0.3884, -0.4545, 0.1578), + "pinky_dip": (0.0, 0.8990, 0.3138, -0.1728, -0.03666), +} + +# Polynomial inputs are active SDK coordinates, not necessarily the immediate +# linear URDF mimic source. In particular ring/pinky DIP is solved directly +# from the single active MCP coordinate by the vendor implementation. +PASSIVE_SDK_SOURCE_BY_JOINT: Mapping[str, str] = { + "thumb_dip": "thumb_mcp", + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "ring_pip": "ring_mcp_pitch", + "ring_dip": "ring_mcp_pitch", + "pinky_pip": "pinky_mcp_pitch", + "pinky_dip": "pinky_mcp_pitch", +} + + +def evaluate_vendor_passive_joint( + joint: str, + sdk_feedback_rad: float, + *, + sdk_to_urdf_sign: float, +) -> float: + """Evaluate one passive O12 joint in the target URDF convention.""" + try: + coefficients = PASSIVE_POLYNOMIAL_BY_JOINT[str(joint)] + except KeyError as error: + raise ValueError(f"unknown O12 passive joint: {joint}") from error + x = float(sdk_feedback_rad) + sign = float(sdk_to_urdf_sign) + if not math.isfinite(x) or sign not in {-1.0, 1.0}: + raise ValueError("O12 vendor passive input/sign is invalid") + value = 0.0 + power = 1.0 + for coefficient in coefficients: + value += float(coefficient) * power + power *= x + result = sign * value + if not math.isfinite(result): + raise ValueError(f"O12 vendor passive result is invalid: {joint}") + return result + + +def vendor_passive_curve( + joint: str, + sdk_feedback_rad: Sequence[float], + *, + sdk_to_urdf_sign: float, +) -> tuple[float, ...]: + return tuple( + evaluate_vendor_passive_joint( + joint, value, sdk_to_urdf_sign=sdk_to_urdf_sign + ) + for value in sdk_feedback_rad + ) + + +__all__ = [ + "PASSIVE_POLYNOMIAL_BY_JOINT", + "PASSIVE_SDK_SOURCE_BY_JOINT", + "evaluate_vendor_passive_joint", + "vendor_passive_curve", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py index 8eec1f5..4f06d6f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py @@ -31,6 +31,62 @@ def cosine_position_trajectory_rad( ) +def cosine_ramp_velocity_trajectory_rad( + start_rad: float, + target_rad: float, + elapsed_seconds: float, + maximum_speed_rad_s: float, + ramp_seconds: float, +) -> tuple[float, float, float]: + """Velocity-limited trajectory with cosine ramps and a constant-speed core.""" + speed = float(maximum_speed_rad_s) + ramp = float(ramp_seconds) + if not math.isfinite(speed) or speed <= 0.0: + raise ValueError("maximum_speed_rad_s must be positive") + if not math.isfinite(ramp) or ramp <= 0.0: + raise ValueError("ramp_seconds must be positive") + start = float(start_rad) + target = float(target_rad) + distance = abs(target - start) + if distance <= 0.0: + return target, 1.0, 0.0 + + # For very short moves there is no room for a constant-speed section; + # retain the bounded position-cosine trajectory. + if distance <= speed * ramp: + return cosine_position_trajectory_rad( + start, target, elapsed_seconds, speed + ) + + cruise_seconds = distance / speed - ramp + duration = 2.0 * ramp + cruise_seconds + elapsed = min(duration, max(0.0, float(elapsed_seconds))) + ramp_distance = 0.5 * speed * ramp + if elapsed < ramp: + travelled = speed * ( + 0.5 * elapsed + - ramp * math.sin(math.pi * elapsed / ramp) / (2.0 * math.pi) + ) + elif elapsed < ramp + cruise_seconds: + travelled = ramp_distance + speed * (elapsed - ramp) + else: + down = elapsed - ramp - cruise_seconds + travelled = ( + ramp_distance + + speed * cruise_seconds + + speed * ( + 0.5 * down + + ramp * math.sin(math.pi * down / ramp) / (2.0 * math.pi) + ) + ) + fraction = min(1.0, max(0.0, travelled / distance)) + return ( + start + (target - start) * fraction, + min(1.0, max(0.0, elapsed / duration)), + duration, + ) + + def build_calibration_motion_command( task: TaskSpec, command_rad: float, @@ -73,4 +129,5 @@ def build_calibration_return_waypoints( __all__ = [ "build_calibration_motion_command", "build_calibration_preparation_waypoints", "build_calibration_return_waypoints", "cosine_position_trajectory_rad", + "cosine_ramp_velocity_trajectory_rad", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py index 2e75b62..217fd7f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py @@ -2,30 +2,155 @@ from __future__ import annotations +from dataclasses import dataclass import math import time import numpy as np +import cv2 import rclpy +from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.executors import MultiThreadedExecutor from scipy.spatial.transform import Rotation +from sensor_msgs.msg import JointState from std_msgs.msg import Empty, Int8MultiArray, Int16MultiArray -from ...storage import append_jsonl -from ..l6.node import L6ThreeCameraCalibrationNode, MotionStep +from ...acquisition import interpolate_state_u8 +from ...extrinsics import matrix_payload, transform_matrix +from ...pnp import SquareTagPose, solve_square_tag_ippe +from ...storage import append_jsonl, append_jsonl_many +from ...runtime import CalibrationEngine +from ..l6.node import ( + L6ThreeCameraCalibrationNode, + MotionStep as LegacyMotionStep, + _stamp_ns, +) from .pipeline import finalize_o12_session +from .health import ( + assess_o12_error_report, + decoded_faults, + historical_communication_latch_confirmed, +) +from .motion import cosine_ramp_velocity_trajectory_rad +from .pipeline import load_o12_raw_samples +from .quality import QUALITY_POLICY_VERSION from .profile import ( + CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD, + CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION, + CLEARANCE_SPLAY_ENDPOINT_TOLERANCE_RAD, + EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION, + FORMAL_SPEED_CAP_RAD_S, + INITIAL_FEEDBACK_SPAN_FRACTION, + INDEX_CLEARANCE_MCP_RAD, INDEX_CLEARANCE_RAD, - PARK_FINGER_RAD, PARK_MIDDLE_MCP_RAD, PARK_MIDDLE_PIP_RAD, + PARK_PINKY_MCP_RAD, + PARK_RING_MCP_RAD, + NORMALIZED_SWEEP_BIN_COUNT, + ROLL_CROSS_VIEW_BY_TASK, build_typed_profile, ) +from .resume import build_resume_checkpoint_from_rows +from .pnp import O12ThumbPoseTracker, THUMB_ROLES POSITION_MODE = 0 +@dataclass(frozen=True) +class MotionStep(LegacyMotionStep): + """O12-only motion step with a feedback-relative mapping probe.""" + + relative_feedback_delta: float | None = None + required_endpoint_indices: tuple[int, ...] = () + + class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): + def _record_capture_pose_evidence(self, view, step, roles, corners, selected, stamp, *, locked_roles=()): + if (step is None or step.task_key is None + or step.task_key == 'thumb_mcp_dip_front'): + return # The articulated selector already records thumb evidence. + def payload(p): + return {'quaternion_xyzw': list(p.quaternion_xyzw), + 'translation_xyz_m': list(p.translation_xyz_m), + 'reprojection_error_px': float(p.reprojection_error_px)} + tracker = self.trackers[view] + ids = {t.role: t.tag_id for t in self._view(view).tags} + append_jsonl(self.raw_path, { + 'kind': 'o12_pnp_candidate_frame', 'diagnostic_schema': 1, + 'selection_policy': 'independent_provisional', + 'task_name': step.task_key, 'cycle': step.cycle, + 'direction': step.direction, 'attempt': step.attempt, + 'phase': step.phase, 'view': view, 'image_stamp_ns': stamp, + 'tag_size_m': float(self.tag_size_m), + 'camera_matrix': np.asarray(self.camera_matrices[view]).tolist(), + 'camera_matrix_source': 'CameraInfo.P[:3,:3]', + 'input_is_rectified': True, 'distortion_coefficients': [0.0]*4, + 'roles': {role: { + 'tag_id': ids[role], + 'corners_xy': np.asarray(corners[role]).tolist() if role in corners else None, + 'observation_source': 'locked_reference' if role in locked_roles else 'image', + 'candidates': [payload(p) for p in tracker.last_candidates_by_role.get(role, ())] + if role not in locked_roles else [], + 'selected': payload(selected[role]), + 'maximum_reprojection_error_px': tracker.maximum_reprojection_error_px, + } for role in roles}, + }) + + def _select_articulated_capture_poses(self, view, step, roles, corners, stamp): + if (view != 'front' or step is None + or step.task_key != 'thumb_mcp_dip_front' + or set(roles) != set(THUMB_ROLES)): + return None + if not hasattr(self, '_thumb_pose_group'): + self._thumb_pose_group = O12ThumbPoseTracker() + tracker = self.trackers[view] + candidates = {} + all_candidates = {} + for role in THUMB_ROLES: + try: + all_candidates[role] = solve_square_tag_ippe( + corners[role], tag_size_m=float(self.tag_size_m), + camera_matrix=self.camera_matrices[view]) + except (ValueError, cv2.error): + all_candidates[role] = () + candidates[role] = tuple( + p for p in all_candidates[role] + if p.reprojection_error_px <= tracker.maximum_reprojection_error_px) + selected, reason = self._thumb_pose_group.select(candidates, stamp_ns=stamp) + def payload(pose): + return {'quaternion_xyzw': list(pose.quaternion_xyzw), + 'translation_xyz_m': list(pose.translation_xyz_m), + 'reprojection_error_px': float(pose.reprojection_error_px)} + # Save once per image, including discarded frames. Existing joint + # samples / schemas / resume units are unchanged. + append_jsonl(self.raw_path, { + 'kind': 'o12_pnp_candidate_frame', 'diagnostic_schema': 1, + 'selection_policy': 'o12_thumb_parallel_axes_v1', + 'task_name': step.task_key, 'cycle': step.cycle, + 'direction': step.direction, 'attempt': step.attempt, + 'phase': step.phase, 'view': view, 'image_stamp_ns': stamp, + 'tag_size_m': float(self.tag_size_m), + 'camera_matrix': np.asarray(self.camera_matrices[view]).tolist(), + 'camera_matrix_source': 'CameraInfo.P[:3,:3]', + 'input_is_rectified': True, 'distortion_coefficients': [0.0]*4, + 'roles': {role: { + 'tag_id': next(t.tag_id for t in self._view(view).tags if t.role == role), + 'corners_xy': np.asarray(corners[role]).tolist(), + 'candidates': [payload(p) for p in all_candidates[role]], + 'eligible_candidate_count': len(candidates[role]), + 'maximum_reprojection_error_px': tracker.maximum_reprojection_error_px, + 'selected': None if selected is None else payload(selected[role]), + } for role in THUMB_ROLES}, + 'selection_reason': reason, + }) + return selected, reason + + @staticmethod + def _uses_isolated_motion_callbacks() -> bool: + return True + def __init__(self) -> None: super().__init__( profile=build_typed_profile(), @@ -38,11 +163,34 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): self.temperature_fallback_active = False self.health_check_started_at = time.monotonic() self.latest_errors: tuple[int, ...] = () + self.error_health_classification = "awaiting_error_report" + self.error_report_count = 0 + self.error_report_first_matching_at = 0.0 + self.error_report_matching_count = 0 + self.error_report_matching_codes: tuple[int, ...] = () + self.confirmed_historical_communication_channels: tuple[int, ...] = () + self.error_health_audit_written = False self.latest_temperatures: tuple[int, ...] = () self.last_health_query_at = 0.0 + self.last_temperature_query_at = 0.0 self.preflight_reference_by_task: dict[str, Rotation] = {} self.preflight_maximum_rotation_by_task: dict[str, float] = {} self.measured_direction_axis_by_task: dict[str, tuple[float, float, float]] = {} + self.locked_base_pose_by_view: dict[str, SquareTagPose] = {} + self.resolved_probe_target_by_step: dict[int, tuple[float, ...]] = {} + self.probe_feedback_origin_by_step: dict[int, float] = {} + self.resume_source_session = "" + self.resume_compatibility = "not_requested" + self.resumed_unit_keys: frozenset[tuple[str, int, str]] = frozenset() + self.resumed_task_keys: frozenset[str] = frozenset() + self.resume_skipped_step_count = 0 + self.normalized_sweep_bin_count = NORMALIZED_SWEEP_BIN_COUNT + self.latest_o12_feedback_coupling: dict[str, object] = {} + self.latest_o12_auxiliary_tracking: dict[str, object] = {} + self.o12_auxiliary_violation_since: dict[int, float] = {} + self._reset_o12_cross_view_counters() + if self.resume_raw_samples_path is not None: + self._restore_resume_checkpoint() self.control_mode_publisher = self.create_publisher( Int8MultiArray, "/o12/right/joint_control_mode_cmd", 10 ) @@ -71,12 +219,563 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): 10, ) + def _roll_cross_view_observer(self, step=None): + selected = self._current_step() if step is None else step + if selected is None or selected.task_key is None: + return None + return ROLL_CROSS_VIEW_BY_TASK.get(str(selected.task_key)) + + def _reset_o12_cross_view_counters(self) -> None: + self.o12_cross_view_total_frames = 0 + self.o12_cross_view_valid_frames = 0 + self.o12_cross_view_tag_seen_frames: dict[str, int] = {} + self.o12_cross_view_tag_quality_frames: dict[str, int] = {} + self.o12_cross_view_pnp_valid_frames = 0 + self.o12_cross_view_state_sync_frames = 0 + self.o12_cross_view_rejection_counts: dict[str, int] = {} + self.o12_cross_view_recognized_tag_ids: tuple[int, ...] = () + self.o12_cross_view_unrecognized_tag_ids: tuple[int, ...] = () + + def _reset_step_observation_counters( + self, roles: tuple[str, ...] = () + ) -> None: + super()._reset_step_observation_counters(roles) + self._reset_o12_cross_view_counters() + self.latest_o12_auxiliary_tracking = {} + self.o12_auxiliary_violation_since = {} + observer = self._roll_cross_view_observer() + if observer is not None: + cross_roles = (observer.parent_role, observer.child_role) + self.o12_cross_view_tag_seen_frames = { + role: 0 for role in cross_roles + } + self.o12_cross_view_tag_quality_frames = { + role: 0 for role in cross_roles + } + id_by_role = { + tag.role: int(tag.tag_id) + for tag in self._view(observer.view).tags + } + self.o12_cross_view_unrecognized_tag_ids = tuple( + sorted(id_by_role[role] for role in cross_roles) + ) + + def _count_o12_cross_view_rejection(self, reason: str) -> None: + self.o12_cross_view_rejection_counts[reason] = ( + self.o12_cross_view_rejection_counts.get(reason, 0) + 1 + ) + + def _o12_cross_view_metrics(self, observer=None) -> dict[str, object]: + selected = observer or self._roll_cross_view_observer() + roles = ( + () + if selected is None + else (selected.parent_role, selected.child_role) + ) + total = int(self.o12_cross_view_total_frames) + + def rate(count: int) -> float: + return float(count) / total if total else 0.0 + + quality = { + role: rate(self.o12_cross_view_tag_quality_frames.get(role, 0)) + for role in roles + } + return { + "view": None if selected is None else selected.view, + "tag_detection_rate_by_role": quality, + "tag_detection_rate": min(quality.values(), default=0.0), + "pnp_valid_rate": rate(self.o12_cross_view_pnp_valid_frames), + "state_sync_rate": rate(self.o12_cross_view_state_sync_frames), + "joint_frame_rate": rate(self.o12_cross_view_valid_frames), + "rejection_counts": dict( + sorted(self.o12_cross_view_rejection_counts.items()) + ), + } + + def _state_callback(self, message: JointState) -> None: + """Record all twelve SDK coordinates without model-theory stop gates.""" + L6ThreeCameraCalibrationNode._state_callback(self, message) + step = self._current_step() + if ( + step is None + or step.task_key is None + or len(self.latest_state_u8) != self.command_count + ): + self.latest_o12_auxiliary_tracking = {} + return + task = self._task(str(step.task_key)) + target = self._target_command(step) + self.latest_o12_auxiliary_tracking = { + "active": bool(task.auxiliary_commands), + "task_name": task.key, + "decision": "diagnostic_only", + "channels": [ + { + "channel": self.command_names[int(index)], + "feedback_rad": round(float(self.latest_state_u8[int(index)]), 9), + "target_rad": round(float(target[int(index)]), 9), + "error_rad": round(abs( + float(self.latest_state_u8[int(index)]) + - float(target[int(index)]) + ), 9), + } + for index, _value in task.auxiliary_commands + ], + } + + def _apply_o12_tracking_diagnostics( + self, status: dict[str, object] + ) -> dict[str, object]: + """Compare O12 feedback with the command published this tick.""" + if ( + len(self.latest_state_u8) == self.command_count + and self.step_last_command_u8 is not None + and len(self.step_last_command_u8) == self.command_count + ): + errors = [ + abs(float(actual) - float(command)) + for actual, command in zip( + self.latest_state_u8, self.step_last_command_u8 + ) + ] + status["channel_errors_rad"] = [ + round(error, 6) for error in errors + ] + status["maximum_error_channel"] = self.command_names[ + int(np.argmax(errors)) + ] + status["maximum_error_rad"] = round(max(errors), 3) + return status + + def _detections_callback( + self, view: str, message: AprilTagDetectionArray + ) -> None: + step = self._current_step() + observer = self._roll_cross_view_observer(step) + if ( + observer is not None + and view == observer.view + and step is not None + and step.recording + and self.step_command_sent + ): + self._record_o12_roll_cross_view(observer, step, message) + return + super()._detections_callback(view, message) + + def _record_o12_roll_cross_view( + self, observer, step: MotionStep, message: AprilTagDetectionArray + ) -> None: + """Record the side camera as a required observer of one roll sweep.""" + view = str(observer.view) + if view not in self.camera_matrices: + return + roles = (observer.parent_role, observer.child_role) + role_by_id = {tag.tag_id: tag.role for tag in self._view(view).tags} + id_by_role = {tag.role: int(tag.tag_id) for tag in self._view(view).tags} + corners: dict[str, np.ndarray] = {} + good: dict[str, bool] = {} + detected: set[str] = set() + failures_by_role: dict[str, tuple[str, ...]] = {} + width, height = self.image_sizes.get(view, (0, 0)) + for detection in message.detections: + role = role_by_id.get(int(detection.id)) + if role not in roles: + continue + detected.add(str(role)) + points = np.asarray( + [[float(point.x), float(point.y)] for point in detection.corners], + dtype=float, + ) + if points.shape != (4, 2): + failures_by_role[str(role)] = ("malformed_corners",) + continue + edges = np.linalg.norm(points - np.roll(points, -1, axis=0), axis=1) + border_ok = bool( + width > 0 and height > 0 + and np.min(points[:, 0]) >= 2.0 + and np.max(points[:, 0]) <= width - 3.0 + and np.min(points[:, 1]) >= 2.0 + and np.max(points[:, 1]) <= height - 3.0 + ) + reasons: list[str] = [] + if int(detection.hamming) > int(self.maximum_hamming): + reasons.append("hamming") + if float(detection.decision_margin) < float( + self.minimum_decision_margin + ): + reasons.append("decision_margin") + if float(np.min(edges)) < float(self.minimum_edge_pixels): + reasons.append("edge_pixels") + if not border_ok: + reasons.append("image_border") + candidate_good = not reasons + if role not in corners or candidate_good or not good.get(role, False): + corners[str(role)] = points + good[str(role)] = candidate_good + failures_by_role[str(role)] = tuple(reasons) + + self.o12_cross_view_total_frames += 1 + for role in roles: + if role in detected: + self.o12_cross_view_tag_seen_frames[role] += 1 + else: + self._count_o12_cross_view_rejection(f"tag:{role}:missing") + if good.get(role, False): + self.o12_cross_view_tag_quality_frames[role] += 1 + else: + for reason in failures_by_role.get(role, ()): + self._count_o12_cross_view_rejection( + f"tag:{role}:{reason}" + ) + self.o12_cross_view_recognized_tag_ids = tuple( + sorted(id_by_role[role] for role in roles if good.get(role, False)) + ) + self.o12_cross_view_unrecognized_tag_ids = tuple( + sorted(id_by_role[role] for role in roles if not good.get(role, False)) + ) + if not all(role in corners and good.get(role, False) for role in roles): + return + + base_role = observer.parent_role + if base_role in self.base_corner_reference: + drift = float(np.max(np.linalg.norm( + corners[base_role] - self.base_corner_reference[view], axis=1 + ))) + self.latest_base_drift_px[view] = drift + self.base_drift_counts[view] = ( + self.base_drift_counts.get(view, 0) + 1 + if drift > float(self.fixed_base_maximum_corner_drift_px) + else 0 + ) + if self.base_drift_counts[view] >= int( + self.fixed_base_movement_confirmation_frames + ): + append_jsonl(self.raw_path, { + "kind": "fixed_base_reference_moved", + "view": view, + "tag_id": id_by_role[base_role], + "corner_drift_px": round(drift, 6), + "maximum_corner_drift_px": float( + self.fixed_base_maximum_corner_drift_px + ), + "confirmation_frames": self.base_drift_counts[view], + "task_name": step.task_key, + }) + self._pause( + f"fixed_base_tag_moved:{view}:drift_px={drift:.3f}" + ) + return + + stamp = _stamp_ns(message.header.stamp) + selected: dict[str, SquareTagPose] = {} + for role in roles: + pose, reason = self.trackers[view].estimate( + role, + corners[role], + tag_size_m=float(self.tag_size_m), + camera_matrix=self.camera_matrices[view], + stamp_ns=stamp, + ) + if pose is None: + self._count_o12_cross_view_rejection( + f"pnp:{role}:{reason or 'rejected'}" + ) + return + selected[role] = pose + self.o12_cross_view_pnp_valid_frames += 1 + self._record_capture_pose_evidence(view, step, roles, corners, selected, stamp) + self.last_view_valid_at[view] = time.monotonic() + + matched = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched is None: + self._count_o12_cross_view_rejection( + "state_sync:no_sample_within_limit" + ) + return + state_rad, skew_ns = matched + self.o12_cross_view_state_sync_frames += 1 + task = self._task(str(step.task_key)) + feedback = float(state_rad[task.command_index]) + progress = float(np.clip( + self.profile.command.normalize(task.command_index, feedback), + 0.0, + 1.0, + )) + parent = selected[observer.parent_role] + child = selected[observer.child_role] + common_from_view = self.extrinsics.transform(view) + parent_matrix = common_from_view @ transform_matrix( + parent.translation_xyz_m, parent.quaternion_xyzw + ) + child_matrix = common_from_view @ transform_matrix( + child.translation_xyz_m, child.quaternion_xyzw + ) + relative = ( + Rotation.from_matrix(parent_matrix[:3, :3]).inv() + * Rotation.from_matrix(child_matrix[:3, :3]) + ) + relative_translation = Rotation.from_quat( + parent.quaternion_xyzw + ).inv().apply( + np.asarray(child.translation_xyz_m, dtype=float) + - np.asarray(parent.translation_xyz_m, dtype=float) + ) + record = { + "kind": "o12_roll_cross_view_sample", + "profile_id": self.profile.key.profile_id, + "task_name": task.key, + "view": view, + "joint": observer.source_joint, + "model_joint": observer.model_joint, + "sdk_channel": self.command_names[task.command_index], + "motor_index": int(task.command_index), + "coupled_sdk_channel": self.command_names[ + observer.coupled_channel_index + ], + "cycle": int(step.cycle), + "direction": str(step.direction), + "attempt": int(step.attempt), + "requested_command_rad": round(float(step.target_u8), 9), + "trajectory_command_rad": round(self.step_requested_u8, 9), + "command_rad": round(self.step_requested_u8, 9), + "feedback_rad": round(feedback, 9), + "coupled_feedback_rad": round( + float(state_rad[observer.coupled_channel_index]), 9 + ), + "progress_01": round(progress, 9), + "state_rad": [round(float(value), 9) for value in state_rad], + "state_image_sync_error_ms": round( + abs(skew_ns) / 1_000_000.0, 6 + ), + "relative_quaternion_xyzw": [ + float(value) for value in relative.as_quat() + ], + "relative_translation_xyz_m": [ + float(value) for value in relative_translation + ], + "parent_pose_common": matrix_payload(parent_matrix), + "child_pose_common": matrix_payload(child_matrix), + "pnp_reprojection_error_px": round( + max( + parent.reprojection_error_px, + child.reprojection_error_px, + ), + 6, + ), + "image_stamp_ns": int(stamp), + } + self.raw_records.append(record) + append_jsonl(self.raw_path, record) + self.o12_cross_view_valid_frames += 1 + + def _qualify_recording_step(self, step: MotionStep) -> None: + """Apply only retained-data and required dual-view gates.""" + task = self._task(str(step.task_key)) + engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile)) + observer = self._roll_cross_view_observer(step) + if observer is not None: + rows = [ + row for row in self.raw_records + if row.get("kind") == "o12_roll_cross_view_sample" + and row.get("task_name") == observer.task_name + and int(row.get("cycle", -1)) == int(step.cycle) + and row.get("direction") == step.direction + and int(row.get("attempt", 1)) == int(step.attempt) + and row.get("joint") == observer.source_joint + ] + feedback = np.asarray( + [float(row["feedback_rad"]) for row in rows], dtype=float + ) + normalized = self._normalize_o12_feedback(task, step, feedback) + observation = self._o12_cross_view_metrics(observer) + decision = engine.evaluate_sweep( + normalized, + minimum_span=self._required_radian_feedback_span_fraction( + task, step, feedback + ), + total_frames=self.o12_cross_view_total_frames, + joint_frame_rate=float(observation["joint_frame_rate"]), + feedback_hz=self._feedback_hz(), + detection_rate=float(observation["tag_detection_rate"]), + bin_count=int(self.normalized_sweep_bin_count), + ) + append_jsonl(self.raw_path, { + "kind": "o12_roll_cross_view_quality", + "task_name": observer.task_name, + "source_joint": observer.source_joint, + "model_joint": observer.model_joint, + "view": observer.view, + "cycle": step.cycle, + "direction": step.direction, + "attempt": step.attempt, + "total_frames": self.o12_cross_view_total_frames, + **observation, + **decision.metrics, + "quality_policy_version": QUALITY_POLICY_VERSION, + "warnings": list(decision.warnings), + "failures": list(decision.failures), + "passed": decision.passed, + }) + # Run the side gate first. A failed side attempt must never leave + # behind a passing primary quality row that checkpoint recovery + # could mistake for a complete dual-view transaction. + if decision.failures: + raise ValueError(",".join( + f"side_{failure}" for failure in decision.failures + )) + self._qualify_o12_primary_recording_step(step) + + def _qualify_o12_primary_recording_step(self, step: MotionStep) -> None: + """Judge a radian sweep by retained fitting information, not drop rate.""" + task = self._task(str(step.task_key)) + rows = [ + row for row in self.raw_records + if row.get("kind", "o12_joint_sample") == "o12_joint_sample" + and row.get("task_name") == task.key + and int(row.get("cycle", -1)) == int(step.cycle) + and row.get("direction") == step.direction + and int(row.get("attempt", 1)) == int(step.attempt) + and row.get("joint") == task.joints[0] + and math.isfinite(float(row.get("feedback_rad", math.nan))) + ] + feedback = np.asarray( + [float(row["feedback_rad"]) for row in rows], dtype=float + ) + normalized = self._normalize_o12_feedback(task, step, feedback) + observation = self._step_observation_metrics() + engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile)) + decision = engine.evaluate_sweep( + normalized, + minimum_span=self._required_radian_feedback_span_fraction( + task, step, feedback + ), + total_frames=self.step_total_frames, + joint_frame_rate=float(observation["joint_frame_rate"]), + feedback_hz=self._feedback_hz(), + detection_rate=float(observation["tag_detection_rate"]), + bin_count=int(self.normalized_sweep_bin_count), + ) + append_jsonl(self.raw_path, { + "kind": self.sweep_quality_kind, + "task_name": step.task_key, + "cycle": step.cycle, + "direction": step.direction, + "attempt": step.attempt, + "total_frames": self.step_total_frames, + **observation, + **decision.metrics, + "quality_policy_version": QUALITY_POLICY_VERSION, + "warnings": list(decision.warnings), + "failures": list(decision.failures), + "passed": decision.passed, + }) + if decision.failures: + raise ValueError(",".join(decision.failures)) + + def _required_radian_feedback_span_fraction( + self, + task, + step: MotionStep, + _feedback: np.ndarray, + ) -> float: + """Return the O12-only effective-travel gate for one sweep. + + O12 reports a continuous physical coordinate whose endpoint scale and + zero offset are part of the curve being calibrated. Cycle zero must + therefore establish the observed full-stroke reference instead of + requiring feedback to numerically equal the command endpoints. Every + later cycle must reproduce that same measured travel. + """ + floor = float(INITIAL_FEEDBACK_SPAN_FRACTION) + if step.cycle is None or int(step.cycle) == 0: + return floor + + reference = self._o12_cycle_zero_feedback_span_fraction( + task, str(step.direction) + ) + if reference is None: + # A normal run cannot reach a later cycle without an accepted + # cycle-zero unit. Retaining the absolute floor makes a damaged + # or legacy checkpoint fail safely without changing other hands. + return floor + return float(EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION) * reference + + def _normalize_o12_feedback( + self, task, step: MotionStep, feedback: np.ndarray + ) -> np.ndarray: + """Normalize against measured O12 travel, never command endpoints.""" + values = np.asarray(feedback, dtype=float) + if values.size == 0: + return values + if step.cycle is None or int(step.cycle) == 0: + lower, upper = float(np.min(values)), float(np.max(values)) + else: + bounds = self._o12_cycle_zero_feedback_bounds( + task, str(step.direction) + ) + if bounds is None: + return np.zeros_like(values) + lower, upper = bounds + span = upper - lower + if not math.isfinite(span) or span <= 1.0e-9: + return np.zeros_like(values) + return (values - lower) / span + + def _o12_cycle_zero_feedback_bounds( + self, task, direction: str + ) -> tuple[float, float] | None: + """Return the widest accepted/current cycle-zero feedback interval.""" + bounds: list[tuple[float, float]] = [] + attempts = sorted({ + int(row.get("attempt", 1)) + for row in self.raw_records + if row.get("kind", "o12_joint_sample") == "o12_joint_sample" + and row.get("task_name") == task.key + and int(row.get("cycle", -1)) == 0 + and row.get("direction") == direction + and row.get("joint") == task.joints[0] + }) + for attempt in attempts: + feedback = [ + float(row["feedback_rad"]) + for row in self.raw_records + if row.get("kind", "o12_joint_sample") == "o12_joint_sample" + and row.get("task_name") == task.key + and int(row.get("cycle", -1)) == 0 + and row.get("direction") == direction + and int(row.get("attempt", 1)) == attempt + and row.get("joint") == task.joints[0] + and math.isfinite(float(row.get("feedback_rad", math.nan))) + ] + if feedback: + bounds.append((min(feedback), max(feedback))) + if not bounds: + return None + return max(bounds, key=lambda item: item[1] - item[0]) + + def _o12_cycle_zero_feedback_span_fraction( + self, task, direction: str + ) -> float | None: + """Find the accepted cycle-zero physical span for this O12 direction.""" + bounds = self._o12_cycle_zero_feedback_bounds(task, direction) + if bounds is None: + return None + # Cycle-zero bounds define the normalized physical coordinate. + return 1.0 if bounds[1] - bounds[0] > 1.0e-9 else 0.0 + def _declare_parameters(self) -> None: super()._declare_parameters() self.declare_parameter("maximum_temperature_c", 70) self.declare_parameter("temperature_report_required", False) self.declare_parameter("temperature_fallback_after_seconds", 5.0) self.declare_parameter("motion_speed_scale", 1.0) + self.declare_parameter("trajectory_ramp_seconds", 0.4) def _load_parameters(self) -> None: super()._load_parameters() @@ -92,12 +791,17 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): self.motion_speed_scale = float( self.get_parameter("motion_speed_scale").value ) + self.trajectory_ramp_seconds = float( + self.get_parameter("trajectory_ramp_seconds").value + ) if not 40 <= self.maximum_temperature_c <= 90: raise ValueError("maximum_temperature_c must be in [40, 90]") if not 1.0 <= self.temperature_fallback_after_seconds <= 30.0: raise ValueError("temperature_fallback_after_seconds must be in [1, 30]") - if not 0.5 <= self.motion_speed_scale <= 2.0: - raise ValueError("motion_speed_scale must be in [0.5, 2.0]") + if not 0.5 <= self.motion_speed_scale <= 4.0: + raise ValueError("motion_speed_scale must be in [0.5, 4.0]") + if not 0.2 <= self.trajectory_ramp_seconds <= 1.0: + raise ValueError("trajectory_ramp_seconds must be in [0.2, 1.0]") def _temperature_ready(self) -> bool: return self.temperature_verified or self.temperature_fallback_active @@ -132,18 +836,120 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): self.mode_verified = True def _error_callback(self, message: Int16MultiArray) -> None: - self.latest_errors = tuple(int(value) for value in message.data) - if len(self.latest_errors) != 12: + now = time.monotonic() + assessment = assess_o12_error_report(message.data) + self.latest_errors = assessment.codes + self.error_report_count += 1 + if not assessment.valid: + self.error_verified = False + self.error_health_classification = "invalid_error_report" self._pause("invalid_error_report_length") - elif any(self.latest_errors): - self._pause("o12_error_report_nonzero") - else: + return + if assessment.active_fault_channels or assessment.unknown_bit_channels: + self.error_verified = False + self.error_health_classification = "active_motor_fault" + self._pause("o12_active_motor_fault") + return + if assessment.clear: self.error_verified = True + self.error_health_classification = "clear" + self.error_report_matching_codes = () + self.error_report_matching_count = 0 + self.error_report_first_matching_at = 0.0 + self.confirmed_historical_communication_channels = () + return + + # bit4 is a latched/diagnostic communication flag, not proof that the + # live command/feedback path has disappeared. This hand can raise a + # new bit4 while all 12 feedback channels continue at full rate. Do + # not terminate a scan from one report: repeated health queries + # classify a fresh-feedback flag as historical, while the independent + # feedback-age and no-progress watchdogs stop a real communication + # loss. Bits 0..3 above remain immediate hard faults. + if assessment.codes != self.error_report_matching_codes: + self.error_report_matching_codes = assessment.codes + self.error_report_matching_count = 1 + self.error_report_first_matching_at = now + self.error_verified = False + self.error_health_classification = "confirming_historical_communication" + append_jsonl(self.raw_path, { + "kind": "o12_communication_flag_observed", + "while_started": bool(self.started), + "error_codes": list(assessment.codes), + "communication_channels": [ + self.command_names[index] + for index in assessment.communication_channels + ], + "feedback_hz": round(float(self._feedback_hz()), 3), + "feedback_age_seconds": round( + float(self._feedback_age_seconds(now)), 6 + ), + "action": "observe_live_feedback_and_requery", + }) + else: + self.error_report_matching_count += 1 + self._update_error_health(now) + + def _feedback_age_seconds(self, now: float) -> float: + if not self.state_receive_times: + return math.inf + return max(0.0, now - float(self.state_receive_times[-1])) + + def _update_error_health(self, now: float) -> None: + assessment = assess_o12_error_report(self.latest_errors) + if assessment.clear or not assessment.communication_only: + return + elapsed = max(0.0, now - self.error_report_first_matching_at) + feedback_hz = float(self._feedback_hz()) + feedback_age = self._feedback_age_seconds(now) + if not historical_communication_latch_confirmed( + assessment=assessment, + matching_report_count=self.error_report_matching_count, + observation_seconds=elapsed, + feedback_hz=feedback_hz, + feedback_age_seconds=feedback_age, + minimum_feedback_hz=float(self.minimum_feedback_hz), + ): + self.error_verified = False + self.error_health_classification = "confirming_historical_communication" + return + self.error_verified = True + self.error_health_classification = "historical_communication_latch" + self.confirmed_historical_communication_channels = tuple( + assessment.communication_channels + ) + if self.error_health_audit_written: + return + self.error_health_audit_written = True + append_jsonl(self.raw_path, { + "kind": "o12_error_health_classification", + "policy_version": 1, + "classification": self.error_health_classification, + "error_codes": list(assessment.codes), + "communication_channels": [ + self.command_names[index] + for index in assessment.communication_channels + ], + "matching_report_count": self.error_report_matching_count, + "observation_seconds": round(elapsed, 6), + "feedback_hz": round(feedback_hz, 3), + "feedback_age_seconds": round(feedback_age, 6), + "decision_basis": ( + "vendor_documented_historical_bit4_plus_fresh_" + "command_triggered_feedback" + ), + }) + self.get_logger().warning( + "O12 commu_except bit4 is a confirmed historical latch; " + "active communication remains protected by feedback freshness" + ) def _temperature_callback(self, message: Int16MultiArray) -> None: self.latest_temperatures = tuple(int(value) for value in message.data) if len(self.latest_temperatures) != 12: - self._pause("invalid_temperature_report_length") + # This firmware commonly returns an empty/unsupported response. + # Error-code bit1 remains the over-temperature interlock. + self.temperature_verified = False elif any(value >= self.maximum_temperature_c for value in self.latest_temperatures): self._pause("o12_over_temperature") else: @@ -157,8 +963,109 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): mode.data = [POSITION_MODE] * 12 self.control_mode_publisher.publish(mode) self.error_query_publisher.publish(Empty()) - self.temperature_query_publisher.publish(Empty()) - self.last_health_query_at = time.monotonic() + now = time.monotonic() + # This O12 firmware returns an empty temperature report only after a + # long blocking timeout. In the default capability mode, do not put + # that unsupported request in front of error queries. Overheat stays + # protected by the independently queried error-report bit1. + if ( + self.temperature_report_required + and not self.temperature_verified + and now - self.last_temperature_query_at >= 5.0 + ): + self.temperature_query_publisher.publish(Empty()) + self.last_temperature_query_at = now + self.last_health_query_at = now + + def _restore_resume_checkpoint(self) -> None: + source = self.resume_raw_samples_path + assert source is not None + if ( + source.name != "raw_samples.jsonl" + or source.parent == self.session_dir + or source.parent.parent != self.session_dir.parent + ): + raise RuntimeError( + "O12 resume raw must come from an older direct sibling session" + ) + rows = load_o12_raw_samples(source) + starts = [row for row in rows if row.get("kind") == "session_start"] + if len(starts) != 1: + raise RuntimeError("O12 resume raw must contain one session_start") + start = starts[0] + if ( + start.get("profile_id") != self.profile.key.profile_id + or start.get("serial_number") != self.serial_number + or int(start.get("sample_schema_version", -1)) + != self.profile.artifacts.output_schema_version + ): + raise RuntimeError("O12 resume identity or schema differs") + recorded = { + key: str(start.get(key, "")) + for key in self.profile.artifacts.protected_input_fields + } + if all(recorded.values()): + if recorded != self.protected_inputs: + raise RuntimeError("O12 resume protected input hashes differ") + compatibility = "protected_hashes_v1" + elif any( + row.get("kind") == "o12_temperature_capability_fallback" + and row.get("sdk_config_sha256") + == self.protected_inputs["sdk_config_sha256"] + for row in rows + ): + # The product runner independently attests unchanged local files + # before it passes a legacy pre-checkpoint session to the node. + compatibility = "legacy_o12_local_inputs_unchanged" + else: + raise RuntimeError("O12 legacy resume checkpoint is not attested") + checkpoint = build_resume_checkpoint_from_rows( + self.profile, + source.parent, + rows, + compatibility=compatibility, + ) + self.resume_source_session = checkpoint.source_session.name + self.resume_compatibility = checkpoint.compatibility + self.resumed_unit_keys = frozenset(checkpoint.completed_units) + self.resumed_task_keys = frozenset(checkpoint.completed_tasks) + audit = { + "kind": "o12_resume_checkpoint_import", + "source_session": self.resume_source_session, + "compatibility": self.resume_compatibility, + "completed_unit_count": len(self.resumed_unit_keys), + "completed_task_keys": sorted(self.resumed_task_keys), + "first_incomplete_unit": next( + ( + [task.key, cycle, direction] + for task in self.profile.motion.tasks + for cycle in (0, 1, 2, 3) + for direction in ("decreasing", "increasing") + if (task.key, cycle, direction) + not in self.resumed_unit_keys + ), + None, + ), + } + # raw_records is the live fitting/quality sample set inherited from + # the acquisition engine. Keep checkpoint audit metadata in the + # append-only session file only: it has no task_name and must never be + # presented to the per-task sample qualifier. + persisted_records = [audit] + for record in checkpoint.imported_records: + copied = dict(record) + persisted_records.append(copied) + if copied.get("kind") in { + "o12_joint_sample", "o12_roll_cross_view_sample" + }: + self.raw_records.append(copied) + # A complete O12 checkpoint contains tens of thousands of records. + # One fsync per imported row can keep the constructor from creating + # its executor/status timer for minutes, which the runner then + # misreports as a device preflight failure. Persist the already + # validated checkpoint as one durable batch without changing which + # rows are imported or the resume compatibility contract. + append_jsonl_many(self.raw_path, persisted_records) @staticmethod def _full_target(**values: float) -> tuple[float, ...]: @@ -180,7 +1087,7 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): steps: list[MotionStep] = [ MotionStep( - "baseline", None, None, 0.0, scaled(0.10, 0.15), + "baseline", None, None, 0.0, scaled(0.10, 0.20), target_command=self._full_target(), ) ] @@ -190,65 +1097,339 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): if group != previous_group: if group == "middle": outer = self._full_target( - ring_mcp=PARK_FINGER_RAD, - pinky_mcp=PARK_FINGER_RAD, + ring_mcp=PARK_RING_MCP_RAD, + pinky_mcp=PARK_PINKY_MCP_RAD, ) - steps.append(MotionStep("clearance_outer", None, None, 0.0, scaled(0.10, 0.15), target_command=outer)) + steps.append(MotionStep( + "clearance_outer", None, None, 0.0, + scaled(0.10, 0.20), target_command=outer, + required_endpoint_indices=(10, 11), + )) target = self._full_target( index_abad=INDEX_CLEARANCE_RAD, - ring_mcp=PARK_FINGER_RAD, - pinky_mcp=PARK_FINGER_RAD, + index_mcp=INDEX_CLEARANCE_MCP_RAD, + ring_mcp=PARK_RING_MCP_RAD, + pinky_mcp=PARK_PINKY_MCP_RAD, ) - steps.append(MotionStep("clearance_splay", None, None, 0.0, scaled(0.04, 0.08), target_command=target)) + steps.append(MotionStep( + "clearance_splay", None, None, 0.0, + scaled(0.04, 0.12), target_command=target, + required_endpoint_indices=(4, 5, 10, 11), + )) elif group == "index": splay_zero = self._full_target( - ring_mcp=PARK_FINGER_RAD, - pinky_mcp=PARK_FINGER_RAD, + ring_mcp=PARK_RING_MCP_RAD, + pinky_mcp=PARK_PINKY_MCP_RAD, ) - steps.append(MotionStep("clearance_splay", None, None, 0.0, scaled(0.04, 0.08), target_command=splay_zero)) + steps.append(MotionStep( + "clearance_splay", None, None, 0.0, + scaled(0.04, 0.12), target_command=splay_zero, + required_endpoint_indices=(7, 10, 11), + )) target = self._full_target( middle_mcp=PARK_MIDDLE_MCP_RAD, middle_pip=PARK_MIDDLE_PIP_RAD, - ring_mcp=PARK_FINGER_RAD, - pinky_mcp=PARK_FINGER_RAD, + ring_mcp=PARK_RING_MCP_RAD, + pinky_mcp=PARK_PINKY_MCP_RAD, ) - steps.append(MotionStep("clearance", None, None, 0.0, scaled(0.10, 0.15), target_command=target)) + steps.append(MotionStep( + "clearance", None, None, 0.0, + scaled(0.10, 0.20), target_command=target, + required_endpoint_indices=(7, 8, 9, 10, 11), + )) previous_group = group - probe_delta = math.copysign( - min(abs(task.end_value - task.start_value), math.radians(3.0)), - task.end_value - task.start_value, - ) - for target in (task.start_value, task.start_value + probe_delta, task.start_value): + engine = getattr(self, "calibration_engine", None) + if engine is None: + engine = CalibrationEngine(self.profile) + probe_delta = engine.mapping_probe_delta(task) + assert probe_delta is not None + for target, relative_delta in ( + (task.start_value, None), + (task.start_value + probe_delta, probe_delta), + (task.start_value, None), + ): steps.append(MotionStep( "preflight", task.key, task.command_index, target, - scaled(float(task.preflight_speed or 0.02), 0.04), + scaled(float(task.preflight_speed or 0.02), 0.06), + relative_feedback_delta=relative_delta, )) for cycle in (0, 1, 2, 3): - formal_speed = scaled(float(task.formal_speed)) + formal_speed = scaled( + float(task.formal_speed), + FORMAL_SPEED_CAP_RAD_S[int(task.command_index)], + ) steps.extend(( - MotionStep("prepare", task.key, task.command_index, task.start_value, formal_speed, cycle), MotionStep("sweep", task.key, task.command_index, task.end_value, formal_speed, cycle, "decreasing"), - MotionStep("prepare", task.key, task.command_index, task.end_value, formal_speed, cycle), MotionStep("sweep", task.key, task.command_index, task.start_value, formal_speed, cycle, "increasing"), )) # Collision-aware return: side-swing neutral, middle open, then outer pair. middle_and_outer_parked = self._full_target( middle_mcp=PARK_MIDDLE_MCP_RAD, middle_pip=PARK_MIDDLE_PIP_RAD, - ring_mcp=PARK_FINGER_RAD, pinky_mcp=PARK_FINGER_RAD + ring_mcp=PARK_RING_MCP_RAD, pinky_mcp=PARK_PINKY_MCP_RAD ) outer_parked = self._full_target( - ring_mcp=PARK_FINGER_RAD, pinky_mcp=PARK_FINGER_RAD + ring_mcp=PARK_RING_MCP_RAD, pinky_mcp=PARK_PINKY_MCP_RAD ) - steps.append(MotionStep("return_splay_zero", None, None, 0.0, scaled(0.04, 0.08), target_command=middle_and_outer_parked)) - steps.append(MotionStep("return_middle_open", None, None, 0.0, scaled(0.10, 0.15), target_command=outer_parked)) - steps.append(MotionStep("return_outer_open", None, None, 0.0, scaled(0.10, 0.15), target_command=self._full_target())) - return steps + steps.append(MotionStep( + "return_splay_zero", None, None, 0.0, scaled(0.04, 0.12), + target_command=middle_and_outer_parked, + required_endpoint_indices=(7, 8, 9, 10, 11), + )) + steps.append(MotionStep( + "return_middle_open", None, None, 0.0, scaled(0.10, 0.20), + target_command=outer_parked, + required_endpoint_indices=(8, 9, 10, 11), + )) + steps.append(MotionStep( + "return_outer_open", None, None, 0.0, scaled(0.10, 0.20), + target_command=self._full_target(), + required_endpoint_indices=(10, 11), + )) + completed_units = set(getattr(self, "resumed_unit_keys", ())) + completed_tasks = set(getattr(self, "resumed_task_keys", ())) + if not completed_units: + return steps + filtered: list[MotionStep] = [] + skipped = 0 + for step in steps: + skip = ( + step.phase == "preflight" + and step.task_key in completed_tasks + ) or ( + step.recording + and ( + str(step.task_key), int(step.cycle), str(step.direction) + ) in completed_units + ) + if skip: + skipped += 1 + else: + filtered.append(step) + first_scan = next( + ( + (index, step) + for index, step in enumerate(filtered) + if step.recording + ), + None, + ) + if first_scan is not None and first_scan[1].direction == "increasing": + index, step = first_scan + task = self._task(str(step.task_key)) + # A recovered increasing sweep normally starts where its preceding + # decreasing sweep ended. Because that passed decreasing unit was + # skipped, recreate only the endpoint pose without recording. + filtered.insert(index, MotionStep( + "resume_prepare", + task.key, + task.command_index, + task.end_value, + step.speed_u8, + step.cycle, + attempt=step.attempt, + )) + self.resume_skipped_step_count = skipped + return filtered + + def _radian_trajectory_fraction( + self, distance: float, elapsed: float, maximum_speed: float + ) -> tuple[float, float, float]: + value, phase, duration = cosine_ramp_velocity_trajectory_rad( + 0.0, + float(distance), + float(elapsed), + float(maximum_speed), + self.trajectory_ramp_seconds, + ) + fraction = 1.0 if distance <= 0.0 else value / float(distance) + return fraction, phase, duration + + def _o12_measured_channel_travel_rad(self, index: int) -> float | None: + """Return cycle-zero feedback travel for one clearance actuator. + + Ring has no visual task, so its endpoint contract deliberately reuses + the pinky actuator's measured feedback travel while retaining ring's + own command and CAD geometry. + """ + source_index = 11 if int(index) == 10 else int(index) + groups: dict[tuple[str, int], list[float]] = {} + for row in self.raw_records: + if row.get("kind") != "o12_joint_sample": + continue + if int(row.get("cycle", -1)) != 0: + continue + if int(row.get("motor_index", -1)) != source_index: + continue + value = float(row.get("feedback_rad", math.nan)) + if not math.isfinite(value): + continue + key = ( + str(row.get("direction", "")), + int(row.get("attempt", 1)), + ) + groups.setdefault(key, []).append(value) + spans = [max(values) - min(values) for values in groups.values()] + travel = max(spans, default=0.0) + return float(travel) if travel > 1.0e-9 else None + + def _minimum_radian_feedback_travel(self, step: MotionStep) -> float: + """Avoid a second command-scale endpoint gate on O12 moves. + + Mapping probes retain the generic minimum-motion evidence. Clearance + and return waypoints are checked per axis below against measured O12 + travel; other positioning moves only need the live no-progress guard. + """ + if step.phase == "preflight": + return L6ThreeCameraCalibrationNode._minimum_radian_feedback_travel( + self, step + ) + return 0.0 + + def _tick_radian_motion(self, step: MotionStep, now: float) -> None: + if ( + self.step_trajectory_phase >= 1.0 + and getattr(step, "required_endpoint_indices", ()) + and len(self.latest_state_u8) == self.command_count + ): + travel = self._radian_feedback_travel(step) + if travel >= float(self.step_last_distance_u8) + 0.001: + self.step_last_distance_u8 = travel + self.step_last_progress_at = now + target = self._target_command(step) + outside = [] + for index in getattr(step, "required_endpoint_indices", ()): + index = int(index) + tolerance = ( + CLEARANCE_SPLAY_ENDPOINT_TOLERANCE_RAD + if index in {4, 7} + else CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD + ) + command_delta = ( + float(target[index]) + - float(self.step_start_state_u8[index]) + ) + if index in {4, 7}: + # ABAD feedback has only a small zero bias, so the explicit + # 0-rad clearance requirement can be checked directly. + error = abs( + float(self.latest_state_u8[index]) + - float(target[index]) + ) + if error > tolerance: + outside.append((index, "error", error, tolerance)) + continue + if abs(command_delta) <= 0.005: + # This flex axis was verified by the preceding waypoint; + # require it to remain there without assuming feedback and + # command radians have identical endpoint zero/scale. + drift = abs( + float(self.latest_state_u8[index]) + - float(self.step_start_feedback_u8[index]) + ) + if drift > tolerance: + outside.append((index, "drift", drift, tolerance)) + continue + feedback_delta = ( + float(self.latest_state_u8[index]) + - float(self.step_start_feedback_u8[index]) + ) + measured_travel = self._o12_measured_channel_travel_rad(index) + if measured_travel is None: + # This only applies to a small auxiliary pose before that + # actuator's visual scan. Check its endpoint directly; + # do not invent a feedback/command scale relationship. + error = abs( + float(self.latest_state_u8[index]) + - float(target[index]) + ) + if error > CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD: + outside.append(( + index, + "error", + error, + CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD, + )) + continue + progress = ( + feedback_delta * (1.0 if command_delta > 0.0 else -1.0) + / measured_travel + ) + minimum = float(CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION) + if progress < minimum: + outside.append(( + index, "measured_progress", progress, minimum + )) + if outside: + self.step_hold_since = None + if ( + now - self.step_last_progress_at + > float(self.motor_stall_timeout_seconds) + ): + index, metric, value, limit = outside[0] + self._pause( + "mechanical_stall:clearance_endpoint:" + f"channel={self.command_names[index]}:" + f"metric={metric}:value={value:.6f}:limit={limit:.6f}" + ) + return + super()._tick_radian_motion(step, now) + + def _target_command(self, step: LegacyMotionStep) -> tuple[float, ...]: + resolved = self.resolved_probe_target_by_step.get(id(step)) + return resolved if resolved is not None else super()._target_command(step) + + def _begin_step_without_vision_callback( + self, step: LegacyMotionStep + ) -> None: + relative_delta = getattr(step, "relative_feedback_delta", None) + ready_to_begin = ( + relative_delta is not None + and self.commanded_speed == step.speed_u8 + and time.monotonic() >= self.step_speed_ready_at + and len(self.latest_state_u8) == self.command_count + ) + if ready_to_begin and id(step) not in self.resolved_probe_target_by_step: + if step.command_index is None: + raise ValueError("O12 relative probe requires one command channel") + index = int(step.command_index) + origin = float(self.latest_state_u8[index]) + delta = float(relative_delta) + resolved_value = float(np.clip( + origin + delta, + self.command_lower[index], + self.command_upper[index], + )) + if abs(resolved_value - origin) + 1.0e-9 < abs(delta): + self._pause( + "feedback_relative_probe_outside_command_domain:" + f"{step.task_key}:origin={origin}:delta={delta}" + ) + return + target = list(super()._target_command(step)) + target[index] = resolved_value + self.resolved_probe_target_by_step[id(step)] = tuple(target) + self.probe_feedback_origin_by_step[id(step)] = origin + super()._begin_step_without_vision_callback(step) + if self.step_command_sent and relative_delta is not None: + resolved = self._target_command(step) + self.reason = ( + f"{step.phase}:{step.task_key or 'all'}:cycle={step.cycle}:" + f"direction={step.direction}:target=" + f"{resolved[int(step.command_index)]}:attempt={step.attempt}" + ) def _start(self, request, response): - if not (self.mode_verified and self.error_verified and self._temperature_ready()): + if not ( + self.mode_verified + and self.error_verified + and self._temperature_ready() + and self._feedback_hz() >= float(self.minimum_feedback_hz) + ): response.success = False - response.message = "O12 POSITION mode/error/temperature precheck is incomplete" + response.message = ( + "O12 POSITION/health/control-stream precheck is incomplete" + ) return response return super()._start(request, response) @@ -280,30 +1461,135 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): float(value) for value in axis ) + def _locked_reference_poses_for_capture( + self, view: str, step: MotionStep | None + ) -> dict[str, SquareTagPose]: + """Reuse the unobstructed O12 palm pose after outer-finger clearance. + + Maximum ring/pinky flexion intentionally covers front Tag 0. The hand + base and cameras are fixed throughout one session, so a palm pose + solved from the pre-clearance median corners is the correct reference + for middle/index roll. Moving Tags remain live requirements. + """ + if str(view) != "front": + return {} + base_role = next( + tag.role for tag in self._view(view).tags if tag.fixed_reference + ) + locked = self.locked_base_pose_by_view.get(view) + if locked is None: + corners = self.base_corner_reference.get(view) + if corners is not None: + locked, reason = self.trackers[view].estimate( + base_role, + np.asarray(corners, dtype=float), + tag_size_m=float(self.tag_size_m), + camera_matrix=self.camera_matrices[view], + stamp_ns=int(self.get_clock().now().nanoseconds), + ) + if locked is not None: + self.locked_base_pose_by_view[view] = locked + append_jsonl(self.raw_path, { + "kind": "o12_fixed_base_reference_locked", + "view": view, + "role": base_role, + "tag_id": next( + int(tag.tag_id) for tag in self._view(view).tags + if tag.role == base_role + ), + "corner_sample_count": len( + self.base_corner_observations[view] + ), + "pnp_reprojection_error_px": round( + float(locked.reprojection_error_px), 6 + ), + }) + elif reason: + self._count_step_rejection( + f"locked_base_pnp:{base_role}:{reason}" + ) + if locked is None or step is None or step.task_key is None: + return {} + group = str(step.task_key).split("_", 1)[0] + if group not in {"middle", "index"}: + return {} + return {base_role: locked} + def _finish_step(self, step: MotionStep) -> None: previous_task = step.task_key - if step.phase == "preflight" and step.task_key is not None: - task = self._task(step.task_key) - is_probe = abs(float(step.target_u8) - task.start_value) > 1.0e-9 - if is_probe: - rotation = self.preflight_maximum_rotation_by_task.get(task.key, 0.0) - if rotation < math.radians(0.25): - self._pause(f"mapping_preflight_no_target_motion:{task.key}") - return - append_jsonl(self.raw_path, { - "kind": "o12_fixed_mapping_preflight", - "task_name": task.key, - "motor_index": task.command_index, - "sdk_channel": self.command_names[task.command_index], - "urdf_joint": task.joints[0], - "command_delta_rad": round(float(step.target_u8) - task.start_value, 9), - "observed_rotation_rad": round(rotation, 9), - "observed_axis_tag_frame_xyz": list( - self.measured_direction_axis_by_task[task.key] - ), - "channel_exchange_allowed": False, - }) - super()._finish_step(step) + with self.step_data_lock: + if self.vision_callbacks_inflight: + return + if step.phase == "preflight" and step.task_key is not None: + task = self._task(step.task_key) + is_probe = abs(float(step.target_u8) - task.start_value) > 1.0e-9 + if is_probe: + rotation = self.preflight_maximum_rotation_by_task.get(task.key, 0.0) + expected_delta = float(step.relative_feedback_delta) + feedback_delta = ( + float(self.latest_state_u8[task.command_index]) + - float(self.probe_feedback_origin_by_step[id(step)]) + ) + projected_feedback_travel = feedback_delta * ( + 1.0 if expected_delta > 0.0 else -1.0 + ) + if projected_feedback_travel < min( + 0.004, 0.25 * abs(expected_delta) + ): + self._pause( + "mapping_preflight_wrong_feedback_direction:" + f"{task.key}" + ) + return + visual_verified = rotation >= math.radians(0.25) + measured_axis = self.measured_direction_axis_by_task.get( + task.key + ) + append_jsonl(self.raw_path, { + "kind": "o12_fixed_mapping_preflight", + "task_name": task.key, + "motor_index": task.command_index, + "sdk_channel": self.command_names[task.command_index], + "urdf_joint": task.joints[0], + "command_delta_rad": round( + float(step.relative_feedback_delta), 9 + ), + "feedback_origin_rad": round( + float(self.probe_feedback_origin_by_step[id(step)]), 9 + ), + "absolute_command_target_rad": round( + float(self._target_command(step)[ + int(task.command_index) + ]), + 9, + ), + "feedback_travel_rad": round( + float(self._radian_feedback_travel(step)), 9 + ), + "projected_feedback_travel_rad": round( + projected_feedback_travel, 9 + ), + "observed_rotation_rad": round(rotation, 9), + "observed_axis_tag_frame_xyz": ( + None if measured_axis is None else list(measured_axis) + ), + "visual_mapping_verified": visual_verified, + "verification_basis": ( + "feedback_and_visual" + if visual_verified + else "feedback_only_live_tag_unavailable" + ), + "channel_exchange_allowed": False, + }) + super()._finish_step(step) + if not self.step_command_sent: + self.resolved_probe_target_by_step.pop(id(step), None) + self.probe_feedback_origin_by_step.pop(id(step), None) + # The next step is already selected at this boundary. Clear + # the completed sweep counters immediately so status and + # safety logic cannot present or interpret stale scan state + # while waiting for the next timer tick to begin it. + self._reset_step_observation_counters(()) next_step = self._current_step() if self.state == "RUNNING" and ( next_step is None @@ -312,29 +1598,92 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): ): self._query_health() + def _finalize(self) -> None: + """Expose the long O12 spatial solve before running it synchronously. + + A complete 16-Tag session contains tens of thousands of pose records; + the spatial solve and guarded URDF write can legitimately take close + to a minute. Publish the phase transition immediately so the outer + process monitor does not apply its three-second *runtime* heartbeat + rule to this CPU-bound, non-motion stage. + """ + self.state = "FINALIZING" + self.reason = "fitting_validating_and_writing_artifacts" + self._publish_status() + super()._finalize() + def _tick(self) -> None: now = time.monotonic() + self._update_error_health(now) + if ( + self.started + and self.error_health_classification + == "confirming_historical_communication" + and now - self.last_health_query_at >= 0.5 + ): + self._query_health() self._activate_temperature_fallback_if_allowed(now) + if ( + self.started + and self.state not in {"PAUSED", "ABORTED", "PASSED"} + and self._feedback_age_seconds(now) > 1.0 + ): + self._pause("o12_feedback_stream_timeout") + return if not self.started and self.state not in {"PAUSED", "ABORTED", "PASSED"}: if now - self.last_health_query_at >= 1.0: self._query_health() - # The SDK's joint state is command-triggered; a 20 Hz zero command - # supplies the feedback stream used by READY and frequency checks. + # The SDK's joint state is command-triggered. The configured zero + # stream supplies READY feedback and verifies the smooth cadence. self._publish_command(list(self.baseline_command)) super()._tick() if ( not self.started and self.state == "READY" - and not (self.mode_verified and self.error_verified and self._temperature_ready()) + and not ( + self.mode_verified + and self.error_verified + and self._temperature_ready() + and self._feedback_hz() >= float(self.minimum_feedback_hz) + ) ): self.state = "WAIT_DEVICES" - self.reason = "waiting_for_o12_position_mode_and_health_reports" + self.reason = "waiting_for_o12_position_health_and_smooth_stream" def _status(self): value = super()._status() + # The common status payload retains the endpoint error used by legacy + # byte-command hands. During an O12 continuous trajectory that makes + # a healthy in-flight move look far from its final endpoint. Report + # O12 tracking against the command actually published this tick while + # keeping target_state_rad as the separate final destination. + value = self._apply_o12_tracking_diagnostics(value) + skipped = int(self.resume_skipped_step_count) + if skipped: + value["step_index"] = int(value["step_index"]) + skipped + value["step_count"] = int(value["step_count"]) + skipped + step = self._current_step() + locked_front_active = bool( + "front" in self.locked_base_pose_by_view + and step is not None + and step.task_key is not None + and str(step.task_key).split("_", 1)[0] in {"middle", "index"} + and self._task(str(step.task_key)).view == "front" + ) value.update({ "position_mode_verified": self.mode_verified, "error_report_verified": self.error_verified, + "error_health_classification": self.error_health_classification, + "error_report_count": self.error_report_count, + "error_report_matching_count": self.error_report_matching_count, + "error_faults": list(decoded_faults(self.latest_errors)), + "confirmed_historical_communication_channels": [ + self.command_names[index] + for index in self.confirmed_historical_communication_channels + ], + "feedback_age_seconds": round( + self._feedback_age_seconds(time.monotonic()), 6 + ), "temperature_report_verified": self.temperature_verified, "temperature_fallback_active": self.temperature_fallback_active, "temperature_safety_policy": ( @@ -347,6 +1696,38 @@ class O12ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): "latest_error_codes": list(self.latest_errors), "latest_temperatures_c": list(self.latest_temperatures), "motion_speed_scale": self.motion_speed_scale, + "trajectory_ramp_seconds": self.trajectory_ramp_seconds, + "locked_reference_tag_ids": [0] if locked_front_active else [], + "feedback_coupling": dict( + self.latest_o12_feedback_coupling + ), + "auxiliary_tracking": dict( + self.latest_o12_auxiliary_tracking + ), + "roll_cross_view": { + **self._o12_cross_view_metrics(), + "active": bool( + self._roll_cross_view_observer() is not None + and self._current_step() is not None + and self._current_step().recording + ), + "valid_frames": self.o12_cross_view_valid_frames, + "total_frames": self.o12_cross_view_total_frames, + "recognized_tag_ids": list( + self.o12_cross_view_recognized_tag_ids + ), + "unrecognized_tag_ids": list( + self.o12_cross_view_unrecognized_tag_ids + ), + }, + "resume": { + "used": bool(self.resumed_unit_keys), + "source_session": self.resume_source_session, + "compatibility": self.resume_compatibility, + "completed_unit_count": len(self.resumed_unit_keys), + "completed_task_keys": sorted(self.resumed_task_keys), + "skipped_step_count": skipped, + }, }) return value @@ -356,7 +1737,15 @@ def main(args: list[str] | None = None) -> None: node: O12ThreeCameraCalibrationNode | None = None try: node = O12ThreeCameraCalibrationNode() - rclpy.spin(node) + # One worker for motion/feedback and one for each camera. The camera + # subscriptions use separate callback groups in the shared node base, + # so a busy front detector cannot starve the side observer required by + # O12 roll calibration. + executor = MultiThreadedExecutor( + num_threads=1 + len(node.profile.vision.view_names) + ) + executor.add_node(node) + executor.spin() except KeyboardInterrupt: pass finally: diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py new file mode 100644 index 0000000..35d6c7e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py @@ -0,0 +1,139 @@ +"""Offline thumb candidate resolution using training data only. + +Re-selects actual corner-derived IPPE solutions; does not manufacture rigid +poses, substitute SDK angles, or relax the spatial solver's acceptance gates. +""" +from itertools import product +import math + +import numpy as np +from scipy.spatial.transform import Rotation as R + +from ...pnp import solve_square_tag_ippe +from .pnp import THUMB_ROLES + +POLICY = 'o12_thumb_training_candidates_v1' +MATRIX_SOURCE = 'CameraInfo.P[:3,:3]' + + +def _rotation(pose): + return R.from_quat(pose['quaternion_xyzw']) + + +def _relative(parent, child): + rp = _rotation(parent) + return ((rp.inv()*_rotation(child)).as_quat(), + rp.inv().apply(np.asarray(child['translation_xyz_m'])-parent['translation_xyz_m'])) + + +def _trajectory_score(parent, child): + pairs = [_relative(a, b) for a, b in zip(parent, child)] + rotations = R.from_quat([q for q, _ in pairs]) + points = np.asarray([p for _, p in pairs]) + vectors = (rotations*rotations[0].inv()).as_rotvec() + axis = np.linalg.svd(vectors, full_matrices=False)[2][0] + projected = R.from_rotvec((vectors@axis)[:, None]*axis) + off_axis = float(np.sqrt(np.mean((projected.inv()*rotations*rotations[0].inv()).magnitude()**2))) + # Free six-parameter fixed-pivot fit, no CAD zero or mounting-angle prior. + a = np.concatenate((np.tile(np.eye(3), (len(points), 1, 1)), rotations.as_matrix()), axis=2).reshape(-1, 6) + predicted = (a@np.linalg.lstsq(a, points.ravel(), rcond=None)[0]).reshape(-1, 3) + rms = float(np.sqrt(np.mean(np.sum((points-predicted)**2, axis=1)))) + return {'pivot_rms_m': rms, 'off_axis_rms_rad': off_axis, + 'score': rms/.0015 + off_axis/math.radians(1)} + + +def resolve_thumb_observations(records, *, projection_override=None): + """Return copied observations and auditable candidate-selection evidence. + + Older captures did not save P. They remain on the legacy path; K is never + guessed to be P. An explicit override is for isolated offline diagnosis + and must be reported as such, not advertised as a verified whole session. + """ + rows = [dict(r) for r in records] + joint_rows = [r for r in rows if r.get('kind') == 'o12_joint_sample' + and r.get('task_name') == 'thumb_mcp_dip_front'] + evidence = {int(r['image_stamp_ns']): r for r in rows + if r.get('kind') == 'o12_pnp_candidate_frame' + and r.get('task_name') == 'thumb_mcp_dip_front'} + report = {'policy': POLICY, 'training_cycles': [0, 1, 2], 'holdout_cycle': 3, + 'projection_override_used': projection_override is not None, + 'is_accuracy_certificate': False} + if not evidence: + return rows, {**report, 'status': 'legacy_no_corners'} + if projection_override is None and any( + r.get('camera_matrix_source') != MATRIX_SOURCE for r in evidence.values()): + return rows, {**report, 'status': 'legacy_projection_unverified', + 'reason': 'Recorded matrix may be raw K; no silent K-to-P substitution.'} + # Use only the same accepted attempt as the downstream fitter. + latest = {} + for r in joint_rows: + key = (int(r['cycle']), r['direction']) + latest[key] = max(latest.get(key, 0), int(r.get('attempt', 1))) + chosen_rows = [r for r in joint_rows if int(r.get('attempt', 1)) == latest[(int(r['cycle']), r['direction'])]] + stamps = sorted({int(r['image_stamp_ns']) for r in chosen_rows}) + if not stamps or any(s not in evidence for s in stamps): + raise ValueError('O12 corner replay lacks evidence for accepted observations') + training = np.asarray([int(evidence[s]['cycle']) in (0, 1, 2) for s in stamps]) + if sum(training) < 40 or not any(int(evidence[s]['cycle']) == 3 for s in stamps): + raise ValueError('O12 corner replay requires training and independent holdout') + if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]: + raise ValueError('O12 holdout must follow the complete training trajectory') + paths = {role: [[], []] for role in THUMB_ROLES} + for stamp in stamps: + frame = evidence[stamp] + matrix = np.asarray(frame['camera_matrix'] if projection_override is None else projection_override) + for role in THUMB_ROLES: + data = frame['roles'][role] + poses = solve_square_tag_ippe(data['corners_xy'], 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'O12 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]: + previous = [_rotation(paths[role][i][-1]) for i in range(2)] + def cost(order): + return sum((previous[i].inv()*_rotation(cs[order[i]])).magnitude() for i in range(2)) + order = min(((0, 1), (1, 0)), key=cost) + cs = [cs[i] for i in order] + for i, pose in enumerate(cs): + paths[role][i].append(pose) + scores = [] + for branches in product(range(2), repeat=3): + train = [[p for p, keep in zip(paths[role][branch], training) if keep] + for role, branch in zip(THUMB_ROLES, branches)] + metrics = [_trajectory_score(train[i], train[i+1]) for i in (0, 1)] + image_penalty = float(np.mean([p['reprojection_error_px'] for ps in train for p in ps])) + scores.append({'branches': list(branches), 'score': sum(m['score'] for m in metrics)+.5*image_penalty, + 'training_pairs': metrics, 'mean_reprojection_px': image_penalty}) + selected = min(scores, key=lambda s: s['score']) + lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(THUMB_ROLES, selected['branches'])} + for i, stamp in enumerate(stamps)} + changed = 0 + for row in chosen_rows: + stamp = int(row['image_stamp_ns']) + parent_role, child_role = THUMB_ROLES[:2] if row['joint'] == 'thumb_mcp' else THUMB_ROLES[1:] + parent, child = lookup[stamp][parent_role], lookup[stamp][child_role] + # Preserve the actual camera->common transform, not an assumed identity. + old_camera_parent = evidence[stamp]['roles'][parent_role]['selected'] + if old_camera_parent is None: + raise ValueError('Accepted joint row has no original selected parent pose') + rc = _rotation(row['parent_pose_common'])*_rotation(old_camera_parent).inv() + tc = np.asarray(row['parent_pose_common']['translation_xyz_m'])-rc.apply(old_camera_parent['translation_xyz_m']) + quaternion, translation = _relative(parent, child) + changed += int((_rotation({'quaternion_xyzw': row['relative_quaternion_xyzw']}).inv()*R.from_quat(quaternion)).magnitude() > 1e-7) + row['relative_quaternion_xyzw'] = quaternion.tolist() + row['relative_translation_xyz_m'] = translation.tolist() + for field, pose in (('parent_pose_common', parent), ('child_pose_common', child)): + row[field] = {'quaternion_xyzw': (rc*_rotation(pose)).as_quat().tolist(), + 'translation_xyz_m': (rc.apply(pose['translation_xyz_m'])+tc).tolist()} + row['pnp_reprojection_error_px'] = max(parent['reprojection_error_px'], child['reprojection_error_px']) + row['pose_selection_policy'] = POLICY + if projection_override is not None: + row['projection_reprocessing_scope'] = 'thumb_only_external_projection' + return rows, {**report, 'status': 'resolved', 'selected_branches': selected['branches'], + 'hypotheses': scores, 'training_frames': int(sum(training)), + 'holdout_frames': int(sum(~training)), 'changed_joint_rows': changed} diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py index a33f2a1..86f2bf1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py @@ -3,17 +3,30 @@ from __future__ import annotations from datetime import datetime +from dataclasses import asdict, replace import json +import math import os from pathlib import Path from typing import Any, Mapping, Sequence from ...product import sha256_file +from ...runtime.engine import CalibrationEngine from ...storage import atomic_write_json -from .artifacts import build_o12_runtime_payload +from .artifacts import ( + build_o12_runtime_payload, + validate_o12_runtime_payload_against_urdf, +) from .fitting import O12FitResult, fit_o12_session -from .profile import CALIBRATED_ACTIVE_JOINTS, MEASURED_PASSIVE_JOINTS +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + MEASURED_PASSIVE_JOINTS, + STATIC_ZERO_EXCLUDED_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + build_typed_profile, +) from .urdf import O12UrdfCorrection, write_o12_corrected_urdf +from .zero import O12SpatialZeroError, SPATIAL_ZERO_POLICY MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS @@ -54,6 +67,36 @@ def accepted_records_by_joint( return result +def accepted_roll_cross_view_records( + records: Sequence[Mapping[str, Any]], +) -> dict[str, list[dict[str, Any]]]: + """Select the latest complete-attempt O12 side observations by roll joint.""" + samples = [ + dict(row) for row in records + if row.get("kind") == "o12_roll_cross_view_sample" + and str(row.get("model_joint", "")) + in {"middle_mcp_roll", "index_mcp_roll"} + ] + latest: dict[tuple[str, int, str], int] = {} + for row in samples: + key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"])) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + result = {"middle_mcp_roll": [], "index_mcp_roll": []} + for row in samples: + key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"])) + if int(row.get("attempt", 1)) != latest[key]: + continue + result[str(row["model_joint"])].append({ + "cycle": int(row["cycle"]), + "direction": str(row["direction"]), + "feedback_rad": float(row["feedback_rad"]), + "relative_quaternion_xyzw": list( + row["relative_quaternion_xyzw"] + ), + }) + return {name: rows for name, rows in result.items() if rows} + + def load_o12_raw_samples(path: str | Path) -> list[dict[str, Any]]: source = Path(path).expanduser().resolve() if not source.is_file(): @@ -97,7 +140,92 @@ def finalize_o12_session( ) -> tuple[dict[str, Any], O12FitResult, O12UrdfCorrection]: directory = Path(session_dir).expanduser().resolve() directory.mkdir(parents=True, exist_ok=True) - result = fit_o12_session(source_urdf, accepted_records_by_joint(records)) + if any(row.get("projection_reprocessing_scope") == "thumb_only_external_projection" + for row in records): + raise ValueError("Partial external camera override is diagnostic-only; " + "other task projections are unverified. Cannot finalize a whole-hand artifact.") + # Reconsider ambiguous online choices with the complete TRAINING trajectory. + # Fourth-cycle candidates are assigned using the frozen training choice; + # the original raw file is never rewritten and all quality gates remain. + from .observations import resolve_thumb_observations + try: + records, pose_selection = resolve_thumb_observations(records) + except ValueError as error: + atomic_write_json(directory / "pose_selection_diagnostics.json", { + "status": "failed", "reason": str(error), "publication_allowed": False, + }) + raise ValueError("O12 corner candidate resolution failed:" + str(error)) from error + atomic_write_json(directory / "pose_selection_diagnostics.json", pose_selection) + try: + result = fit_o12_session( + source_urdf, + accepted_records_by_joint(records), + cross_view_records_by_joint=accepted_roll_cross_view_records(records), + require_cross_view=True, + require_full_hand_spatial_zero=True, + ) + if pose_selection['status'] == 'legacy_projection_unverified': + # A successful fit cannot certify observations known to have an + # unverified rectified-pixel projection contract. Keep a review + # artifact, but never silently publish mixed K/P observations. + spatial = replace(result.full_hand_zero_result, passed=False, + failure_reasons={**result.full_hand_zero_result.failure_reasons, + 'camera_projection': 'legacy_projection_unverified'}) + error = O12SpatialZeroError( + 'O12 camera projection is unverified for legacy corner records; review only', + {'passed': False, 'stage': 'camera_projection', + 'result': asdict(spatial), 'pose_selection': pose_selection}) + error.review_fit = replace(result, full_hand_zero_result=spatial) + raise error + except O12SpatialZeroError as error: + # A review model is not a calibration PASS. Keep it out of the + # publication directory's artifact set and never produce runtime JSON. + if error.review_fit is not None: + review = directory / "review_only" + try: + if any(not math.isfinite(value) or abs(value) > math.radians(20) + for value in error.review_fit.zero_offsets_rad.values()): + raise ValueError("review zero estimate is outside the physical correction budget") + correction = write_o12_corrected_urdf( + source_urdf=source_urdf, output_directory=review, + serial_number=serial_number + "_REVIEW_ONLY", + timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"), + result=error.review_fit, + ) + atomic_write_json(review / "review_manifest.json", { + "status": "REVIEW_ONLY_NOT_CALIBRATION_PASS", + "publication_allowed": False, + "policy": SPATIAL_ZERO_POLICY, + "urdf": correction.path.name, + "source_urdf": str(Path(source_urdf).resolve()), + "source_urdf_sha256": sha256_file(source_urdf), + "candidate_urdf_sha256": sha256_file(correction.path), + "protected_inputs": dict(protected_inputs), + "static_origin_offsets_rad": dict(error.review_fit.zero_offsets_rad), + "spatial_validation": error.diagnostics, + "limitations": ["Not approved for hardware/control", + "Passive URDF mimic is a linear approximation", + "Fingertip contact accuracy not verified"], + }) + error.diagnostics = {**error.diagnostics, "review_urdf": str(correction.path)} + error.args = (str(error) + "; 仅供复核、未发布的 URDF:" + str(correction.path),) + except Exception as review_error: + # Preserve the original failure even if diagnostic export fails. + error.diagnostics = {**error.diagnostics, "review_export_error": str(review_error)} + atomic_write_json(directory / "spatial_zero_diagnostics.json", error.diagnostics) + raise + atomic_write_json(directory / "spatial_zero_diagnostics.json", { + "passed": True, "policy": SPATIAL_ZERO_POLICY, + "result": asdict(result.full_hand_zero_result), + "static_zero_exclusions": { + name: "immutable_source_cad" + for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS) + }, + }) + CalibrationEngine(build_typed_profile()).result_from_fit( + result, + transfers=TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + ) payload = build_o12_runtime_payload( serial_number=serial_number, source_urdf=source_urdf, @@ -105,6 +233,26 @@ def finalize_o12_session( protected_inputs=protected_inputs, passed=True, ) + resume_rows = [ + row for row in records + if row.get("kind") == "o12_resume_checkpoint_import" + ] + resume = ( + { + "used": True, + "source_session": str(resume_rows[-1]["source_session"]), + "compatibility": str(resume_rows[-1]["compatibility"]), + "completed_unit_count": int( + resume_rows[-1]["completed_unit_count"] + ), + "completed_task_keys": list( + resume_rows[-1].get("completed_task_keys", ()) + ), + } + if resume_rows + else {"used": False} + ) + payload["quality"]["resume"] = resume correction = write_o12_corrected_urdf( source_urdf=source_urdf, output_directory=directory, @@ -112,6 +260,9 @@ def finalize_o12_session( result=result, timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"), ) + validate_o12_runtime_payload_against_urdf( + payload, correction.path, source_urdf=source_urdf + ) json_path = directory / f"o12_right_{serial_number}_calibration.json" atomic_write_json(json_path, payload) summary = { @@ -119,13 +270,52 @@ def finalize_o12_session( "profile_id": "O12/right/o12_right_16/v1", "serial_number": str(serial_number), "result": "PASS", + "calibration_scope": payload["calibration_scope"], + "spatial_zero_policy": SPATIAL_ZERO_POLICY, + "static_origin_offsets_rad": dict(result.zero_offsets_rad), + "static_zero_methods": dict(result.zero_method_by_joint), + "static_zero_exclusions": { + name: { + "static_origin_offset_rad": result.zero_offsets_rad[name], + "source": "immutable_source_cad", + "dynamic_curve_and_holdout_passed": True, + } + for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS) + }, "measured_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS), "measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS), + "thumb_static_origin_offsets_rad": { + name: correction.origin_offsets_rad[name] + for name in ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + ) + }, + "thumb_static_zero_methods": { + name: result.zero_method_by_joint[name] + for name in ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + ) + }, + "mechanical_endpoint_origin_offsets_rad": { + name: correction.origin_offsets_rad[name] + for name in sorted(build_typed_profile().zero.endpoint_anchor_by_joint) + }, + "corrected_limits_rad": { + name: list(values) + for name, values in sorted(correction.corrected_limits_rad.items()) + }, "ring_transfer": { "source": "pinky_mcp_pitch", "target": "ring_mcp_pitch", "preserved_fields": list(correction.preserved_ring_fields), }, + "resume": resume, "artifacts": { "json": json_path.name, "urdf": correction.path.name, @@ -140,6 +330,7 @@ def finalize_o12_session( __all__ = [ - "MEASURED_JOINTS", "accepted_records_by_joint", "finalize_o12_session", + "MEASURED_JOINTS", "accepted_records_by_joint", + "accepted_roll_cross_view_records", "finalize_o12_session", "load_o12_raw_samples", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py new file mode 100644 index 0000000..5825b6e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py @@ -0,0 +1,76 @@ +"""O12 thumb-chain branch selection; never alters a measured pose or angle.""" +import math +import numpy as np +from scipy.spatial.transform import Rotation + +from ...pnp import SquareTagGroupPoseTracker + + +THUMB_ROLES = ('thumb_cmc', 'thumb_mcp', 'thumb_dip') + + +class O12ThumbPoseTracker(SquareTagGroupPoseTracker): + """Use parallel hinge directions, not a CAD zero or passive angle ratio. + + Both relative rotations live in different marker frames. Transport the + follower's rotation vector through the *reference* parent rotation before + comparing axes. Fixed arbitrary marker mounting rotations cancel out. + This is a soft ambiguity discriminator, not a new capture/stop gate. + """ + + def __init__(self): + super().__init__( + roles=THUMB_ROLES, + adjacent_pairs=(THUMB_ROLES[:2], THUMB_ROLES[1:]), + maximum_pose_jump_rad=math.pi, + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=math.radians(5), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + initialization_frames=8, + ) + self.reference = None + + def reset(self, *, preserve_task_reference=False): + super().reset(preserve_task_reference=preserve_task_reference) + self.reference = None + + @staticmethod + def relative(poses): + rotations = [Rotation.from_quat(poses[r].quaternion_xyzw) for r in THUMB_ROLES] + return rotations[0].inv()*rotations[1], rotations[1].inv()*rotations[2] + + def _informative_coupled_rotation_costs(self, combinations): + # Reuse the shared group selector's soft-cost extension point without + # installing coupled_rotation_pairs (there is NO angle-ratio prior). + if self.reference is None: + return tuple(0.0 for _ in combinations) + driver_ref, follower_ref = self.reference + residuals = [] + for poses in combinations: + driver, follower = self.relative(poses) + a = (driver*driver_ref.inv()).as_rotvec() + b = driver_ref.apply((follower*follower_ref.inv()).as_rotvec()) + if min(np.linalg.norm(a), np.linalg.norm(b)) < math.radians(5): + residuals.append(None) + continue + axis = a/np.linalg.norm(a) + residuals.append(float(np.linalg.norm(b-axis*np.dot(axis,b)))) + # Insufficient excitation or no geometrically credible candidate: + # keep ordinary visual continuity, rather than inventing a pose or + # rejecting all observations because a prior did not match. + if not any(r is not None and r < math.radians(10) for r in residuals): + return tuple(0.0 for _ in combinations) + return tuple(0.0 if r is None else r/math.radians(3) for r in residuals) + + def select(self, candidates_by_role, *, stamp_ns, **kwargs): + # O12 radians never enter the shared tracker's u8 return-path cache. + if (self._previous_stamp_ns is not None and + not 0 <= stamp_ns-self._previous_stamp_ns <= self.reset_after_ns): + self.reset() + selected, reason = super().select(candidates_by_role, stamp_ns=stamp_ns) + if selected is not None and self.reference is None: + self.reference = self.relative(selected) + return selected, reason diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py index 3395f55..4943466 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py @@ -2,9 +2,11 @@ from __future__ import annotations +from dataclasses import dataclass import math from ...core import ( + AcquisitionPolicy, ArtifactPolicy, CalibrationProfile, CommandLayout, @@ -58,15 +60,24 @@ SDK_UPPER_RAD = ( 1.53588974175501, 1.53588974175501, ) -# Source-URDF-safe intersections. The SDK exposes a larger range on several -# axes, but calibration must never command outside the immutable CAD model. -SAFE_LOWER_RAD = ( - 0.0, -0.94, -0.8272860654453121, -1.29, - -0.26, 0.0, 0.0, -0.26, 0.0, 0.0, 0.0, 0.0, +# The calibration domain is the vendor's physical O12 range. Restricting a +# scan to the source URDF would be circular: that URDF is precisely the model +# being corrected, and its smaller thumb/outer-finger limits previously hid +# the real hardware endpoints. "SAFE" is retained as the public profile name +# but now means the reviewed SDK hard range, not a CAD intersection. +SAFE_LOWER_RAD = SDK_LOWER_RAD +SAFE_UPPER_RAD = SDK_UPPER_RAD + +# Joint-state feedback can cross a commanded endpoint slightly because it is +# measured after servo tracking and encoder conversion. The envelope remains +# anchored to the reviewed SDK hard range, with an explicit two-degree +# observation allowance that is never used to generate a command. +FEEDBACK_DOMAIN_MARGIN_RAD = math.radians(2.0) +FEEDBACK_LOWER_RAD = tuple( + value - FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_LOWER_RAD ) -SAFE_UPPER_RAD = ( - 0.73, 0.0, 0.0, 0.0, - 0.26, 1.33, 1.48, 0.26, 1.33, 1.71, 1.38, 1.38, +FEEDBACK_UPPER_RAD = tuple( + value + FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_UPPER_RAD ) SDK_TO_URDF_JOINT: tuple[str, ...] = ( @@ -84,6 +95,16 @@ SDK_TO_URDF_JOINT: tuple[str, ...] = ( "pinky_mcp_pitch", ) +# Fixed direction contract between vendor channels and the target CAD axes. +# The magnitude remains visually calibrated because O12's 12 active SDK +# coordinates include tendon/solver coordinates that are not one-to-one with +# all 19 physical URDF joints. +SDK_TO_URDF_SIGN: tuple[float, ...] = ( + 1.0, -1.0, -1.0, -1.0, + -1.0, 1.0, 1.0, -1.0, + 1.0, 1.0, 1.0, 1.0, +) + ACTIVE_JOINTS = SDK_TO_URDF_JOINT PASSIVE_JOINTS: tuple[str, ...] = ( "thumb_dip", @@ -110,14 +131,125 @@ MIMIC_SOURCE_BY_JOINT = { "pinky_pip": "pinky_mcp_pitch", "pinky_dip": "pinky_pip", } + +# SDK endpoints measure travel; they have not been surveyed as CAD contact +# datums. In particular CAD.upper - travel is NOT an encoder zero. The root +# thumb axes observe roll/yaw phase; parallel flexion zeros additionally need +# adjacent axis-line observations. The one declared exclusion below remains +# CAD-owned; all other observable active zeros are required for publication. +ROOT_GEOMETRIC_ZERO_JOINTS = frozenset({"thumb_cmc_roll", "thumb_cmc_yaw"}) +# ID2/ID3 are still used to fit and independently validate thumb MCP/DIP +# motion, but the curved-shell ID3 observation is not a reliable absolute +# axis-line datum for the thumb MCP assembly phase. Keep that one static +# origin at immutable source CAD until a rigid downstream fiducial is +# available. This is an explicit O12 profile contract, not an exception-path +# quality bypass; every other observable active zero remains mandatory. +STATIC_ZERO_EXCLUDED_JOINTS = frozenset({"thumb_mcp"}) +GEOMETRIC_ZERO_JOINTS = frozenset( + CALIBRATED_ACTIVE_JOINTS - STATIC_ZERO_EXCLUDED_JOINTS +) +ENDPOINT_ANCHOR_BY_JOINT: dict[str, str] = {} COMMAND_INDEX_BY_JOINT = { joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT) } -PARK_FINGER_RAD = 0.65 * 1.38 -PARK_MIDDLE_MCP_RAD = 0.65 * 1.33 -PARK_MIDDLE_PIP_RAD = 0.65 * 1.71 +# The outer pair is used only as collision clearance after the pinky's own +# full-range scan has already exercised the same safe endpoint. O12 has only +# one active coordinate on ring/pinky, so partial MCP flexion leaves both long +# passive chains inside the middle/index camera corridor. Park them at the +# reviewed CAD/SDK intersection instead of the former 65% pose. +OUTER_CLEARANCE_FRACTION = 1.0 +PARK_RING_MCP_RAD = OUTER_CLEARANCE_FRACTION * SAFE_UPPER_RAD[10] +PARK_PINKY_MCP_RAD = OUTER_CLEARANCE_FRACTION * SAFE_UPPER_RAD[11] +# Compatibility name for downstream diagnostics; motion code uses the two +# channel-specific values above and never takes their minimum. +PARK_FINGER_RAD = PARK_PINKY_MCP_RAD +PARK_MIDDLE_MCP_RAD = SAFE_UPPER_RAD[8] +PARK_MIDDLE_PIP_RAD = SAFE_UPPER_RAD[9] +# Feedback radians are the quantity being calibrated and need not numerically +# equal the command endpoint. Once a finger has been scanned, clearance uses +# that measured feedback travel as its endpoint reference. Simultaneous MCP +# and PIP flexion may shorten the vendor solver coordinate slightly, so 90% +# of the isolated measured stroke plus a settled hold is the accepted physical +# maximum for the coupled clearance pose. +CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION = 0.90 +CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD = 0.06 +CLEARANCE_SPLAY_ENDPOINT_TOLERANCE_RAD = 0.03 INDEX_CLEARANCE_RAD = -math.radians(10.0) +# O12's index ABAD/MCP and middle ABAD/MCP are coupled tendon coordinates. +# The vendor solver clamps MCP upward when a large ABAD command is paired +# with MCP=0. These reviewed values keep the requested pose inside the +# solver's feasible set instead of relying on an invisible SDK correction: +# - index ABAD -10 deg requires MCP >= 0.109846 rad; +# - either ABAD roll endpoint (+/-0.26 rad) requires MCP >= 0.163636 rad. +INDEX_CLEARANCE_MCP_RAD = 0.11 +ROLL_CLEARANCE_MCP_RAD = 0.17 +# O12 feedback is continuous radians. Use 64 possible normalized cells and +# require at least 32 of them; using exactly 32 possible cells accidentally +# required both mechanical endpoints despite the separate 90% span gate. +NORMALIZED_SWEEP_BIN_COUNT = 64 +# A feedback curve is the quantity being identified, so its unknown endpoint +# scale must never be judged against the command endpoint as if they were the +# same coordinate. Cycle 0 is normalized over its own measured endpoints and +# therefore establishes a unit-span physical reference. Later cycles must +# reproduce at least 90% of that measured full-stroke reference. +INITIAL_FEEDBACK_SPAN_FRACTION = 0.99 +EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION = 0.90 +# Both FRONT/TOP and FRONT/SIDE are close to orthogonal on the reviewed O12 +# rig. Their common planar checkerboard must be oblique in at least one view, +# so the G20 1.2 px (and O6 1.5 px) batch gates reject highly repeatable O12 +# solutions for image-geometry reasons. At the measured 3700--3800 px focal +# length, 2.0 px is about 0.030 degrees. Treat RMS as a gross-error gate and +# keep the independent 0.3 degree and 1.5 mm pose-repeatability gates as the +# final geometric consistency evidence. +MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX = 2.0 + + +@dataclass(frozen=True) +class RollCrossViewObserver: + """O12-only side-camera contract for one front-camera roll sweep.""" + + task_name: str + view: str + parent_role: str + child_role: str + source_joint: str + model_joint: str + coupled_channel_index: int + maximum_coupled_motion_rad: float + + +# The downstream PIP Tag is rigidly carried by MCP roll, so it supplies an +# independent side-view roll curve while the front Tag supplies the published +# curve. O12's tendon solver also has a small, deterministic PIP back-drive +# around centred ABAD: joint->actuator->joint reaches about 0.0124 rad even +# when PIP is commanded to zero. The reviewed 0.020 rad envelope includes +# encoder quantisation and applies only to roll-task cross-view integrity. +ROLL_CROSS_VIEW_OBSERVERS: tuple[RollCrossViewObserver, ...] = ( + RollCrossViewObserver( + "middle_roll_front", "side", "side_base", "middle_pip", + "middle_mcp_roll_side", "middle_mcp_roll", 9, 0.020, + ), + RollCrossViewObserver( + "index_roll_front", "side", "side_base", "index_pip", + "index_mcp_roll_side", "index_mcp_roll", 6, 0.020, + ), +) +ROLL_CROSS_VIEW_BY_TASK = { + observer.task_name: observer for observer in ROLL_CROSS_VIEW_OBSERVERS +} + +# Fast-calibration caps by fixed SDK channel. The thumb CMC pitch cap keeps +# explicit margin below its unusually low 0.11 rad/s source-URDF limit; the +# remaining caps are deliberately far below their corresponding CAD limits +# while keeping one visual sweep long enough for dense 30 Hz observations. +FORMAL_SPEED_CAP_RAD_S: tuple[float, ...] = ( + 0.16, 0.16, 0.10, 0.32, + # Index/middle ABAD at 0.16 rad/s repeatedly returned only 89.2--89.5% + # feedback coverage; 0.12 rad/s measured 91.2% on the physical hand. + 0.12, 0.32, 0.32, 0.12, + 0.32, 0.32, 0.32, 0.32, +) def _task( @@ -147,15 +279,24 @@ def _task( def build_typed_profile() -> CalibrationProfile: middle_clearance = ( (4, INDEX_CLEARANCE_RAD), - (10, PARK_FINGER_RAD), - (11, PARK_FINGER_RAD), + (5, INDEX_CLEARANCE_MCP_RAD), + (10, PARK_RING_MCP_RAD), + (11, PARK_PINKY_MCP_RAD), + ) + middle_roll_clearance = ( + *middle_clearance, + (8, ROLL_CLEARANCE_MCP_RAD), ) index_clearance = ( (7, 0.0), (8, PARK_MIDDLE_MCP_RAD), (9, PARK_MIDDLE_PIP_RAD), - (10, PARK_FINGER_RAD), - (11, PARK_FINGER_RAD), + (10, PARK_RING_MCP_RAD), + (11, PARK_PINKY_MCP_RAD), + ) + index_roll_clearance = ( + *index_clearance, + (5, ROLL_CLEARANCE_MCP_RAD), ) tasks = ( _task("thumb_pitch_front", "front", 2, ("thumb_cmc_pitch",), start=0.0, end=SAFE_LOWER_RAD[2], speed=0.03), @@ -163,10 +304,10 @@ def build_typed_profile() -> CalibrationProfile: _task("thumb_mcp_dip_front", "front", 3, ("thumb_mcp", "thumb_dip"), start=0.0, end=SAFE_LOWER_RAD[3], speed=0.08), _task("thumb_yaw_top", "top", 1, ("thumb_cmc_yaw",), start=0.0, end=SAFE_LOWER_RAD[1], speed=0.04), _task("pinky_chain_side", "side", 11, ("pinky_mcp_pitch", "pinky_pip", "pinky_dip"), start=0.0, end=SAFE_UPPER_RAD[11], speed=0.08), - _task("middle_roll_front", "front", 7, ("middle_mcp_roll",), start=SAFE_UPPER_RAD[7], end=SAFE_LOWER_RAD[7], speed=0.04, auxiliary=middle_clearance), + _task("middle_roll_front", "front", 7, ("middle_mcp_roll",), start=SAFE_UPPER_RAD[7], end=SAFE_LOWER_RAD[7], speed=0.04, auxiliary=middle_roll_clearance), _task("middle_mcp_side", "side", 8, ("middle_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[8], speed=0.08, auxiliary=middle_clearance), _task("middle_pip_dip_side", "side", 9, ("middle_pip", "middle_dip"), start=0.0, end=SAFE_UPPER_RAD[9], speed=0.08, auxiliary=middle_clearance), - _task("index_roll_front", "front", 4, ("index_mcp_roll",), start=SAFE_UPPER_RAD[4], end=SAFE_LOWER_RAD[4], speed=0.04, auxiliary=index_clearance), + _task("index_roll_front", "front", 4, ("index_mcp_roll",), start=SAFE_UPPER_RAD[4], end=SAFE_LOWER_RAD[4], speed=0.04, auxiliary=index_roll_clearance), _task("index_mcp_side", "side", 5, ("index_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[5], speed=0.08, auxiliary=index_clearance), _task("index_pip_dip_side", "side", 6, ("index_pip", "index_dip"), start=0.0, end=SAFE_UPPER_RAD[6], speed=0.08, auxiliary=index_clearance), ) @@ -184,7 +325,7 @@ def build_typed_profile() -> CalibrationProfile: "middle_pip": MeasurementSpec("middle_pip", "relative_rotation", "side", "side_base", "middle_pip"), "middle_dip": MeasurementSpec("middle_dip", "relative_rotation", "side", "middle_pip", "middle_dip", pose_axis_line_required=False), "index_mcp_roll": MeasurementSpec("index_mcp_roll", "relative_rotation", "front", "front_base", "index_roll"), - "index_mcp_pitch": MeasurementSpec("index_mcp_pitch", "relative_rotation", "side", "side_base", "index_pip"), + "index_mcp_pitch": MeasurementSpec("index_mcp_pitch", "relative_rotation", "side", "side_base", "index_dip"), "index_pip": MeasurementSpec("index_pip", "relative_rotation", "side", "side_base", "index_pip"), "index_dip": MeasurementSpec("index_dip", "relative_rotation", "side", "index_pip", "index_dip", pose_axis_line_required=False), } @@ -196,6 +337,8 @@ def build_typed_profile() -> CalibrationProfile: "transferred_static_dynamic" if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT else "measured_static_dynamic" + if name in GEOMETRIC_ZERO_JOINTS + else "measured_dynamic_cad_static" ) for name in active }, @@ -218,6 +361,8 @@ def build_typed_profile() -> CalibrationProfile: # The public command domain is the reviewed SDK/CAD intersection. lower_bounds=SAFE_LOWER_RAD, upper_bounds=SAFE_UPPER_RAD, + feedback_lower_bounds=FEEDBACK_LOWER_RAD, + feedback_upper_bounds=FEEDBACK_UPPER_RAD, unit="rad", feedback_by_index=True, command_index_by_joint=COMMAND_INDEX_BY_JOINT, @@ -243,7 +388,7 @@ def build_typed_profile() -> CalibrationProfile: common_frame="calibration_common", extrinsic_reference_view="front", extrinsics_quality_limits={ - "reprojection_rms_px": 1.2, + "reprojection_rms_px": MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, "maximum_rotation_repeatability_deg": 0.3, "maximum_translation_repeatability_m": 0.0015, }, @@ -251,10 +396,12 @@ def build_typed_profile() -> CalibrationProfile: ), motion=MotionPolicy( tasks=tasks, - precheck_sweeps=True, - steady_command_checkpoints=True, + # O12 uses AcquisitionPolicy.mapping_probe_maximum_rad for one + # channel-mapping jog; it does not inherit legacy sweep prechecks. + precheck_sweeps=False, + steady_command_checkpoints=False, speed_parameters={ - "command_rate_hz": 20.0, + "command_rate_hz": 50.0, "clearance_flex_rad_s": 0.10, "clearance_splay_rad_s": 0.04, "probe_travel_rad": math.radians(3.0), @@ -267,22 +414,31 @@ def build_typed_profile() -> CalibrationProfile: zero=ZeroSolvePolicy( active_joints=active, passive_joints=passive, - direct_zero_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS)), - axis_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS)), - mechanical_endpoint_joints=frozenset(CALIBRATED_ACTIVE_JOINTS), + direct_zero_joints=tuple(sorted(GEOMETRIC_ZERO_JOINTS)), + axis_joints=tuple(sorted( + CALIBRATED_ACTIVE_JOINTS + | (MEASURED_PASSIVE_JOINTS - {"thumb_dip"}) + )), + mechanical_endpoint_joints=frozenset(ENDPOINT_ANCHOR_BY_JOINT), post_solve_endpoint_joints=frozenset(), mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT, - cad_frozen_joints=passive, - endpoint_anchor_by_joint={name: "lower_at_start" for name in CALIBRATED_ACTIVE_JOINTS}, - fitted_mimic_joints=MEASURED_PASSIVE_JOINTS, - coupling_model_by_joint={name: "quadratic_runtime" for name in MEASURED_PASSIVE_JOINTS}, + cad_frozen_joints=passive | (active - GEOMETRIC_ZERO_JOINTS - set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT)), + endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT, + # AprilTags validate passive motion, but the deployed passive + # mapping is owned by the O12 SDK solver. A planar-PnP branch must + # never replace that product kinematic contract in the URDF. + fitted_mimic_joints=frozenset(), + coupling_model_by_joint={ + name: "vendor_o12_polynomial" + for name in MEASURED_PASSIVE_JOINTS + }, ), quality=QualityPolicy( training_cycles=(0, 1, 2), holdout_cycle=3, hard_threshold_keys=frozenset({ "minimum_detection_rate", "maximum_state_image_skew_ms", - "maximum_hysteresis_rad", "maximum_validation_error_rad", + "maximum_validation_error_rad", "maximum_mimic_residual_rad", }), isolated_holdout=True, @@ -301,9 +457,16 @@ def build_typed_profile() -> CalibrationProfile: "calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256", }), publication_pointer="latest_passed", - session_compatibility_tokens=frozenset({"o12_right_16_v1", "feedback_rad_v1"}), + session_compatibility_tokens=frozenset({ + "o12_right_16_v1", "feedback_rad_v1", "full_sdk_range_v2" + }), publish_corrected_urdf=True, ), + acquisition=AcquisitionPolicy( + mapping_probe_maximum_rad=math.radians(3.0), + physical_first_cycle_minimum_span_01=INITIAL_FEEDBACK_SPAN_FRACTION, + physical_repeat_minimum_fraction=EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION, + ), joint_coverage=coverage, ) @@ -336,9 +499,19 @@ def build_profile() -> RegisteredProfile: __all__ = [ "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT", - "COMMAND_NAMES", "INDEX_CLEARANCE_RAD", "KEY", "MEASURED_PASSIVE_JOINTS", - "MIMIC_SOURCE_BY_JOINT", "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD", + "CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD", + "CLEARANCE_SPLAY_ENDPOINT_TOLERANCE_RAD", "COMMAND_NAMES", + "GEOMETRIC_ZERO_JOINTS", "INDEX_CLEARANCE_RAD", "KEY", + "MEASURED_PASSIVE_JOINTS", + "EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION", + "INITIAL_FEEDBACK_SPAN_FRACTION", "MIMIC_SOURCE_BY_JOINT", + "NORMALIZED_SWEEP_BIN_COUNT", "OUTER_CLEARANCE_FRACTION", + "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD", "PARK_PINKY_MCP_RAD", + "PARK_RING_MCP_RAD", "CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION", "PARK_MIDDLE_PIP_RAD", "PASSIVE_JOINTS", "SAFE_LOWER_RAD", "SAFE_UPPER_RAD", + "ROLL_CROSS_VIEW_BY_TASK", "ROLL_CROSS_VIEW_OBSERVERS", + "RollCrossViewObserver", "SDK_LOWER_RAD", "SDK_TO_URDF_JOINT", "SDK_UPPER_RAD", + "STATIC_ZERO_EXCLUDED_JOINTS", "TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py new file mode 100644 index 0000000..421099f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py @@ -0,0 +1,115 @@ +"""O12-only sweep observability policy. + +The O12 records continuous radian feedback into a fixed normalized grid. A +raw per-frame Tag rate is useful for camera diagnostics, but it is not by +itself evidence that a fitted curve is unobservable: a long sweep may retain +hundreds of synchronized samples and dense travel coverage after a short Tag +occlusion. This module makes the actual fitting information the acceptance +contract and keeps the stricter camera targets as warnings. +""" + +from __future__ import annotations + +import math +from typing import Any, Mapping, Sequence + +import numpy as np + + +QUALITY_POLICY_VERSION = 5 +# G20 permits roughly one sixteenth of its 256-bin domain to be unobserved +# contiguously. Express that invariant as a fraction so O12's 64-bin grid is +# judged at the same physical scale instead of copying a raw bin count. +MAXIMUM_UNOBSERVED_BIN_FRACTION = 1.0 / 16.0 + + +def evaluate_o12_observation_quality( + normalized_feedback: Sequence[float], + observation: Mapping[str, Any], + *, + normalized_bin_count: int, + required_feedback_span: float, + minimum_sweep_frames: int, + minimum_sweep_bins: int, + minimum_joint_frame_rate: float, + minimum_feedback_hz: float, + feedback_hz: float, + target_detection_rate: float, + target_maximum_bin_gap: int, +) -> dict[str, Any]: + """Return one authoritative O12 acceptance decision and diagnostics.""" + count = int(normalized_bin_count) + if count < 32: + raise ValueError("normalized sweep bin count must be at least 32") + values = np.asarray(normalized_feedback, dtype=float) + values = values[np.isfinite(values)] + clipped = np.clip(values, 0.0, 1.0) + bins = sorted(set( + min(count - 1, max(0, int(value * count))) + for value in clipped + )) + span = float(np.ptp(clipped)) if clipped.size else 0.0 + maximum_gap = max( + (right - left - 1 for left, right in zip(bins, bins[1:])), + default=0 if bins else count, + ) + maximum_unobserved_bins = max( + 1, int(math.floor(count * MAXIMUM_UNOBSERVED_BIN_FRACTION)) + ) + allowed_maximum_gap = maximum_unobserved_bins + detection_rate = float(observation.get("tag_detection_rate", 0.0)) + joint_frame_rate = float(observation.get("joint_frame_rate", 0.0)) + + failures: list[str] = [] + warnings: list[str] = [] + if clipped.size < int(minimum_sweep_frames): + failures.append(f"frames={clipped.size}") + if clipped.size == 0 or span < float(required_feedback_span): + failures.append("feedback_span") + if len(bins) < int(minimum_sweep_bins): + failures.append(f"bins={len(bins)}") + if maximum_gap > allowed_maximum_gap: + failures.append(f"maximum_gap={maximum_gap}") + if joint_frame_rate < float(minimum_joint_frame_rate): + warnings.append(f"joint_frame_rate={joint_frame_rate:.3f}") + if float(feedback_hz) < float(minimum_feedback_hz): + warnings.append(f"feedback_hz={float(feedback_hz):.2f}") + + # These remain explicit operator diagnostics. They do not duplicate the + # observability gates above or force a complete rescan of otherwise dense + # data after a short, localized occlusion. + if detection_rate < float(target_detection_rate): + role_rates = observation.get("tag_detection_rate_by_role", {}) + worst_role = min(role_rates, key=role_rates.get, default="unknown") + warnings.append( + f"tag_rate_target[{worst_role}]={detection_rate:.3f}" + ) + if maximum_gap > int(target_maximum_bin_gap): + warnings.append(f"maximum_gap_target={maximum_gap}") + + return { + "quality_policy_version": QUALITY_POLICY_VERSION, + "decision_basis": "normalized_fit_observability", + "valid_frames": int(clipped.size), + "feedback_bins": len(bins), + "feedback_span": round(span, 9), + "required_feedback_span": round(float(required_feedback_span), 9), + "maximum_bin_gap": int(maximum_gap), + "allowed_maximum_bin_gap": int(allowed_maximum_gap), + "gap_scope": "observed_feedback_span", + "maximum_unobserved_bin_fraction": ( + MAXIMUM_UNOBSERVED_BIN_FRACTION + ), + "target_detection_rate": float(target_detection_rate), + "target_maximum_bin_gap": int(target_maximum_bin_gap), + "warnings": warnings, + "failures": failures, + "passed": not failures, + } + + +__all__ = [ + "MAXIMUM_UNOBSERVED_BIN_FRACTION", + "QUALITY_POLICY_VERSION", + "evaluate_o12_observation_quality", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/resume.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/resume.py new file mode 100644 index 0000000..30c9a5e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/resume.py @@ -0,0 +1,384 @@ +"""Durable, safety-checked O12 scan checkpoint recovery.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +from pathlib import Path +from typing import Any, Mapping, Sequence + +from ...product import ProductConfig +from ...runtime import ACQUISITION_POLICY_VERSION +from .pipeline import load_o12_raw_samples +from .profile import ROLL_CROSS_VIEW_BY_TASK + + +PROTECTED_INPUT_KEYS = ( + "source_urdf_sha256", + "camera_extrinsics_sha256", + "calibration_config_sha256", + "tag_config_sha256", + "sdk_config_sha256", +) + +ResumeUnit = tuple[str, int, str] + + +@dataclass(frozen=True) +class O12ResumeCheckpoint: + source_session: Path + compatibility: str + completed_units: tuple[ResumeUnit, ...] + completed_tasks: tuple[str, ...] + imported_records: tuple[dict[str, Any], ...] + + +def ordered_resume_units(profile) -> tuple[ResumeUnit, ...]: + return tuple( + (task.key, cycle, direction) + for task in profile.motion.tasks + for cycle in (0, 1, 2, 3) + for direction in ("decreasing", "increasing") + ) + + +def _quality_evidence_accepted( + row: Mapping[str, Any], valid: int, total: int +) -> bool: + """Use the decision recorded by the matching O12 quality-policy version.""" + if int(row.get("quality_policy_version", 0)) >= 2: + return bool( + row.get("passed") + and not list(row.get("failures", ())) + and valid >= 40 + and total > 0 + ) + # Preserve the exact contract used by sessions written before the O12 + # observability policy existed. This branch is compatibility only; it + # does not reinterpret a previously rejected sweep as passing. + return bool( + not list(row.get("failures", ())) + and valid >= 40 + and total > 0 + # Older sessions can contain one final synchronized callback after + # the total counter reset (for example 259/258); cap the ratio at one. + and valid / max(total, valid) >= 0.85 + and float(row.get("tag_detection_rate", 0.0)) >= 0.95 + and int(row.get("feedback_bins", 0)) >= 32 + and int(row.get("maximum_bin_gap", 999)) <= 2 + ) + + +def _session_start(rows: Sequence[Mapping[str, Any]]) -> Mapping[str, Any]: + starts = [row for row in rows if row.get("kind") == "session_start"] + if len(starts) != 1: + raise ValueError("resume raw must contain exactly one session_start") + return starts[0] + + +def _protected_values(config: ProductConfig) -> dict[str, str]: + return { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "tag_config_sha256": config.tag_config_sha256, + "sdk_config_sha256": config.sdk_config_sha256, + } + + +def _legacy_checkpoint_is_attested( + config: ProductConfig, + session: Path, + rows: Sequence[Mapping[str, Any]], +) -> bool: + """Accept pre-checkpoint O12 data only when its local inputs are unchanged.""" + raw_path = session / "raw_samples.jsonl" + try: + raw_time = raw_path.stat().st_mtime + protected_paths = ( + config.source_urdf, + config.camera_extrinsics, + config.calibration_config, + config.tag_config, + config.sdk_config, + ) + if any( + path is None or path.stat().st_mtime > raw_time + for path in protected_paths + ): + return False + log = (session / "calibration.log").read_text( + encoding="utf-8", errors="replace" + ) + except OSError: + return False + if str(config.source_urdf) not in log: + return False + return any( + row.get("kind") == "o12_temperature_capability_fallback" + and row.get("sdk_config_sha256") == config.sdk_config_sha256 + for row in rows + ) + + +def validate_resume_source( + config: ProductConfig, + session: str | Path, +) -> tuple[list[dict[str, Any]], str]: + candidate = Path(session).expanduser().resolve(strict=True) + root = config.session_root.expanduser().resolve(strict=True) + if candidate.parent != root or not candidate.is_dir(): + raise ValueError("resume session must be a direct child of the serial root") + summary_path = candidate / "calibration_summary_zh.json" + if summary_path.is_file(): + try: + summary = json.loads(summary_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as error: + raise ValueError("resume session has an invalid summary") from error + if isinstance(summary, Mapping) and summary.get("result") == "PASS": + raise ValueError("a passed O12 session cannot be used as a checkpoint") + rows = load_o12_raw_samples(candidate / "raw_samples.jsonl") + start = _session_start(rows) + if ( + start.get("profile_id") != config.profile_key.profile_id + or start.get("serial_number") != config.serial_number + or int(start.get("sample_schema_version", -1)) + != config.calibration_contract.typed_profile.artifacts.output_schema_version + ): + raise ValueError("resume profile, serial number, or schema differs") + if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION: + raise ValueError( + "resume acquisition policy differs; unified_engine_v1 requires " + "a new full capture" + ) + expected = _protected_values(config) + recorded = {key: str(start.get(key, "")) for key in PROTECTED_INPUT_KEYS} + if all(recorded.values()): + if recorded != expected: + raise ValueError("resume protected input hashes differ") + compatibility = "protected_hashes_v1" + else: + raise ValueError("resume checkpoint has no protected input hashes") + return rows, compatibility + + +def build_resume_checkpoint( + config: ProductConfig, + session: str | Path, +) -> O12ResumeCheckpoint: + rows, compatibility = validate_resume_source(config, session) + return build_resume_checkpoint_from_rows( + config.calibration_contract.typed_profile, + session, + rows, + compatibility=compatibility, + ) + + +def build_resume_checkpoint_from_rows( + profile, + session: str | Path, + rows: Sequence[Mapping[str, Any]], + *, + compatibility: str, +) -> O12ResumeCheckpoint: + task_by_key = {task.key: task for task in profile.motion.tasks} + cross_view_passing: set[tuple[ResumeUnit, int]] = set() + for row in rows: + if row.get("kind") != "o12_roll_cross_view_quality": + continue + try: + unit = ( + str(row["task_name"]), + int(row["cycle"]), + str(row["direction"]), + ) + attempt = int(row.get("attempt", 1)) + valid = int(row.get("valid_frames", 0)) + total = int(row.get("total_frames", 0)) + accepted = ( + unit[0] in ROLL_CROSS_VIEW_BY_TASK + and unit[1] in (0, 1, 2, 3) + and unit[2] in {"decreasing", "increasing"} + and _quality_evidence_accepted(row, valid, total) + ) + except (KeyError, TypeError, ValueError, ZeroDivisionError): + continue + if accepted: + cross_view_passing.add((unit, attempt)) + + passing: dict[ResumeUnit, tuple[int, Mapping[str, Any]]] = {} + for row in rows: + if row.get("kind") != "o12_sweep_observation_quality": + continue + try: + unit = ( + str(row["task_name"]), + int(row["cycle"]), + str(row["direction"]), + ) + attempt = int(row.get("attempt", 1)) + valid = int(row.get("valid_frames", 0)) + total = int(row.get("total_frames", 0)) + accepted = ( + unit[0] in task_by_key + and unit[1] in (0, 1, 2, 3) + and unit[2] in {"decreasing", "increasing"} + and _quality_evidence_accepted(row, valid, total) + and ( + unit[0] not in ROLL_CROSS_VIEW_BY_TASK + or (unit, attempt) in cross_view_passing + ) + ) + except (KeyError, TypeError, ValueError, ZeroDivisionError): + continue + if accepted and attempt >= passing.get(unit, (0, {}))[0]: + passing[unit] = (attempt, row) + + ordered = ordered_resume_units(profile) + completed: list[ResumeUnit] = [] + for unit in ordered: + if unit not in passing: + break + completed.append(unit) + if not completed: + raise ValueError("resume session has no contiguous passed scan unit") + # A session created while resuming declares how many source units it + # intended to import. If startup was interrupted during persistence, its + # JSONL can contain that audit header but only a prefix of the associated + # samples. Do not let such a newer, truncated session shadow the older + # complete checkpoint during automatic selection. + declared_import_counts = [ + int(row.get("completed_unit_count", -1)) + for row in rows + if row.get("kind") == "o12_resume_checkpoint_import" + ] + if any(count < 0 or count > len(completed) for count in declared_import_counts): + raise ValueError("resume checkpoint import is incomplete or truncated") + + selected_attempt = { + unit: passing[unit][0] for unit in completed + } + imported: list[dict[str, Any]] = [] + for row in rows: + kind = row.get("kind") + if kind not in { + "o12_joint_sample", + "o12_pnp_candidate_frame", + "o12_sweep_observation_quality", + "o12_roll_cross_view_sample", + "o12_roll_cross_view_quality", + }: + continue + try: + unit = ( + str(row["task_name"]), + int(row["cycle"]), + str(row["direction"]), + ) + attempt = int(row.get("attempt", 1)) + except (KeyError, TypeError, ValueError): + continue + if unit not in selected_attempt or attempt != selected_attempt[unit]: + continue + copied = dict(row) + copied["resume_imported"] = True + copied["resume_source_session"] = Path(session).resolve().name + imported.append(copied) + + completed_set = set(completed) + completed_tasks = tuple( + task.key + for task in profile.motion.tasks + if all( + (task.key, cycle, direction) in completed_set + for cycle in (0, 1, 2, 3) + for direction in ("decreasing", "increasing") + ) + ) + for row in rows: + if ( + row.get("kind") == "o12_fixed_mapping_preflight" + and str(row.get("task_name", "")) in completed_tasks + ): + copied = dict(row) + copied["resume_imported"] = True + copied["resume_source_session"] = Path(session).resolve().name + imported.append(copied) + + primary_by_unit = { + unit: 0 for unit in completed + } + for row in imported: + if row.get("kind") != "o12_joint_sample": + continue + unit = ( + str(row["task_name"]), int(row["cycle"]), str(row["direction"]) + ) + task = task_by_key[unit[0]] + if row.get("joint") == task.joints[0]: + primary_by_unit[unit] += 1 + if any(count < 40 for count in primary_by_unit.values()): + raise ValueError("resume quality record lacks its primary joint samples") + + cross_samples_by_unit = { + unit: 0 for unit in completed if unit[0] in ROLL_CROSS_VIEW_BY_TASK + } + for row in imported: + if row.get("kind") != "o12_roll_cross_view_sample": + continue + unit = ( + str(row["task_name"]), int(row["cycle"]), str(row["direction"]) + ) + if unit in cross_samples_by_unit: + cross_samples_by_unit[unit] += 1 + if any(count < 40 for count in cross_samples_by_unit.values()): + raise ValueError("resume roll quality lacks its side-view samples") + + return O12ResumeCheckpoint( + source_session=Path(session).expanduser().resolve(), + compatibility=compatibility, + completed_units=tuple(completed), + completed_tasks=completed_tasks, + imported_records=tuple(imported), + ) + + +def automatic_resume_candidate(config: ProductConfig) -> O12ResumeCheckpoint | None: + root = config.session_root + try: + candidates = sorted( + ( + path for path in root.resolve(strict=True).iterdir() + if path.is_dir() and not path.name.startswith("latest_") + ), + key=lambda path: path.name, + reverse=True, + ) + except OSError: + return None + passed: Path | None = None + pointer = root / "latest_passed" + if pointer.exists(): + try: + passed = pointer.resolve(strict=True) + except OSError: + pass + for candidate in candidates: + if passed is not None and candidate.name <= passed.name: + continue + try: + return build_resume_checkpoint(config, candidate) + except (OSError, ValueError): + continue + return None + + +__all__ = [ + "O12ResumeCheckpoint", + "automatic_resume_candidate", + "build_resume_checkpoint", + "build_resume_checkpoint_from_rows", + "ordered_resume_units", + "validate_resume_source", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py index deed742..b038106 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py @@ -7,6 +7,7 @@ from datetime import datetime import json import os from pathlib import Path +import re import subprocess import time from typing import Any @@ -22,10 +23,10 @@ from ..l6.runner import ( _l6_reason_zh, _launch_command, _stop_stack, - _wait_until, render_six_channel_progress_zh, ) from .pipeline import finalize_o12_session, load_o12_raw_samples +from .resume import automatic_resume_candidate, ordered_resume_units _TASK_LABELS = { @@ -34,17 +35,30 @@ _TASK_LABELS = { "thumb_mcp_dip_front": "拇指 MCP / 被动 DIP(正面 ID1→ID2→ID3)", "thumb_yaw_top": "拇指 CMC yaw(顶部 ID14→ID15)", "pinky_chain_side": "小指 MCP / 被动 PIP/DIP(侧面 ID4→ID5→ID6→ID7)", - "middle_roll_front": "中指 MCP roll(正面 ID0→ID12)", + "middle_roll_front": "中指 MCP roll(正面 ID0→ID12 + 侧面 ID4→ID8)", "middle_mcp_side": "中指 MCP pitch(侧面 ID4→ID8)", "middle_pip_dip_side": "中指 PIP / 被动 DIP(侧面 ID8→ID9)", - "index_roll_front": "食指 MCP roll(正面 ID0→ID13)", - "index_mcp_side": "食指 MCP pitch(侧面 ID4→ID10)", + "index_roll_front": "食指 MCP roll(正面 ID0→ID13 + 侧面 ID4→ID10)", + "index_mcp_side": "食指 MCP pitch(侧面 ID4→ID11)", "index_pip_dip_side": "食指 PIP / 被动 DIP(侧面 ID10→ID11)", } def _o12_reason_zh(status): reason = str(status.get("reason", "")) + if reason.startswith("sweep_quality_failed:"): + details = reason.split(":", 2)[-1] + if any( + item in details + for item in ("side_frames=", "side_bins=", "side_maximum_gap=") + ): + return ( + "O12-CROSS-VIEW-SAMPLING-104", + "侧摆任务的主视角数据已采到,但辅助侧面视角在本段轨迹内" + f"保留的时序覆盖不足:{details}。这不表示 Tag 被遮挡。", + "程序会保留已通过断点;重新启动后只重做该扫描单元。" + "若 Tag 持续可见,优先检查侧面 AprilTag 节点的发布频率和 CPU 调度。", + ) if reason.startswith("mapping_preflight_no_target_motion:"): task = reason.split(":", 1)[1] return ( @@ -52,11 +66,42 @@ def _o12_reason_zh(status): f"固定 SDK 映射点动时没有观察到目标关节运动:{task}。", "检查对应 Tag、通道映射和机械连接;不要交换通道后强行继续。", ) - if reason.startswith("o12_error_report_nonzero"): + if reason.startswith("mapping_preflight_wrong_feedback_direction:"): + task = reason.split(":", 1)[1] + return ( + "O12-MAPPING-302", + f"固定 SDK 映射点动的目标反馈方向不正确:{task}。", + "检查对应 SDK 通道和机械连接;不要交换通道后强行继续。", + ) + if reason.startswith("o12_active_motor_fault"): return ( "O12-HEALTH-201", - f"O12 返回非零错误码:{status.get('latest_error_codes', [])}。", - "检查堵转、过流、过热或通信异常,排除后重新启动。", + f"O12 返回活动电机故障:{status.get('error_faults', [])}。", + "检查对应电机的堵转、过流、过热或电机异常;排除后从断点启动。", + ) + if reason.startswith("o12_new_communication_fault"): + return ( + "O12-HEALTH-202", + f"运行中出现了新的 O12 通信异常:{status.get('error_faults', [])}。", + "检查 HCAN 线缆、供电和对应电机通信;程序已保持当前位置。", + ) + if reason.startswith("o12_feedback_stream_timeout"): + return ( + "O12-HEALTH-203", + "O12 命令触发反馈流中断超过 1 秒。", + "检查 HCAN 连接、SDK 节点和供电;恢复后从断点启动。", + ) + if reason.startswith("feedback_outside_registered_feedback_domain:"): + return ( + "O12-FEEDBACK-202", + "O12 反馈超出了已登记的物理反馈范围。", + "保持当前位置,检查诊断中的通道、反馈值和机械端点。", + ) + if reason == "calibration_node_status_timeout": + return ( + "O12-NODE-500", + "O12 标定节点已停止发布状态,运行栈将自动退出。", + "查看本次 calibration.log 中的 Python traceback,并从断点重启。", ) return _l6_reason_zh(status, model_name="O12") @@ -76,12 +121,107 @@ def render_o12_progress_zh(status, estimator=None) -> str: if status.get("temperature_fallback_active") else "等待温度能力确认" ) - return text + ( + resume = status.get("resume", {}) + resume_text = "" + if isinstance(resume, dict) and resume.get("used"): + resume_text = ( + "\n断点恢复:已复用 " + f"{int(resume.get('completed_unit_count', 0))} 个扫描单元;" + f"来源 {resume.get('source_session', '-')}" + ) + cross_view = status.get("roll_cross_view", {}) + cross_view_text = "" + if isinstance(cross_view, dict) and cross_view.get("active"): + recognized = "/".join( + f"ID{int(value)}" + for value in cross_view.get("recognized_tag_ids", ()) + ) or "无" + missing = "/".join( + f"ID{int(value)}" + for value in cross_view.get("unrecognized_tag_ids", ()) + ) or "无" + cross_view_text = ( + "\n侧摆双机位:侧面已识别 " + f"{recognized};未识别/不合格 {missing};联合 " + f"{int(cross_view.get('valid_frames', 0))}/" + f"{int(cross_view.get('total_frames', 0))} 帧(" + f"{float(cross_view.get('joint_frame_rate', 0.0)):.1%})" + ) + coupling = status.get("feedback_coupling", {}) + coupling_text = "" + if isinstance(coupling, dict) and coupling.get("active"): + stage = { + "training_observation": "训练采集", + "holdout_pending_full_sweep_validation": "第四轮整段验证", + "vendor_solver_motion_prior": "运动准备", + }.get(str(coupling.get("reference_kind", "")), "耦合观测") + coupling_text = ( + "\nO12耦合:" + f"{coupling.get('coupled_channel', '-')} 位移 " + f"{float(coupling.get('displacement_rad', 0.0)):.3f}/" + f"{float(coupling.get('hard_displacement_limit_rad', 0.0)):.3f} rad;" + f"{stage};vendor偏差 " + f"{float(coupling.get('residual_rad', 0.0)):.3f} rad(仅诊断)" + ) + auxiliary = status.get("auxiliary_tracking", {}) + auxiliary_text = "" + if isinstance(auxiliary, dict) and auxiliary.get("active"): + channels = list(auxiliary.get("channels", ())) + worst = max( + channels, + key=lambda item: float(item.get("error_rad", 0.0)), + default={}, + ) + stage = ( + "平滑过渡中" + if auxiliary.get("mode") == "transitioning" + else "已稳定保持" + if auxiliary.get("ready") + else "等待稳定" + ) + auxiliary_text = ( + "\nO12避让轴:" + f"{stage};最大偏差 {worst.get('channel', '-')}=" + f"{float(worst.get('error_rad', 0.0)):.3f} rad" + ) + locked_ids = list(status.get("locked_reference_tag_ids", ())) + locked_reference_text = "" + if locked_ids: + locked_reference_text = ( + "\n固定基准:" + + "/".join(f"ID{int(value)}" for value in locked_ids) + + " 已锁定,避让遮挡期间复用" + ) + error_health = str( + status.get("error_health_classification", "awaiting_error_report") + ) + if error_health == "historical_communication_latch": + channels = "/".join( + str(value) for value in status.get( + "confirmed_historical_communication_channels", () + ) + ) or "未知通道" + error_health_text = f"历史通信位已核验({channels})" + elif error_health == "confirming_historical_communication": + error_health_text = ( + "确认历史通信位 " + f"{int(status.get('error_report_matching_count', 0))}/3" + ) + elif error_health == "clear": + error_health_text = "正常" + else: + error_health_text = error_health + return ( + text + cross_view_text + coupling_text + auxiliary_text + + locked_reference_text + resume_text + ( "\nO12安全:POSITION=" f"{bool(status.get('position_mode_verified'))};" - f"错误码通道={bool(status.get('error_report_verified'))};" + f"错误码通道={bool(status.get('error_report_verified'))}" + f"({error_health_text});" f"温度策略={temperature};速度倍率=" - f"{float(status.get('motion_speed_scale', 1.0)):.1f}x" + f"{float(status.get('motion_speed_scale', 1.0)):.1f}x;" + f"控制发布={float(status.get('command_publish_hz', 0.0)):.1f} Hz" + ) ) @@ -89,6 +229,7 @@ class _Monitor(Node): def __init__(self, progress: _ProgressConsole) -> None: super().__init__("o12_calibration_runner") self.status: dict[str, Any] = {} + self.last_status_at = 0.0 self.progress = progress self.create_subscription(String, "/o12_calibration/status", self._status, 10) self.start_client = self.create_client(Trigger, "/o12_calibration/start") @@ -101,9 +242,76 @@ class _Monitor(Node): return if isinstance(value, dict): self.status = value + self.last_status_at = time.monotonic() self.progress.update(value) +def _wait_until_o12( + monitor: _Monitor, + process: subprocess.Popen[Any], + predicate, + *, + timeout: float | None, + status_stale_after: float = 3.0, +) -> bool: + """Wait for O12 while also detecting a dead calibration child node.""" + started = time.monotonic() + while rclpy.ok(): + if process.poll() is not None: + return False + rclpy.spin_once(monitor, timeout_sec=0.2) + if predicate(monitor.status): + return True + now = time.monotonic() + stale_limit = ( + 180.0 + if monitor.status.get("state") == "FINALIZING" + else float(status_stale_after) + ) + if ( + monitor.last_status_at > 0.0 + and now - monitor.last_status_at > stale_limit + ): + status_publishers = monitor.count_publishers( + "/o12_calibration/status" + ) + monitor.status = { + **monitor.status, + "state": "ABORTED", + "reason": ( + "calibration_node_process_exited" + if status_publishers == 0 + else "calibration_node_status_timeout" + ), + } + return False + if timeout is not None and now - started > timeout: + return False + return False + + +def _log_exception_summary(log_path: Path) -> str: + """Return the final child exception instead of hiding it as status loss.""" + try: + lines = log_path.read_text( + encoding="utf-8", errors="replace" + ).splitlines() + except OSError: + return "" + prefixes = ( + "AttributeError:", "AssertionError:", "ImportError:", + "IndexError:", "KeyError:", "ModuleNotFoundError:", + "OSError:", "RuntimeError:", "TypeError:", "ValueError:", + ) + ansi = re.compile(r"\x1b\[[0-9;]*m") + for raw in reversed(lines[-300:]): + line = ansi.sub("", raw).strip() + payload = line.rsplit("] ", 1)[-1].strip() + if payload.startswith(prefixes): + return payload + return "" + + def _overlay_environment(setup: Path) -> dict[str, str]: completed = subprocess.run( ["bash", "-c", 'source "$1" >/dev/null 2>&1; env -0', "bash", str(setup)], @@ -118,39 +326,140 @@ def _overlay_environment(setup: Path) -> dict[str, str]: return environment +def _protected_inputs(config: ProductConfig) -> dict[str, str]: + return { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "tag_config_sha256": config.tag_config_sha256, + "sdk_config_sha256": config.sdk_config_sha256, + } + + +def _finalize_completed_resume(config, resume, session: Path) -> int: + """Publish a complete checkpoint without touching calibration hardware.""" + expected = ordered_resume_units( + config.calibration_contract.typed_profile + ) + if tuple(resume.completed_units) != tuple(expected): + raise ValueError("completed-resume finalization requires every scan unit") + print( + "O12 完整断点已包含全部 " + f"{len(expected)} 个扫描单元;不启动 SDK、相机或避让运动," + "直接拟合、验证并生成 URDF(约需 1 分钟)。", + flush=True, + ) + try: + payload, _fit, correction = finalize_o12_session( + session_dir=session, + serial_number=config.serial_number, + source_urdf=config.source_urdf, + protected_inputs=_protected_inputs(config), + # The checkpoint builder imports only matching, passed scan + # transactions plus their PnP provenance. This also avoids + # recursively replaying prior session/audit wrapper records. + records=list(resume.imported_records), + publish=True, + ) + except BaseException as error: + print( + "O12 完整断点的拟合、验证或 URDF 写回失败:" + f"{error}", + flush=True, + ) + return 3 + print("\n".join(( + "PASS:O12 完整断点已通过拟合和独立 holdout;" + "thumb_mcp 静态零位保留原始 CAD。", + f"发布结果:{config.session_root / 'latest_passed'}", + f"JSON:{session / config.calibration_contract.typed_profile.artifacts.calibration_filename.format(serial_number=config.serial_number)}", + f"URDF:{correction.path}", + )), flush=True) + return 0 + + def _run_online( - config: ProductConfig, *, record_bag: bool, commands_enabled: bool + config: ProductConfig, *, record_bag: bool, commands_enabled: bool, + allow_resume: bool, ) -> int: if config.sdk_setup is None or config.sdk_config is None: raise ValueError("O12 vendor SDK overlay/config are required") + resume = automatic_resume_candidate(config) if allow_resume else None session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S") while session.exists(): time.sleep(1.0) session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S") session.mkdir(parents=True) + expected_resume_units = ordered_resume_units( + config.calibration_contract.typed_profile + ) + if ( + resume is not None + and tuple(resume.completed_units) == tuple(expected_resume_units) + ): + return _finalize_completed_resume(config, resume, session) log_path = session / "calibration.log" log_stream = log_path.open("a", encoding="utf-8", buffering=1) - print(f"O12 标定环境正在启动;日志:{log_path}", flush=True) - process = subprocess.Popen( - _launch_command( - config, session, record_bag=record_bag, - commands_enabled=commands_enabled, - ), - cwd=config.workspace, - env=_overlay_environment(config.sdk_setup), - stdout=log_stream, - stderr=subprocess.STDOUT, - text=True, - start_new_session=True, - ) rclpy.init() monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh)) + process: subprocess.Popen[str] | None = None try: - ready = _wait_until( + # Discover already-running SDK/GUI/calibration publishers before this + # command starts its own vendor node. Otherwise a vendor open failure + # could be masked by feedback from the stale process holding HCAN. + discovery_deadline = time.monotonic() + 1.5 + while rclpy.ok() and time.monotonic() < discovery_deadline: + rclpy.spin_once(monitor, timeout_sec=0.1) + existing_state = monitor.count_publishers("/o12/right/joint_states") + existing_command = monitor.count_publishers("/o12/right/joint_cmd") + if existing_state or existing_command: + print( + "O12 启动前独占检查失败:检测到既有 SDK/GUI/标定进程" + f"(状态发布者={existing_state},命令发布者={existing_command})。" + "请先关闭它们,再只运行本标定命令。", + flush=True, + ) + return 2 + print(f"O12 标定环境正在启动;日志:{log_path}", flush=True) + if resume is not None: + print( + "O12 断点恢复:来源 " + f"{resume.source_session},复用 " + f"{len(resume.completed_units)} 个已通过扫描单元," + "启动后先恢复安全姿态。", + flush=True, + ) + process = subprocess.Popen( + _launch_command( + config, session, record_bag=record_bag, + commands_enabled=commands_enabled, + resume_from=( + None if resume is None else resume.source_session + ), + ), + cwd=config.workspace, + env=_overlay_environment(config.sdk_setup), + stdout=log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + ready = _wait_until_o12( monitor, process, lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"}, timeout=120.0, ) + if not ready and not monitor.status: + exception = _log_exception_summary(log_path) + print( + "O12 标定节点初始化超时:设备进程已启动,但标定节点未发布状态;" + "请查看日志中的节点构造或断点导入阶段。" + f"日志:{log_path}", + flush=True, + ) + if exception: + print(f"标定节点异常:{exception}", flush=True) + return 2 if not ready or monitor.status.get("state") != "READY": print( "O12 启动预检失败:请检查 HCAN、POSITION 模式、错误/温度回读、" @@ -167,21 +476,29 @@ def _run_online( if response is None or not response.success: print(f"O12 标定未启动:{getattr(response, 'message', '')}", flush=True) return 2 - print("O12 标定已自动开始:POSITION,20 Hz,弧度余弦轨迹。", flush=True) - finished = _wait_until( + print( + "O12 标定已自动开始:POSITION,50 Hz,平滑限速轨迹。", + flush=True, + ) + finished = _wait_until_o12( monitor, process, lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"}, timeout=None, ) if not finished or monitor.status.get("state") != "PASSED": + exception = _log_exception_summary(log_path) print( "O12 标定已安全停止并保持当前位置:" + str(monitor.status.get("reason", "process_exit")), flush=True, ) + if exception: + print(f"标定节点异常:{exception}", flush=True) + print(f"诊断日志:{log_path}", flush=True) return 3 print("\n".join(( - "PASS:O12 右手 11 个实测任务和独立 holdout 已通过。", + "PASS:O12 右手 11 个实测任务和独立 holdout 已通过;" + "thumb_mcp 静态零位保留原始 CAD。", f"发布结果:{config.session_root / 'latest_passed'}", f"JSON:{monitor.status.get('final_json')}", f"URDF:{monitor.status.get('final_urdf')}", @@ -196,7 +513,8 @@ def _run_online( monitor.destroy_node() if rclpy.ok(): rclpy.shutdown() - _stop_stack(process) + if process is not None: + _stop_stack(process) log_stream.flush() os.fsync(log_stream.fileno()) log_stream.close() @@ -212,6 +530,10 @@ def main(args: list[str] | None = None) -> None: parser.add_argument("--offline-raw", default="") parser.add_argument("--offline-output", default="") parser.add_argument("--publish-offline", action="store_true") + parser.add_argument( + "--no-resume", action="store_true", + help="忽略兼容的失败会话,从头开始采集", + ) selected = parser.parse_args(args) config = load_product_config( selected.config, @@ -234,22 +556,20 @@ def main(args: list[str] | None = None) -> None: session_dir=output, serial_number=config.serial_number, source_urdf=config.source_urdf, - protected_inputs={ - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - "tag_config_sha256": config.tag_config_sha256, - "sdk_config_sha256": config.sdk_config_sha256, - }, + protected_inputs=_protected_inputs(config), records=load_o12_raw_samples(selected.offline_raw), publish=selected.publish_offline, ) - print(f"离线回放PASS:schema {payload['schema_version']},URDF {correction.path}") + print( + f"离线回放PASS:schema {payload['schema_version']}," + f"thumb_mcp 静态零位保留原始 CAD,URDF {correction.path}" + ) return raise SystemExit(_run_online( config, record_bag=selected.record_bag, commands_enabled=not selected.commands_disabled, + allow_resume=not selected.no_resume, )) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py index e81079d..a1c1595 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py @@ -10,23 +10,144 @@ import re from typing import Mapping import xml.etree.ElementTree as ET -from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches -from .fitting import O12FitResult +import numpy as np + +from ...core.urdf import ( + UrdfJointPatch, + UrdfPatchSet, + validate_urdf_mimic_ranges, + write_urdf_patches, +) +from .fitting import O12FitResult, measured_curve_bounds +from .kinematics import PASSIVE_SDK_SOURCE_BY_JOINT, vendor_passive_curve +from ..l6.urdf import _corrected_origin_rpy from .profile import ( + ACTIVE_JOINTS, CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, MEASURED_PASSIVE_JOINTS, + MIMIC_SOURCE_BY_JOINT, + SDK_TO_URDF_SIGN, + STATIC_ZERO_EXCLUDED_JOINTS, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, ) -SPLAY_JOINTS = frozenset( - {"index_mcp_roll", "middle_mcp_roll"} -) +SPLAY_JOINTS = frozenset({"index_mcp_roll", "middle_mcp_roll"}) + + +def _measured_curve_bounds( + name: str, result: O12FitResult, *, sign: float +) -> tuple[float, float]: + donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name) + return measured_curve_bounds( + donor, + result.curves[donor], + result.feedback_domains_rad[donor], + sign=sign, + ) + + +def _source_reachable_upper( + name: str, + joints: Mapping[str, ET.Element], + cache: dict[str, float], +) -> float: + if name in cache: + return cache[name] + node = joints[name] + limit = node.find("limit") + if limit is None or limit.get("upper") is None: + raise ValueError(f"source URDF joint has no upper limit: {name}") + mimic = node.find("mimic") + if mimic is None: + value = float(limit.get("upper")) + else: + source_name = str(mimic.get("joint", "")) + if source_name not in joints: + raise ValueError(f"source URDF mimic source is missing: {source_name}") + value = ( + float(mimic.get("offset", "0")) + + float(mimic.get("multiplier", "1")) + * _source_reachable_upper(source_name, joints, cache) + ) + if not math.isfinite(value): + raise ValueError(f"source URDF reachable endpoint is invalid: {name}") + cache[name] = value + return value + + +def o12_active_ranges(result: O12FitResult) -> dict[str, tuple[float, float]]: + """Return the exact active ranges used by O12 JSON and URDF publication.""" + ranges: dict[str, tuple[float, float]] = {} + for name in sorted( + CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT) + ): + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + continue + measured_lower, measured_upper = _measured_curve_bounds( + name, + result, + sign=SDK_TO_URDF_SIGN[COMMAND_INDEX_BY_JOINT[name]], + ) + ranges[name] = ( + (measured_lower, measured_upper) + if name in SPLAY_JOINTS + else (min(0.0, measured_lower), max(0.0, measured_upper)) + ) + return ranges + + +def o12_endpoint_mimic_contract( + source_urdf: str | Path, + result: O12FitResult, +) -> dict[str, float]: + """Linear URDF mimics that preserve the source-CAD closed chain pose. + + O12 runtime uses the nonlinear vendor solver. URDF ``mimic`` is only a + linear visualization fallback, so choose its coefficient to reproduce the + reviewed CAD mechanical endpoint rather than fitting an arbitrary planar + Tag phase. Chained pinky DIP is resolved after pinky PIP. + """ + root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() + joints = { + str(node.get("name")): node + for node in root.findall("joint") + if node.get("type") == "revolute" + } + reachable_source: dict[str, float] = {} + desired = { + name: _source_reachable_upper(name, joints, reachable_source) + for name in MEASURED_PASSIVE_JOINTS + } + corrected_source_upper = { + name: values[1] for name, values in o12_active_ranges(result).items() + } + multipliers: dict[str, float] = {} + for name in ( + "thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip" + ): + node = joints[name] + mimic = node.find("mimic") + if mimic is None: + raise ValueError(f"O12 passive joint has no mimic element: {name}") + source_name = MIMIC_SOURCE_BY_JOINT[name] + source_endpoint = corrected_source_upper.get(source_name, desired.get(source_name)) + if source_endpoint is None or abs(source_endpoint) <= 1.0e-12: + raise ValueError(f"O12 mimic source endpoint is invalid: {source_name}") + offset = float(mimic.get("offset", "0")) + multiplier = (desired[name] - offset) / source_endpoint + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"O12 endpoint mimic is invalid: {name}") + multipliers[name] = multiplier + corrected_source_upper[name] = desired[name] + return multipliers @dataclass(frozen=True) class O12UrdfCorrection: path: Path + origin_offsets_rad: Mapping[str, float] corrected_limits_rad: Mapping[str, tuple[float, float]] mimic_multipliers: Mapping[str, float] preserved_ring_fields: tuple[str, ...] @@ -40,11 +161,13 @@ def write_o12_corrected_urdf( result: O12FitResult, timestamp: str | None = None, ) -> O12UrdfCorrection: - """Patch only measured travel and dynamic mimic terms. + """Write full-range visual joint limits and equivalent endpoint mimics. - Tag mounting angle cannot be separated from a static passive-joint zero, - so origins and passive limits stay byte-for-byte CAD. The unobserved ring - keeps its own origin, limits and both CAD mimic ratios. + O12 SDK feedback is the continuous input coordinate, while the 16-Tag + trajectories identify the corresponding 19-joint URDF motion. The source + CAD is never allowed to truncate acquisition. Ring geometry and mimic + ratios remain CAD-owned; its active range remains constrained by that + preserved chain. """ source = Path(source_urdf).expanduser().resolve() if not source.is_file(): @@ -57,6 +180,23 @@ def write_o12_corrected_urdf( raise ValueError("O12 travel result has the wrong active joint set") if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS: raise ValueError("O12 mimic result has the wrong passive joint set") + for name in STATIC_ZERO_EXCLUDED_JOINTS: + if ( + not math.isclose( + float(result.zero_offsets_rad.get(name, math.nan)), + 0.0, + rel_tol=0.0, + abs_tol=1.0e-12, + ) + or result.zero_method_by_joint.get(name) + != "source_cad_zero_profile_excluded" + ): + raise ValueError( + f"O12 {name} must retain immutable source-CAD static zero" + ) + for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): + if not math.isclose(result.zero_offsets_rad[target], result.zero_offsets_rad[donor], abs_tol=1.0e-9, rel_tol=0.0): + raise ValueError(f"{target} static zero differs from transfer donor {donor}") root = ET.parse(source).getroot() joints = { @@ -72,40 +212,95 @@ def write_o12_corrected_urdf( corrected_limits: dict[str, tuple[float, float]] = {} patches: dict[str, UrdfJointPatch] = {} - for name in sorted(CALIBRATED_ACTIVE_JOINTS): + active_targets = CALIBRATED_ACTIVE_JOINTS | set( + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT + ) + measured_active_ranges = o12_active_ranges(result) + active_ranges: dict[str, tuple[float, float]] = {} + for name in sorted(active_targets): limit = joints[name].find("limit") if limit is None: raise ValueError(f"O12 source active joint has no limit: {name}") - cad_lower = float(limit.get("lower", "nan")) - cad_upper = float(limit.get("upper", "nan")) + lower_text = limit.get("lower") + upper_text = limit.get("upper") + if lower_text is None or upper_text is None: + raise ValueError(f"O12 source active joint has incomplete limits: {name}") + cad_lower = float(lower_text) + cad_upper = float(upper_text) travel = abs(float(result.travels_rad[name])) if not math.isfinite(travel) or travel <= math.radians(2.0): raise ValueError(f"invalid measured O12 travel: {name}") - if name in SPLAY_JOINTS: - half = min(0.5 * travel, abs(cad_lower), abs(cad_upper)) - lower, upper = -half, half - else: - lower, upper = max(0.0, cad_lower), min(cad_upper, travel) - if not lower < upper: + if not cad_lower < cad_upper: raise ValueError(f"invalid corrected O12 limits: {name}") + if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: + # The ring has no Tag. Preserve its own reviewed CAD range and + # passive-chain contract rather than transplanting pinky geometry. + lower, upper = cad_lower, cad_upper + else: + lower, upper = measured_active_ranges[name] + if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper: + raise ValueError(f"invalid measured O12 limits: {name}") corrected_limits[name] = (lower, upper) + active_ranges[name] = (lower, upper) + origin_offset = float(result.zero_offsets_rad.get(name, 0.0)) patches[name] = UrdfJointPatch( + origin_rpy=( + _corrected_origin_rpy(joints[name], origin_offset) + if abs(origin_offset) > 1.0e-12 else None + ), limit_lower=f"{lower:.15g}", limit_upper=f"{upper:.15g}" ) - mimic_multipliers: dict[str, float] = {} + mimic_multipliers = o12_endpoint_mimic_contract(source, result) for name in sorted(MEASURED_PASSIVE_JOINTS): mimic = joints[name].find("mimic") if mimic is None: raise ValueError(f"O12 passive joint has no mimic element: {name}") - multiplier = float(result.mimic_fits[name].urdf_mimic_multiplier) + multiplier = float(mimic_multipliers[name]) if not math.isfinite(multiplier) or not 0.5 <= multiplier <= 2.2: raise ValueError(f"invalid O12 mimic multiplier: {name}") - mimic_multipliers[name] = multiplier patches[name] = UrdfJointPatch( mimic_multiplier=f"{multiplier:.15g}" ) + # Passive limits remain CAD-owned unless the reviewed vendor polynomial + # itself has a small interior extremum outside that range (index DIP does). + # Never expand a physical limit from the noisier Tag-derived curve. + for name in ( + "thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip" + ): + node = joints[name] + limit = node.find("limit") + assert limit is not None + cad_lower = float(limit.get("lower", "nan")) + cad_upper = float(limit.get("upper", "nan")) + sdk_source = PASSIVE_SDK_SOURCE_BY_JOINT[name] + feedback_lower, feedback_upper = result.feedback_domains_rad[name] + inputs = np.linspace( + min(0.0, feedback_lower), max(0.0, feedback_upper), 257 + ) + motor = COMMAND_INDEX_BY_JOINT[sdk_source] + vendor_values = vendor_passive_curve( + name, + inputs, + sdk_to_urdf_sign=SDK_TO_URDF_SIGN[motor], + ) + lower = min(cad_lower, *vendor_values) + upper = max(cad_upper, *vendor_values) + if not lower < upper: + raise ValueError(f"invalid corrected O12 passive limits: {name}") + corrected_limits[name] = (lower, upper) + existing = patches[name] + patches[name] = UrdfJointPatch( + limit_lower=( + f"{lower:.15g}" if lower < cad_lower - 1.0e-12 else None + ), + limit_upper=( + f"{upper:.15g}" if upper > cad_upper + 1.0e-12 else None + ), + mimic_multiplier=existing.mimic_multiplier, + ) + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: raise ValueError("O12 URDF timestamp must use YYYYMMDD_HHMMSS") @@ -125,16 +320,32 @@ def write_o12_corrected_urdf( forbidden_source_stem_patterns=(r"calibrated",), copy_complete_mesh_directory=True, ) + try: + validate_urdf_mimic_ranges(destination, reference_urdf=source) + except Exception: + # Publication is transactional: a generated file that failed the + # physical mimic-chain gate must never remain as a plausible result. + destination.unlink(missing_ok=True) + raise return O12UrdfCorrection( path=destination, + origin_offsets_rad={ + name: float(result.zero_offsets_rad[name]) + for name in ACTIVE_JOINTS + }, corrected_limits_rad=corrected_limits, mimic_multipliers=mimic_multipliers, preserved_ring_fields=( - "ring_mcp_pitch.origin", "ring_mcp_pitch.limit", + "ring_mcp_pitch.origin.xyz", "ring_mcp_pitch.limit", "ring_pip.origin", "ring_pip.limit", "ring_pip.mimic", "ring_dip.origin", "ring_dip.limit", "ring_dip.mimic", ), ) -__all__ = ["O12UrdfCorrection", "write_o12_corrected_urdf"] +__all__ = [ + "O12UrdfCorrection", + "o12_active_ranges", + "o12_endpoint_mimic_contract", + "write_o12_corrected_urdf", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py new file mode 100644 index 0000000..7b6306d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py @@ -0,0 +1,180 @@ +"""O12 observation graph for the shared G20 spatial-zero solver. + +Parallel flexion axes observe phase through their line positions, not their +directions. SDK travel endpoints are deliberately not treated as CAD datums. +Passive joints supply observations but never independently fitted static zeros. +""" + +from dataclasses import asdict, replace +import math +from pathlib import Path +from typing import Any, Mapping, Sequence + +from ..g20.profile import JointCurveFit, JointSpec +from ..g20.zero_solver import ( + ZeroCalibrationProfile, ZeroSolveResult, + fit_joint_axis_measurement, solve_urdf_zero_offsets, + with_depth_free_axis_projection, +) +from .profile import ( + CALIBRATED_ACTIVE_JOINTS, + COMMAND_INDEX_BY_JOINT, +) +from .kinematics import PASSIVE_SDK_SOURCE_BY_JOINT + +SPATIAL_ZERO_POLICY = ( + "o12_full_hand_spatial_v5_thumb_mcp_cad_static_stable_pnp_bias" +) + +AXIS_PARENT = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + "index_mcp_pitch": "index_mcp_roll", + "middle_mcp_pitch": "middle_mcp_roll", +} +PHASE_PARENT = { + "thumb_mcp": "thumb_cmc_pitch", + "index_pip": "index_mcp_pitch", "index_dip": "index_pip", + "middle_pip": "middle_mcp_pitch", "middle_dip": "middle_pip", + "pinky_pip": "pinky_mcp_pitch", +} +ZERO_OBSERVER = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "index_mcp_roll": "index_mcp_pitch", + "index_mcp_pitch": "index_pip", "index_pip": "index_dip", + "middle_mcp_roll": "middle_mcp_pitch", + "middle_mcp_pitch": "middle_pip", "middle_pip": "middle_dip", + "pinky_mcp_pitch": "pinky_pip", +} +# Order also ensures an upstream axis exists before its passive observer. +AXIS_JOINTS = ( + "thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp", + "pinky_mcp_pitch", "pinky_pip", + "middle_mcp_roll", "middle_mcp_pitch", "middle_pip", "middle_dip", + "index_mcp_roll", "index_mcp_pitch", "index_pip", "index_dip", +) + + +def motor_index(joint: str) -> int: + return COMMAND_INDEX_BY_JOINT[PASSIVE_SDK_SOURCE_BY_JOINT.get(joint, joint)] + + +def full_hand_zero_profile() -> ZeroCalibrationProfile: + # Reuse the existing root orientation datum; adding flexion phases must + # not silently change the palm frame or the fixed SDK channel contract. + from .fitting import _thumb_root_zero_profile + root = _thumb_root_zero_profile() + specs = { + name: JointSpec(name, motor_index(name), name in CALIBRATED_ACTIVE_JOINTS, + None, None, None, pose_axis_line_required=name in PHASE_PARENT) + for name in AXIS_JOINTS + } + return replace( + root, hand=replace(root.hand, joint_specs=specs), + direct_zero_joints=tuple(ZERO_OBSERVER), axis_joints=AXIS_JOINTS, + constrained_circle_joints=frozenset(AXIS_JOINTS), + axis_parent_joint=AXIS_PARENT, phase_parent_joint=PHASE_PARENT, + offset_observer_joint=ZERO_OBSERVER, + accept_validated_zero_in_confidence_interval=True, + project_axis_gauge_before_image=True, + ) + + +class O12SpatialZeroError(ValueError): + """Unpublishable geometry, with machine-readable per-joint evidence.""" + + def __init__(self, message: str, diagnostics: Mapping[str, Any]) -> None: + super().__init__(message) + self.diagnostics = diagnostics + # Populated only after the full dynamic fit and an identifiable + # spatial estimate exist. Never a substitute for a passed result. + self.review_fit = None + + +def solve_full_hand_zero( + source_urdf: str | Path, + virtual_records: Mapping[str, Sequence[Mapping[str, Any]]], + solver_curves: Mapping[str, JointCurveFit], +) -> ZeroSolveResult: + profile = full_hand_zero_profile() + measurements = [] + for cycle in range(4): + by_joint = {} + for name in AXIS_JOINTS: + rows = virtual_records[name] + # Relative passive motion is measured in its moving parent Tag + # frame. Its common-frame line is evaluated at the same baseline + # condition, just as in G20; it is not a circle of the entire chain. + parent = PHASE_PARENT.get(name) + # PHASE_PARENT declares parallel physical flexion axes, including + # ACTIVE PIP/MCP axes. Independent planar-PnP direction errors must + # not turn those into skew lines and then appear as encoder zeros. + # The task profile holds nonparallel upstream axes at the same + # pose for these paired captures. Preserve each child's measured + # scalar rotation; constrain only the shared physical direction. + constraint = by_joint[parent].axis_common_xyz if parent in by_joint else None + try: + item = fit_joint_axis_measurement( + name, rows, cycle=cycle, zero_command_u8=255, + constrained_circle_joints=profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + axis_common_constraint=constraint, + separate_axial_residual=True, + ) + except ValueError as error: + raise O12SpatialZeroError( + f"O12 spatial zero observation failed:{name}:cycle={cycle}:{error}", + {"passed": False, "stage": "axis_observation", "joint": name, + "cycle": cycle, "reason": str(error)}, + ) from error + item = with_depth_free_axis_projection(item, rows[0]["camera_center_common_xyz_m"]) + measurements.append(item) + by_joint[name] = item + try: + result = solve_urdf_zero_offsets( + source_urdf=source_urdf, measurements=measurements, curves=solver_curves, + motor_by_joint={name: motor_index(name) for name in AXIS_JOINTS}, + training_cycles=(0, 1, 2), validation_cycle=3, + maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20), + maximum_cycle_difference_rad=math.radians(.75), + minimum_applied_offset_rad=math.radians(.1), + maximum_validation_mae_rad=math.radians(1), + maximum_validation_p95_rad=math.radians(2), + maximum_validation_error_rad=math.radians(3), + maximum_confidence_half_width_rad=math.radians(1), + # Match G20's established treatment of a repeatable planar-Tag + # direction bias: 5 deg remains the normal cone target, while a + # stable (<=0.75 deg cycle spread) bias up to 15 deg is retained + # as an explicit diagnostic instead of corrupting/rejecting an + # otherwise observable encoder zero. + maximum_systematic_axis_cone_bias_rad=math.radians(15), + maximum_pose_axis_line_rms_m=.0015, + hand_type="right", tag_layout="o12_right_16", zero_profile=profile, + ) + except ValueError as error: + raise O12SpatialZeroError( + "O12 full-hand spatial zero geometry is invalid:" + str(error), + {"passed": False, "stage": "spatial_solve", "reason": str(error), + "axis_measurements": [asdict(item) for item in measurements]}, + ) from error + result = replace(result, axis_residual_diagnostics={ + f"{item.joint}:cycle{item.cycle}": { + "axial_component_separated": item.axis_point_axial_component_separated, + "fit_rms_m": item.pose_axis_line_rms_m, + "raw_rms_m": item.pose_axis_line_raw_rms_m, + "axial_rms_m": item.pose_axis_line_axial_rms_m, + "transverse_rms_m": item.pose_axis_line_transverse_rms_m, + } for item in measurements + }) + if not result.passed: + details = ",".join(f"{n}={r}" for n, r in sorted(result.failure_reasons.items())) + raise O12SpatialZeroError( + "O12 full-hand spatial zero solve failed:" + details, + {"passed": False, "stage": "spatial_solve", "policy": SPATIAL_ZERO_POLICY, + "result": asdict(result), + "axis_measurements": [asdict(item) for item in measurements]}, + ) + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py index 42c7c70..8386b0f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py @@ -272,11 +272,6 @@ def fit_o6_session( ) if fit.maximum_monotonic_correction_rad > correction_limit: raise ValueError(f"{name} monotonic correction exceeds limit") - if fit.maximum_hysteresis_rad > math.radians(MAXIMUM_HYSTERESIS_DEG): - raise ValueError( - f"{name} hysteresis {math.degrees(fit.maximum_hysteresis_rad):.3f} " - f"degrees exceeds O6 limit {MAXIMUM_HYSTERESIS_DEG:.3f} degrees" - ) curves[name] = fit holdout[name] = errors cycle_curves[name] = { diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py index e0445b4..2210b65 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py @@ -8,6 +8,8 @@ import math from pathlib import Path from typing import Any, Mapping, Sequence +from ...runtime.engine import CalibrationEngine + from .artifacts import ( artifact_hashes, atomic_write_json, @@ -24,6 +26,7 @@ from .profile import ( MEASURED_PASSIVE_JOINTS, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, ) from .urdf import O6UrdfCorrection, write_o6_corrected_urdf @@ -111,6 +114,13 @@ def finalize_o6_session( result = fit_o6_session( source_urdf, accepted_records_by_joint(records), require_thumb_axis_zero=True ) + CalibrationEngine(build_typed_profile()).result_from_fit( + result, + transfers={ + **TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + **TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + }, + ) payload = build_o6_runtime_payload( serial_number=serial_number, source_urdf=source_urdf, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py index 127eded..84112c2 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py @@ -217,8 +217,8 @@ def build_typed_profile() -> CalibrationProfile: ), motion=MotionPolicy( tasks=tasks, - precheck_sweeps=True, - steady_command_checkpoints=True, + precheck_sweeps=False, + steady_command_checkpoints=False, speed_parameters={ "baseline_u8": BASELINE_SPEED_U8, "preflight_u8": PREFLIGHT_SPEED_U8, @@ -253,7 +253,6 @@ def build_typed_profile() -> CalibrationProfile: hard_threshold_keys=frozenset({ "minimum_detection_rate", "maximum_state_image_skew_ms", - "maximum_hysteresis_rad", "maximum_validation_error_rad", "maximum_mimic_residual_rad", }), diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py index 79e020e..b2fcd54 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py @@ -6,11 +6,13 @@ from dataclasses import dataclass from typing import Any, Callable, Iterator from ..core import CalibrationProfile, ProfileKey, validate_profile +from ..runtime.engine import CalibrationEngine +from ..runtime.adapters import ProfileSdkAdapter @dataclass(frozen=True) class EngineBindings: - """Temporary bridge from typed policies to the proven engine objects.""" + """Small model plugin: SDK I/O hooks plus compatible CLI serializers.""" hand_profile: Any zero_profile: Any @@ -19,6 +21,7 @@ class EngineBindings: return_waypoints: Callable[..., tuple[tuple[int, ...], ...]] cli_main: Callable[[list[str] | None], None] node_main: Callable[[list[str] | None], None] + sdk_adapter_factory: Callable[[Any], ProfileSdkAdapter] = ProfileSdkAdapter @dataclass(frozen=True) @@ -26,6 +29,12 @@ class RegisteredProfile: profile: CalibrationProfile engine: EngineBindings + def build_engine(self) -> CalibrationEngine: + return CalibrationEngine(self.profile) + + def build_sdk_adapter(self) -> ProfileSdkAdapter: + return self.engine.sdk_adapter_factory(self.profile.command) + class ProfileRegistry: """An in-package registry; no discovery plugins or string evaluation.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py index 7081990..d8ad4cf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py @@ -1,11 +1,22 @@ -"""Common orchestration; ROS integration is isolated below ``nodes``.""" +"""Common calibration engine; model registry imports remain lazy.""" from .controller import ControllerSnapshot, SessionController, SessionState -from .runner import build_session_controller +from .engine import ( + ACQUISITION_POLICY_VERSION, + CalibrationEngine, + CalibrationResult, + ScanUnit, + SweepQuality, +) + + +def build_session_controller(*args, **kwargs): + # Avoid a registry/runtime import cycle while preserving the public API. + from .runner import build_session_controller as build + return build(*args, **kwargs) __all__ = [ - "ControllerSnapshot", - "SessionController", - "SessionState", - "build_session_controller", + "ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult", + "ControllerSnapshot", "ScanUnit", "SessionController", "SessionState", + "SweepQuality", "build_session_controller", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py index 7b286f8..f1ec941 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py @@ -1 +1,5 @@ """Camera, detector, and hand-SDK runtime adapters.""" + +from .base import HardwareHealth, ProfileSdkAdapter, SdkAdapter + +__all__ = ["HardwareHealth", "ProfileSdkAdapter", "SdkAdapter"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py new file mode 100644 index 0000000..5ed9b35 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py @@ -0,0 +1,108 @@ +"""Hardware-facing contract used by the model-independent calibration engine. + +Adapters deliberately stop at the SDK boundary. Tag geometry, task ordering, +fitting, and URDF edits belong to :class:`CalibrationProfile`, so adding a hand +does not add another acquisition state machine. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Protocol, Sequence, runtime_checkable + +from ...core import CommandLayout + + +@dataclass(frozen=True) +class HardwareHealth: + connected: bool + position_mode: bool + active_faults: tuple[str, ...] = () + + @property + def safe(self) -> bool: + return self.connected and self.position_mode and not self.active_faults + + +@runtime_checkable +class SdkAdapter(Protocol): + """Minimum SDK surface required by ``CalibrationEngine``. + + ROS publishers/subscribers or vendor API objects can implement this + protocol. The engine never assumes byte commands, radians, CAN names, or + a particular ``JointState.name`` ordering. + """ + + @property + def command_layout(self) -> CommandLayout: ... + + def parse_feedback( + self, names: Sequence[str], values: Sequence[float] + ) -> tuple[float, ...] | None: ... + + def validate_command(self, values: Sequence[float]) -> tuple[float, ...]: ... + + def health(self) -> HardwareHealth: ... + + def publish_position(self, values: Sequence[float]) -> None: ... + + def set_speed(self, command_index: int, speed: float) -> None: ... + + +class ProfileSdkAdapter: + """Shared profile-driven parsing and physical-domain validation. + + Live model adapters subclass this object only to implement SDK I/O and + health. The parsing rules are shared by legacy-byte and physical-angle + model nodes. + """ + + def __init__(self, command_layout: CommandLayout) -> None: + self.command_layout = command_layout + + def parse_feedback( + self, names: Sequence[str], values: Sequence[float] + ) -> tuple[float, ...] | None: + layout = self.command_layout + if len(values) != layout.command_count: + return None + if names and not layout.feedback_by_index: + by_name = dict(zip((str(name) for name in names), values)) + for alias, canonical in layout.feedback_name_aliases.items(): + if alias in by_name and canonical not in by_name: + by_name[canonical] = by_name[alias] + if any(name not in by_name for name in layout.names): + return None + parsed = tuple(float(by_name[name]) for name in layout.names) + else: + parsed = tuple(float(value) for value in values) + return parsed if all(math.isfinite(value) for value in parsed) else None + + def validate_command(self, values: Sequence[float]) -> tuple[float, ...]: + layout = self.command_layout + command = tuple(float(value) for value in values) + if len(command) != layout.command_count: + raise ValueError("command channel count differs from profile") + for index, value in enumerate(command): + if ( + not math.isfinite(value) + or value < layout.minimum_values[index] + or value > layout.maximum_values[index] + ): + raise ValueError( + f"command outside physical range: channel={index}:value={value}" + ) + return command + + def health(self) -> HardwareHealth: # pragma: no cover - live subclass hook + raise NotImplementedError + + def publish_position(self, values: Sequence[float]) -> None: # pragma: no cover + raise NotImplementedError + + def set_speed(self, command_index: int, speed: float) -> None: # pragma: no cover + raise NotImplementedError + + +__all__ = ["HardwareHealth", "ProfileSdkAdapter", "SdkAdapter"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py new file mode 100644 index 0000000..f24ddd9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py @@ -0,0 +1,322 @@ +"""Model-independent calibration schedule and lightweight acceptance policy.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import math +from typing import Any, Mapping, Sequence + +import numpy as np + +from ..core import CalibrationProfile, TaskSpec, validate_profile + + +ACQUISITION_POLICY_VERSION = "unified_engine_v1" +TRAINING_CYCLES = (0, 1, 2) +HOLDOUT_CYCLE = 3 + + +@dataclass(frozen=True) +class ScanUnit: + task_key: str + cycle: int + direction: str + start: float + end: float + speed: float + + +@dataclass(frozen=True) +class SweepQuality: + passed: bool + failures: tuple[str, ...] + warnings: tuple[str, ...] + metrics: Mapping[str, Any] + + +@dataclass(frozen=True) +class CalibrationResult: + """Unified internal result; serializers retain each deployed schema.""" + + profile_id: str + policy_version: str + curves: Mapping[str, Any] + zero_offsets_rad: Mapping[str, float] + travel: Mapping[str, float] + coupling: Mapping[str, Any] + transfers: Mapping[str, str] + quality: Mapping[str, Any] + metadata: Mapping[str, Any] = field(default_factory=dict) + + +class CalibrationEngine: + """One policy kernel shared by all model-specific ROS/SDK wrappers.""" + + def __init__(self, profile: CalibrationProfile) -> None: + validate_profile(profile) + self.profile = profile + + @property + def checkpoint_token(self) -> str: + return ACQUISITION_POLICY_VERSION + + def scan_units(self) -> tuple[ScanUnit, ...]: + units: list[ScanUnit] = [] + for task in self.profile.motion.tasks: + speed = self._formal_speed(task) + for cycle in (*TRAINING_CYCLES, HOLDOUT_CYCLE): + units.extend(( + ScanUnit( + task.key, cycle, "decreasing", task.start_value, + task.end_value, speed, + ), + ScanUnit( + task.key, cycle, "increasing", task.end_value, + task.start_value, speed, + ), + )) + return tuple(units) + + def mapping_probe_delta(self, task: TaskSpec) -> float | None: + """Only physical-angle profiles perform a single <=3 degree jog.""" + if self.profile.command.unit != "rad": + return None + distance = task.end_value - task.start_value + return math.copysign( + min(abs(distance), self.profile.acquisition.mapping_probe_maximum_rad), + distance, + ) + + def retry_speed(self, original_speed: float, attempt: int) -> float: + if int(attempt) != 2: + raise ValueError("unified engine permits exactly one rescan") + return float(original_speed) + + @staticmethod + def permits_retry(stage: str, completed_retries: int) -> bool: + """Only an under-sampled scan direction gets one same-speed retry.""" + return str(stage) == "sweep_acquisition" and int(completed_retries) == 0 + + def evaluate_sweep( + self, + feedback_progress_01: Sequence[float], + *, + minimum_span: float, + total_frames: int, + joint_frame_rate: float, + feedback_hz: float, + detection_rate: float, + bin_count: int = 256, + ) -> SweepQuality: + """Judge fitting observability; ideal rates are diagnostics only.""" + values = np.asarray(feedback_progress_01, dtype=float) + values = np.clip(values[np.isfinite(values)], 0.0, 1.0) + bins = sorted(set( + min(bin_count - 1, max(0, int(value * bin_count))) + for value in values + )) + span = float(np.ptp(values)) if values.size else 0.0 + internal_missing_runs = [ + right - left - 1 for left, right in zip(bins, bins[1:]) + ] + # Byte-command products have a known 0..255 feedback domain, so an + # unobserved endpoint is a real acquisition hole. A radian product's + # feedback endpoint scale is itself being calibrated: its separate + # span/repeatability gate proves effective travel, while this gate must + # only reject holes *inside* that observed physical stroke. + gap_scope = ( + "full_command_domain" + if self.profile.command.unit == "u8" + else "observed_feedback_span" + ) + missing_runs = ( + [bins[0], bin_count - 1 - bins[-1], *internal_missing_runs] + if bins and gap_scope == "full_command_domain" + else internal_missing_runs + if bins + else [bin_count] + ) + maximum_gap = max(missing_runs, default=0) + policy = self.profile.acquisition + allowed_gap = max( + 1, int(math.floor(bin_count * policy.maximum_unobserved_fraction)) + ) + failures: list[str] = [] + if values.size < policy.minimum_valid_samples: + failures.append(f"frames={values.size}") + if span < float(minimum_span): + failures.append("feedback_span") + if len(bins) < policy.minimum_bins: + failures.append(f"bins={len(bins)}") + if maximum_gap > allowed_gap: + failures.append(f"maximum_gap={maximum_gap}") + warnings: list[str] = [] + if detection_rate < 0.95: + warnings.append(f"tag_rate={detection_rate:.3f}") + if joint_frame_rate < 0.85: + warnings.append(f"joint_frame_rate={joint_frame_rate:.3f}") + # A rate target is useful to diagnose latency but valid synchronized + # samples, coverage, and gaps are the actual correctness evidence. + if feedback_hz <= 0.0: + warnings.append("feedback_rate_unavailable") + metrics = { + "policy_version": ACQUISITION_POLICY_VERSION, + "valid_frames": int(values.size), + "total_frames": int(total_frames), + "feedback_span": span, + "feedback_bins": len(bins), + "maximum_bin_gap": maximum_gap, + "allowed_maximum_bin_gap": allowed_gap, + "gap_scope": gap_scope, + "joint_frame_rate": float(joint_frame_rate), + "feedback_hz": float(feedback_hz), + "tag_detection_rate": float(detection_rate), + } + return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics) + + def resume_compatible(self, session_start: Mapping[str, Any]) -> bool: + return ( + session_start.get("acquisition_policy_version") + == ACQUISITION_POLICY_VERSION + and session_start.get("profile_id") == self.profile.key.profile_id + ) + + def result_from_fit( + self, + fit_result: Any, + *, + transfers: Mapping[str, str] | None = None, + holdout_errors_rad: Mapping[str, Sequence[float]] | None = None, + curves: Mapping[str, Any] | None = None, + zero_offsets_rad: Mapping[str, float] | None = None, + travel: Mapping[str, float] | None = None, + coupling: Mapping[str, Any] | None = None, + metadata: Mapping[str, Any] | None = None, + ) -> CalibrationResult: + """Normalize a model fitter result before its legacy serializer runs. + + Model-specific serializers deliberately remain responsible for the + deployed v4/v6/v7 JSON shapes. This object is the common publication + gate between fitting and both JSON/URDF writers. + """ + holdout = ( + holdout_errors_rad + if holdout_errors_rad is not None + else getattr(fit_result, "holdout_errors_rad", {}) + ) + quality_by_joint: dict[str, Mapping[str, float]] = {} + fit_diagnostics_by_joint: dict[str, Mapping[str, float]] = {} + all_errors: list[float] = [] + for name, values in dict(holdout).items(): + absolute = np.abs(np.asarray(tuple(values), dtype=float)) + if absolute.size == 0 or not np.all(np.isfinite(absolute)): + raise ValueError(f"holdout evidence is missing or invalid: {name}") + mae = float(np.mean(absolute)) + p95 = float(np.percentile(absolute, 95.0)) + maximum = float(np.max(absolute)) + if ( + mae > math.radians(1.0) + or p95 > math.radians(2.0) + or maximum > math.radians(3.0) + ): + raise ValueError(f"holdout quality failed: {name}") + quality_by_joint[str(name)] = { + "mae_rad": mae, + "p95_rad": p95, + "maximum_rad": maximum, + } + all_errors.extend(float(value) for value in absolute) + if not quality_by_joint: + raise ValueError("independent holdout evidence is required") + normalized_curves = dict( + curves if curves is not None else getattr(fit_result, "curves", {}) + ) + for name, curve in normalized_curves.items(): + diagnostics: dict[str, float] = {} + for field_name in ( + "maximum_hysteresis_rad", + "maximum_monotonic_correction_rad", + ): + value = getattr(curve, field_name, None) + if value is None: + continue + numeric = float(value) + if not math.isfinite(numeric) or numeric < 0.0: + raise ValueError( + f"fit diagnostic is invalid: {name}.{field_name}" + ) + diagnostics[field_name] = numeric + if diagnostics: + fit_diagnostics_by_joint[str(name)] = diagnostics + offsets = dict( + zero_offsets_rad + if zero_offsets_rad is not None + else getattr(fit_result, "zero_offsets_rad", {}) + ) + normalized_travel = dict( + travel + if travel is not None + else getattr( + fit_result, + "travels_rad", + getattr(fit_result, "travel", {}), + ) + ) + if not normalized_curves or not offsets: + raise ValueError("fit result is missing curves or zero offsets") + result = CalibrationResult( + profile_id=self.profile.key.profile_id, + policy_version=ACQUISITION_POLICY_VERSION, + curves=normalized_curves, + zero_offsets_rad=offsets, + travel=normalized_travel, + coupling=dict( + coupling + if coupling is not None + else getattr(fit_result, "mimic_fits", {}) + ), + transfers=dict(transfers or {}), + quality={ + "passed": True, + "training_cycles": TRAINING_CYCLES, + "holdout_cycle": HOLDOUT_CYCLE, + "holdout_by_joint": quality_by_joint, + "holdout_sample_count": len(all_errors), + # Direction-aware curves explicitly compensate repeatable + # mechanical backlash. Hysteresis is therefore diagnostic + # evidence, not a standalone rejection criterion; isolated + # holdout accuracy remains the publication gate. + "fit_diagnostics_by_joint": fit_diagnostics_by_joint, + }, + metadata=dict(metadata or {}), + ) + self.validate_result(result) + return result + + def validate_result(self, result: CalibrationResult) -> None: + if result.profile_id != self.profile.key.profile_id: + raise ValueError("calibration result belongs to another profile") + if result.policy_version != ACQUISITION_POLICY_VERSION: + raise ValueError("calibration result uses an obsolete policy") + if not bool(result.quality.get("passed")): + raise ValueError("calibration result is not publishable") + if tuple(result.quality.get("training_cycles", ())) != TRAINING_CYCLES: + raise ValueError("calibration result must use three training cycles") + if int(result.quality.get("holdout_cycle", -1)) != HOLDOUT_CYCLE: + raise ValueError("calibration result must use cycle four as holdout") + + def _formal_speed(self, task: TaskSpec) -> float: + if self.profile.command.unit == "rad": + assert task.formal_speed is not None + return float(task.formal_speed) + return float( + task.formal_speed_u8 + if task.formal_speed_u8 is not None + else self.profile.motion.speed_parameters.get("formal_u8", 1.0) + ) + + +__all__ = [ + "ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult", + "HOLDOUT_CYCLE", "ScanUnit", "SweepQuality", "TRAINING_CYCLES", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 07db0e5..298395b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -24,6 +24,17 @@ def build_session_controller( return SessionController(registered.profile, evaluator, solver) +def build_registered_runtime( + profile_key: ProfileKey, + *, + registry: ProfileRegistry | None = None, +): + """Assemble the same engine/adapter pair for any registered hand.""" + selected_registry = registry or get_default_registry() + registered = selected_registry.get(profile_key) + return registered.build_engine(), registered.build_sdk_adapter() + + def main(args: list[str] | None = None) -> None: """Select the profile first, then delegate to its reviewed CLI strategy.""" selector = argparse.ArgumentParser(add_help=False) diff --git a/src/linkerhand_calibration/linkerhand_calibration/trajectory.py b/src/linkerhand_calibration/linkerhand_calibration/trajectory.py index 67b7f42..9a32ac8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/trajectory.py +++ b/src/linkerhand_calibration/linkerhand_calibration/trajectory.py @@ -635,6 +635,7 @@ def _fit_joint_curve( *, endpoint_reference: Mapping[str, Sequence[float]] | None = None, preserve_direction_offset: bool = False, + require_observed_domain_endpoints: bool = True, ) -> tuple[dict[str, Any], float, float]: by_direction: dict[str, list[list[float]]] = { direction: [[] for _ in range(256)] for direction in DIRECTIONS @@ -655,10 +656,12 @@ def _fit_joint_curve( ], dtype=int, ) - if ( - commands.size < 3 - or int(commands[0]) != 0 - or int(commands[-1]) != 255 + if commands.size < 3: + raise ValueError( + f"{direction} centre trajectory requires at least 3 commands" + ) + if require_observed_domain_endpoints and ( + int(commands[0]) != 0 or int(commands[-1]) != 255 ): raise ValueError( f"{direction} centre trajectory requires commands 0 and 255" diff --git a/src/linkerhand_calibration/linkerhand_calibration/urdf_comparison.py b/src/linkerhand_calibration/linkerhand_calibration/urdf_comparison.py new file mode 100644 index 0000000..dc3786a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/urdf_comparison.py @@ -0,0 +1,175 @@ +"""Read-only, model-independent URDF pose comparison; never a release gate. + +Coordinates are URDF joint radians, NOT SDK feedback. Link origins are not +fingertip contact points. A matching model is not a physical accuracy proof. +""" + +import argparse +import hashlib +import json +import math +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from .storage import atomic_write_json + + +def _vector(value): + a = np.asarray([float(x) for x in value.split()]) + if a.shape != (3,) or not np.isfinite(a).all(): + raise ValueError("invalid URDF vector") + return a + + +class _Model: + def __init__(self, path): + self.path = Path(path).resolve(strict=True) + root = ET.parse(self.path).getroot() + self.links = {x.get("name"): ET.tostring(x) for x in root.findall("link")} + self.joints = {x.get("name"): x for x in root.findall("joint")} + if len(self.links) != len(root.findall("link")) or len(self.joints) != len(root.findall("joint")): + raise ValueError("duplicate URDF names") + self.parents = {} + for name, j in self.joints.items(): + if j.get("type") not in {"fixed", "revolute", "continuous"}: + raise ValueError(f"unsupported joint type: {name}") + p, c = j.find("parent").get("link"), j.find("child").get("link") + if p not in self.links or c not in self.links or c in self.parents: + raise ValueError(f"invalid topology: {name}") + self.parents[c] = name + roots = set(self.links) - set(self.parents) + if len(roots) != 1: + raise ValueError("URDF must have one root") + self.root = roots.pop() + self.active = {n for n, j in self.joints.items() + if j.get("type") != "fixed" and j.find("mimic") is None} + + def bounds(self, name): + j = self.joints[name] + if j.get("type") == "continuous": + return -math.pi, math.pi + lim = j.find("limit") + values = float(lim.get("lower")), float(lim.get("upper")) + if not all(math.isfinite(v) for v in values) or values[0] > values[1]: + raise ValueError(f"invalid limits: {name}") + return values + + def fk(self, values): + angles, poses = {}, {self.root: np.eye(4)} + + def angle(name, stack=()): + if name in angles: + return angles[name] + if name in stack or name not in self.joints: + raise ValueError("invalid mimic dependency") + j = self.joints[name] + mimic = j.find("mimic") + if j.get("type") == "fixed": + v = 0. + elif mimic is None: + v = float(values.get(name, 0.)) + else: + v = (float(mimic.get("multiplier", "1")) * + angle(mimic.get("joint"), (*stack, name)) + float(mimic.get("offset", "0"))) + if not math.isfinite(v): + raise ValueError("non-finite joint coordinate") + angles[name] = v + return v + + def pose(link, stack=()): + if link in poses: + return poses[link] + if link in stack: + raise ValueError("cyclic URDF topology") + name = self.parents[link] + j = self.joints[name] + origin = j.find("origin") + t = np.eye(4) + if origin is not None: + t[:3, 3] = _vector(origin.get("xyz", "0 0 0")) + t[:3, :3] = Rotation.from_euler("xyz", _vector(origin.get("rpy", "0 0 0"))).as_matrix() + q = angle(name) + if j.get("type") != "fixed": + a = j.find("axis") + axis = _vector("1 0 0" if a is None else a.get("xyz", "1 0 0")) + if np.linalg.norm(axis) < 1.e-12: + raise ValueError("zero joint axis") + t[:3, :3] = t[:3, :3] @ Rotation.from_rotvec(axis / np.linalg.norm(axis) * q).as_matrix() + poses[link] = pose(j.find("parent").get("link"), (*stack, link)) @ t + return poses[link] + + for link in self.links: + pose(link) + return poses + + +def compare_urdfs(reference, candidate): + a, b = _Model(reference), _Model(candidate) + if set(a.links) != set(b.links) or set(a.joints) != set(b.joints) or a.active != b.active: + raise ValueError("models have different links/joints/actuators") + for name in a.joints: + x, y = a.joints[name], b.joints[name] + if x.get("type") != y.get("type") or any(x.find(k).attrib != y.find(k).attrib for k in ("parent", "child")): + raise ValueError("models have different topology") + bounds = {} + for name in sorted(a.active): + al, au = a.bounds(name) + bl, bu = b.bounds(name) + bounds[name] = max(al, bl), min(au, bu) + if bounds[name][0] > bounds[name][1]: + raise ValueError(f"no common joint interval: {name}") + neutral = {n: float(np.clip(0., lo, hi)) for n, (lo, hi) in bounds.items()} + samples = {"neutral": neutral} + if len(bounds) > 1: + samples["combined_middle"] = {n: (lo + hi) / 2 for n, (lo, hi) in bounds.items()} + samples["combined_upper"] = {n: hi for n, (lo, hi) in bounds.items()} + for name, (lo, hi) in bounds.items(): + for label, q in (("lower", lo), ("middle", (lo + hi) / 2), ("upper", hi)): + samples[f"{name}:{label}"] = {**neutral, name: q} + errors = {link: {"maximum_origin_distance_mm": 0., "maximum_orientation_difference_deg": 0.} + for link in sorted(a.links)} + for q in samples.values(): + left, right = a.fk(q), b.fk(q) + for link, item in errors.items(): + distance = float(np.linalg.norm(left[link][:3, 3] - right[link][:3, 3]) * 1000) + rotation = float(np.degrees(Rotation.from_matrix(left[link][:3, :3].T @ right[link][:3, :3]).magnitude())) + item["maximum_origin_distance_mm"] = max(item["maximum_origin_distance_mm"], distance) + item["maximum_orientation_difference_deg"] = max(item["maximum_orientation_difference_deg"], rotation) + return { + "comparison_schema": 1, + "scope": "same_urdf_joint_angles_not_sdk_feedback", + "is_accuracy_certificate": False, + "note": "Link origins are not fingertip contact points; never send these offline probe poses to hardware.", + "reference": str(a.path), "candidate": str(b.path), + "reference_sha256": hashlib.sha256(a.path.read_bytes()).hexdigest(), + "candidate_sha256": hashlib.sha256(b.path.read_bytes()).hexdigest(), + "link_elements_identical": a.links == b.links, + "active_joint_ranges_rad": {n: {"reference": a.bounds(n), "candidate": b.bounds(n), + "comparison_interval": bounds[n]} for n in bounds}, + "pose_count": len(samples), "probe_joint_positions_rad": samples, + "link_frame_differences": errors, + "maximum_origin_distance_mm": max(x["maximum_origin_distance_mm"] for x in errors.values()), + "maximum_orientation_difference_deg": max(x["maximum_orientation_difference_deg"] for x in errors.values()), + } + + +def main(args=None): + parser = argparse.ArgumentParser(description="离线 URDF 姿态对比,不驱动硬件、不改变标定结果") + parser.add_argument("--reference", required=True) + parser.add_argument("--candidate", required=True) + parser.add_argument("--output", required=True) + selected = parser.parse_args(args) + target = Path(selected.output).resolve() + if target.exists(): + parser.error("output must be a new file") + result = compare_urdfs(selected.reference, selected.candidate) + atomic_write_json(target, result) + print(f"模型对比完成(非精度PASS):最大连杆原点差 {result['maximum_origin_distance_mm']:.6f} mm;" + f"最大方向差 {result['maximum_orientation_difference_deg']:.6f}°;报告 {target}") + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/setup.py b/src/linkerhand_calibration/setup.py index 858656b..c1a9be1 100644 --- a/src/linkerhand_calibration/setup.py +++ b/src/linkerhand_calibration/setup.py @@ -17,7 +17,12 @@ def package_data_tree(root: str) -> list[tuple[str, list[str]]]: result = [] for directory in directories: files = sorted( - str(path) for path in directory.iterdir() if path.is_file() + str(path) + for path in directory.iterdir() + if path.is_file() + # Generated calibration products belong under calibration_output, + # never in the installed immutable CAD input bundle. + and "calibrated" not in path.stem.lower() ) if files: result.append( @@ -51,6 +56,7 @@ setup( license="MIT", entry_points={ "console_scripts": [ + "compare_calibration_urdfs = linkerhand_calibration.urdf_comparison:main", ( "hikrobot_camera_node = " "linkerhand_calibration.hikrobot_camera:main" diff --git a/src/linkerhand_calibration/test/test_config.py b/src/linkerhand_calibration/test/test_config.py index cce07b2..c1f4572 100644 --- a/src/linkerhand_calibration/test/test_config.py +++ b/src/linkerhand_calibration/test/test_config.py @@ -203,15 +203,15 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0 assert parameters["minimum_sweep_bins"] >= 32 assert parameters["maximum_bin_gap"] <= 16 - assert parameters["automatic_sweep_retry_limit"] == 2 - assert parameters["automatic_fit_retry_limit"] == 2 + assert parameters["automatic_sweep_retry_limit"] == 1 + assert parameters["automatic_fit_retry_limit"] == 0 assert parameters["motor_stall_timeout_seconds"] == 2.0 assert parameters["motor_stall_startup_grace_seconds"] == 1.0 assert parameters["motor_stall_minimum_progress_u8"] == 1.0 - assert parameters["automatic_motion_retry_limit"] == 2 + assert parameters["automatic_motion_retry_limit"] == 0 assert parameters["provisional_warning_ratio"] == 1.25 - assert parameters["retry_speed_scales"] == [0.8, 0.6] - assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0] + assert parameters["retry_speed_scales"] == [1.0] + assert parameters["retry_endpoint_hold_seconds"] == [0.5] assert parameters["position_timeout_seconds"] >= 20.0 assert parameters["maximum_state_image_skew_ms"] <= 50.0 assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0 diff --git a/src/linkerhand_calibration/test/test_g20_right_product.py b/src/linkerhand_calibration/test/test_g20_right_product.py index 64d5e2e..e3d130e 100644 --- a/src/linkerhand_calibration/test/test_g20_right_product.py +++ b/src/linkerhand_calibration/test/test_g20_right_product.py @@ -77,6 +77,7 @@ from linkerhand_calibration.urdf_zero import ( get_zero_calibration_profile, write_zero_corrected_urdf, ) +from linkerhand_calibration.runtime import ACQUISITION_POLICY_VERSION REPO = Path(__file__).resolve().parents[3] @@ -963,6 +964,7 @@ def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> N json.dumps( { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, "source_urdf_sha256": config.source_urdf_sha256, @@ -1180,6 +1182,7 @@ def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary( json.dumps( { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, "source_urdf_sha256": config.source_urdf_sha256, @@ -1205,6 +1208,7 @@ def test_automatic_resume_skips_newer_attempt_without_start_checkpoint( json.dumps( { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, "source_urdf_sha256": config.source_urdf_sha256, @@ -1251,6 +1255,7 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw( rows = [ { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, "view_tags": { @@ -1344,6 +1349,7 @@ def test_full_resume_discards_all_old_tasks_after_start_position_change( rows = [ { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "model": "G20", "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, @@ -1448,6 +1454,7 @@ def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks( rows = [ { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "model": "G20", "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, @@ -2725,6 +2732,7 @@ def test_node_drops_imported_task_failing_hard_gates( rows = [ { "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, "hand_type": "right", "tag_layout": G20_RIGHT_19_LAYOUT, "view_tags": { diff --git a/src/linkerhand_calibration/test/test_l6_right_profile.py b/src/linkerhand_calibration/test/test_l6_right_profile.py index 067c143..feb3ecb 100644 --- a/src/linkerhand_calibration/test/test_l6_right_profile.py +++ b/src/linkerhand_calibration/test/test_l6_right_profile.py @@ -1,6 +1,7 @@ from __future__ import annotations from pathlib import Path +import json import re from types import SimpleNamespace import xml.etree.ElementTree as ET @@ -432,11 +433,13 @@ def test_l6_quality_gates_each_tag_separately_from_joined_frames( L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) -def test_l6_quality_failure_names_the_specific_tag_id(tmp_path: Path) -> None: +def test_l6_low_tag_rate_is_diagnostic_when_samples_are_observable(tmp_path: Path) -> None: fake, step = _l6_sweep_quality_fake(tmp_path) fake.step_tag_quality_frames["pinky_dip"] = 190 - with pytest.raises(ValueError, match=r"tag_rate\[ID5/pinky_dip\]=0\.900"): - L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) + L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) + quality = json.loads(fake.raw_path.read_text().splitlines()[-1]) + assert quality["failures"] == [] + assert any(value.startswith("tag_rate=") for value in quality["warnings"]) def test_l6_operator_progress_shows_exact_failed_sweep_metric() -> None: diff --git a/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py b/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py new file mode 100644 index 0000000..b9dc674 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py @@ -0,0 +1,60 @@ +"""Observable line residuals must not be confused with axial PnP drift.""" + +import inspect +import math +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.models.g20.zero_solver import ( + _fit_axis_point_from_pose_trajectory, fit_joint_axis_measurement, +) + + +def axis_fit(*, axial_drift=0., transverse_drift=0., separate=False): + axis = np.array([0., 0., 1.]) + point = np.array([.012, -.017, 0.]) + reference = np.array([.047, .009, .02]) + rows = [] + for index, angle in enumerate(np.linspace(0., 1.2, 128)): + rotation = Rotation.from_rotvec(axis * angle) + translation = point + rotation.apply(reference - point) + translation += axis * axial_drift * math.sin(angle * 3.) + translation[0] += transverse_drift * math.sin(angle * 11.) + rows.append({ + 'command_u8': 255 - 2 * index, 'direction': 'decreasing', + 'relative_translation_xyz_m': translation.tolist(), + 'relative_quaternion_xyzw': rotation.as_quat().tolist(), + }) + diagnostics = {} + fitted, rms, _ = _fit_axis_point_from_pose_trajectory( + rows, zero_command_u8=255, axis_parent_xyz=axis, + angle_axis_parent_xyz=axis, phase_reference_point_parent_xyz=point, + view_normal_common_xyz=None, canonical_zero_direction='decreasing', + allow_axial_translation=separate, residual_diagnostics=diagnostics, + ) + return fitted, rms, diagnostics + + +def test_axial_component_is_reported_but_not_used_as_transverse_fit_error(): + baseline, baseline_rms, _ = axis_fit(separate=True) + observed, rms, diagnostic = axis_fit(axial_drift=.015, separate=True) + assert observed == pytest.approx(baseline, abs=1.e-9) + assert rms == pytest.approx(baseline_rms, abs=1.e-9) + assert diagnostic['axial_rms_m'] > .005 + assert diagnostic['raw_rms_m'] > rms + assert diagnostic['transverse_rms_m'] == pytest.approx(rms, abs=1.e-9) + + +def test_transverse_model_error_is_not_discarded_by_projection(): + _, rms, diagnostic = axis_fit(transverse_drift=.01, separate=True) + assert rms > .0015 + assert diagnostic['transverse_rms_m'] > .0015 + + +def test_legacy_default_does_not_silently_enable_projection(): + assert inspect.signature(fit_joint_axis_measurement).parameters[ + 'separate_axial_residual'].default is False + _, rms, diagnostic = axis_fit(axial_drift=.015) + assert rms > .0015 + assert diagnostic['raw_rms_m'] == pytest.approx(rms) diff --git a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py new file mode 100644 index 0000000..82a9fbe --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py @@ -0,0 +1,342 @@ +"""Non-zero recovery through G20 geometry, not zero-only CAD fixtures.""" + +from dataclasses import replace +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.models.g20.profile import JointCurveFit +from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.models.o12.profile import ( + CALIBRATED_ACTIVE_JOINTS, + GEOMETRIC_ZERO_JOINTS, + STATIC_ZERO_EXCLUDED_JOINTS, + build_typed_profile, +) +from linkerhand_calibration.models.o12.zero import ( + AXIS_JOINTS, PHASE_PARENT, ZERO_OBSERVER, O12SpatialZeroError, + full_hand_zero_profile, motor_index, solve_full_hand_zero, +) +from linkerhand_calibration.models.o12.kinematics import PASSIVE_SDK_SOURCE_BY_JOINT +from test_o12_right_profile import SOURCE_URDF, _synthetic_records + + +def pose(t): + return {"translation_xyz_m": t[:3, 3].tolist(), + "quaternion_xyzw": Rotation.from_matrix(t[:3, :3]).as_quat().tolist()} + + +def transform(xyz, rpy): + t = np.eye(4) + t[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + t[:3, 3] = xyz + return t + + +def observations(truth, *, coupled_passive=False): + model = UrdfKinematicModel(SOURCE_URDF) + common = transform([.03, -.02, .6], [.15, -.2, .3]) + angle = tuple(np.linspace(.7, 0., 256)) + curve = JointCurveFit(angle, angle, angle, {}, 0., 0., {}) + rows = {} + for i, name in enumerate(AXIS_JOINTS): + mount = transform([.013, -.011, .024], [.21, -.1 * i, .3]) + # Passive trajectories live in the parent Tag frame. A constant + # arbitrary mount must not be mistaken for an encoder zero. + parent = common @ transform([-.01, .015, -.02], [-.2, .11, -.15]) + rows[name] = [] + for cycle in range(4): + for direction, indices in (("decreasing", range(255, -1, -4)), + ("increasing", range(3, 256, 4))): + for index in indices: + state = [255.] * 12 + # A passive axis is independently rotated here to test + # the spatial kernel, not the model's SDK adapter. + if name in CALIBRATED_ACTIVE_JOINTS: + state[motor_index(name)] = index + joint_angles = {name: angle[index]} + sample_parent = parent + if coupled_passive and name in PASSIVE_SDK_SOURCE_BY_JOINT: + state[motor_index(name)] = index + joint_angles[PASSIVE_SDK_SOURCE_BY_JOINT[name]] = angle[index] + sample_parent = common @ model.link_transform( + PHASE_PARENT[name], zero_offsets=truth, joint_angles=joint_angles, + independent_mimic_angles=True, + ) @ transform([.01, .004, .014], [.2, -.1, .3]) + child = common @ model.link_transform( + name, zero_offsets=truth, joint_angles=joint_angles, + independent_mimic_angles=True, + ) @ mount + relative = np.linalg.inv(sample_parent) @ child + rows[name].append({ + "cycle": cycle, "direction": direction, "command_u8": index, + "state_u8": state, + "relative_quaternion_xyzw": pose(relative)["quaternion_xyzw"], + "relative_translation_xyz_m": relative[:3, 3].tolist(), + "parent_pose_common": pose(sample_parent), "child_pose_common": pose(child), + "view_normal_common_xyz": common[:3, 0].tolist(), + "camera_center_common_xyz_m": [0., 0., 0.], + }) + return rows, {name: curve for name in AXIS_JOINTS} + + +def test_profile_authorizes_every_observed_active_zero_without_endpoint_assumptions(): + zero = build_typed_profile().zero + assert set(zero.direct_zero_joints) == GEOMETRIC_ZERO_JOINTS == set(ZERO_OBSERVER) + assert CALIBRATED_ACTIVE_JOINTS - GEOMETRIC_ZERO_JOINTS == STATIC_ZERO_EXCLUDED_JOINTS + assert STATIC_ZERO_EXCLUDED_JOINTS == {"thumb_mcp"} + assert not zero.mechanical_endpoint_joints + assert zero.cad_frozen_joints & CALIBRATED_ACTIVE_JOINTS == STATIC_ZERO_EXCLUDED_JOINTS + from linkerhand_calibration.models.g20.zero_solver import get_right_19_thumb_zero_profile + assert not get_right_19_thumb_zero_profile().accept_validated_zero_in_confidence_interval + assert full_hand_zero_profile().accept_validated_zero_in_confidence_interval + assert not get_right_19_thumb_zero_profile().project_axis_gauge_before_image + assert full_hand_zero_profile().project_axis_gauge_before_image + assert full_hand_zero_profile().hand.stable_cross_view_cone_bias + model = UrdfKinematicModel(SOURCE_URDF) + for child, parent in PHASE_PARENT.items(): + a, _ = model.axis_line(child, zero_offsets={}, joint_angles={}) + b, _ = model.axis_line(parent, zero_offsets={}, joint_angles={}) + assert abs(float(a @ b)) == pytest.approx(1., abs=1.e-8), (parent, child) + + +@pytest.fixture(scope="module") +def full_solution(): + truth = {name: (.04 if i % 2 else -.06) for i, name in enumerate(ZERO_OBSERVER)} + rows, curves = observations(truth) + result = solve_full_hand_zero(SOURCE_URDF, rows, curves) + return truth, result + + +def test_all_ten_observable_nonzero_offsets_recovered_with_arbitrary_mounts(full_solution): + truth, result = full_solution + assert result.passed + assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) + assert len(result.axis_residual_diagnostics) == 4 * len(AXIS_JOINTS) + assert all(item["axial_component_separated"] + for item in result.axis_residual_diagnostics.values()) + + +def test_coupled_passive_parent_motion_is_not_mistaken_for_a_static_zero(): + truth = {name: .04 for name in ZERO_OBSERVER} + rows, curves = observations(truth, coupled_passive=True) + result = solve_full_hand_zero(SOURCE_URDF, rows, curves) + assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) + + +@pytest.mark.parametrize("move_base,remount", [(False, False), (True, False), + (False, True), (True, True)]) +def test_end_on_phase_is_invariant_to_pre_capture_base_and_tag_changes(move_base, remount): + # Previously the synthetic camera faced CAD X, perpendicular to the + # finger Y axes; that never exercised the real side-camera image branch. + truth = {name: .04 for name in ZERO_OBSERVER} + rows, curves = observations(truth, coupled_passive=True) + common = transform([.03, -.02, .6], [.15, -.2, .3]) + normal = common[:3, :3] @ np.array([.1, .99, .1]) + normal /= np.linalg.norm(normal) + body = transform([.03, -.02, .01], [.07, -.08, .05]) if move_base else np.eye(4) + parent_mount = transform([-.005, .004, .012], [-.1, .05, -.11]) if remount else np.eye(4) + child_mount = transform([.008, -.005, .011], [.1, -.12, .2]) if remount else np.eye(4) + def matrix(payload): + t = np.eye(4) + t[:3, :3] = Rotation.from_quat(payload["quaternion_xyzw"]).as_matrix() + t[:3, 3] = payload["translation_xyz_m"] + return t + for samples in rows.values(): + for row in samples: + parent = body @ matrix(row["parent_pose_common"]) @ parent_mount + child = body @ matrix(row["child_pose_common"]) @ child_mount + relative = np.linalg.inv(parent) @ child + row.update(parent_pose_common=pose(parent), child_pose_common=pose(child), + relative_translation_xyz_m=relative[:3, 3].tolist(), + relative_quaternion_xyzw=Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(), + view_normal_common_xyz=normal.tolist()) + result = solve_full_hand_zero(SOURCE_URDF, rows, curves) + assert result.passed + assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) + + +def test_active_parallel_axis_direction_bias_does_not_become_a_zero(): + truth = {name: .04 for name in ZERO_OBSERVER} + rows, curves = observations(truth, coupled_passive=True) + common = transform([.03, -.02, .6], [.15, -.2, .3]) + normal = common[:3, :3] @ np.array([.1, .99, .1]) + normal /= np.linalg.norm(normal) + for samples in rows.values(): + for row in samples: + row["view_normal_common_xyz"] = normal.tolist() + bias = Rotation.from_euler("xyz", [.06, -.04, .07]) + for name in ("index_pip", "middle_pip"): + ref = Rotation.from_quat(rows[name][0]["relative_quaternion_xyzw"]) + for row in rows[name]: + observed = Rotation.from_quat(row["relative_quaternion_xyzw"]) + delta = observed * ref.inv() + changed = bias * delta * bias.inv() * ref + row["relative_quaternion_xyzw"] = changed.as_quat().tolist() + parent = Rotation.from_quat(row["parent_pose_common"]["quaternion_xyzw"]) + row["child_pose_common"]["quaternion_xyzw"] = (parent * changed).as_quat().tolist() + result = solve_full_hand_zero(SOURCE_URDF, rows, curves) + assert result.passed + assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4) + + +def test_rotation_only_data_cannot_be_published_as_full_spatial_calibration(): + from linkerhand_calibration.models.o12.fitting import fit_o12_session + with pytest.raises(O12SpatialZeroError, match="requires pose observations"): + fit_o12_session(SOURCE_URDF, _synthetic_records(), require_full_hand_spatial_zero=True) + + +def test_small_uncertain_zero_is_kept_only_when_frozen_zero_passes_holdout(): + truth = {name: .04 for name in ZERO_OBSERVER} + rows, curves = observations({**truth, "index_mcp_roll": .003}) + for cycle, value in enumerate((.001, .005, .003, 0.)): + changed, _ = observations({**truth, "index_mcp_roll": value}) + for name in rows: + rows[name] = [r for r in rows[name] if r["cycle"] != cycle] + [ + r for r in changed[name] if r["cycle"] == cycle] + result = solve_full_hand_zero(SOURCE_URDF, rows, curves) + assert result.passed + assert result.direct_offsets_rad["index_mcp_roll"] == 0. + assert result.validation_error_by_joint_rad["index_mcp_pitch"] < 1.e-5 + changed, _ = observations({**truth, "index_mcp_roll": .15}) + for name in rows: + rows[name] = [r for r in rows[name] if r["cycle"] != 3] + [ + r for r in changed[name] if r["cycle"] == 3] + with pytest.raises(O12SpatialZeroError): + solve_full_hand_zero(SOURCE_URDF, rows, curves) + + +def test_independent_fourth_cycle_cannot_define_or_hide_a_bad_zero(): + truth = {name: .04 for name in ZERO_OBSERVER} + rows, curves = observations(truth) + moved, _ = observations({**truth, "index_pip": .18}) + for name in rows: + rows[name] = [r for r in rows[name] if r["cycle"] < 3] + [r for r in moved[name] if r["cycle"] == 3] + with pytest.raises(O12SpatialZeroError, match="spatial zero solve failed") as error: + solve_full_hand_zero(SOURCE_URDF, rows, curves) + assert error.value.diagnostics["passed"] is False + + +def test_full_zero_writeback_and_ring_transfer_preserve_each_cad_frame(full_solution, tmp_path): + from linkerhand_calibration.models.o12.fitting import fit_o12_session + from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf + from linkerhand_calibration.models.o12.artifacts import ( + build_o12_runtime_payload, validate_o12_runtime_payload_against_urdf, + ) + truth, solved = full_solution + offsets = { + **truth, + "thumb_mcp": 0.0, + "ring_mcp_pitch": truth["pinky_mcp_pitch"], + } + fit = replace(fit_o12_session(SOURCE_URDF, _synthetic_records()), + zero_offsets_rad=offsets, full_hand_zero_result=solved, + zero_method_by_joint={ + n: ( + "source_cad_zero_profile_excluded" + if n == "thumb_mcp" + else "urdf_serial_axis_geometry" + ) + for n in offsets + }) + written = write_o12_corrected_urdf(source_urdf=SOURCE_URDF, + output_directory=tmp_path, serial_number="FULL", result=fit) + before, after = UrdfKinematicModel(SOURCE_URDF), UrdfKinematicModel(written.path) + for q in ({}, {n: .3 for n in offsets}): + for name in offsets: + assert after.link_transform(name, zero_offsets={}, joint_angles=q) == pytest.approx( + before.link_transform(name, zero_offsets=offsets, joint_angles=q), abs=1.e-8) + hashes = {k: "a" * 64 for k in build_typed_profile().artifacts.protected_input_fields} + payload = build_o12_runtime_payload(serial_number="FULL", source_urdf=SOURCE_URDF, + result=fit, protected_inputs=hashes, passed=True) + validate_o12_runtime_payload_against_urdf(payload, written.path, source_urdf=SOURCE_URDF) + assert payload["calibration_scope"] == "full_dynamic_except_thumb_mcp_static_zero" + assert payload["quality"]["cad_static_joints_not_measured"] == ["thumb_mcp"] + assert set(payload["quality"]["static_zero_exclusions"]) == {"thumb_mcp"} + assert payload["joints"]["thumb_mcp"]["calibration_status"] == "measured_dynamic_cad_static" + assert payload["joints"]["thumb_mcp"]["static_urdf_origin_offset_rad"] == 0.0 + assert payload["quality"]["full_hand_spatial_zero"]["passed"] + invalid = replace( + fit, + zero_offsets_rad={**fit.zero_offsets_rad, "thumb_mcp": 0.01}, + ) + with pytest.raises(ValueError, match="retain immutable source-CAD"): + write_o12_corrected_urdf( + source_urdf=SOURCE_URDF, + output_directory=tmp_path / "invalid", + serial_number="INVALID", + result=invalid, + ) + with pytest.raises(ValueError, match="retain immutable source-CAD"): + build_o12_runtime_payload( + serial_number="INVALID", + source_urdf=SOURCE_URDF, + result=invalid, + protected_inputs=hashes, + passed=True, + ) + + +def test_failed_spatial_solve_writes_evidence_without_replacing_published_result(monkeypatch, tmp_path): + from linkerhand_calibration.models.o12 import pipeline + import json + previous = tmp_path / "previous" + previous.mkdir() + pointer = tmp_path / "latest_passed" + pointer.symlink_to(previous, target_is_directory=True) + diagnostic = {"passed": False, "stage": "axis_observation", "joint": "thumb_mcp"} + def fail(*args, **kwargs): + assert kwargs["require_full_hand_spatial_zero"] + raise O12SpatialZeroError("unreliable geometry", diagnostic) + monkeypatch.setattr(pipeline, "fit_o12_session", fail) + session = tmp_path / "new" + with pytest.raises(O12SpatialZeroError): + pipeline.finalize_o12_session(session_dir=session, serial_number="FULL", + source_urdf=SOURCE_URDF, protected_inputs={}, records=[], publish=True) + assert pointer.resolve() == previous + assert not list(session.glob("*.urdf")) + assert json.loads((session / "spatial_zero_diagnostics.json").read_text()) == diagnostic + + +def test_identifiable_failed_fit_exports_only_review_model(monkeypatch, tmp_path, full_solution): + import json + from dataclasses import asdict + from linkerhand_calibration.models.o12 import pipeline + from linkerhand_calibration.models.o12.fitting import fit_o12_session + from linkerhand_calibration.models.o12.artifacts import build_o12_runtime_payload + truth, valid_zero = full_solution + invalid_zero = replace(valid_zero, passed=False, + failure_reasons={"index_pip": "zero_phase_axis_line_residual_too_large"}) + fit = fit_o12_session(SOURCE_URDF, _synthetic_records()) + offsets = { + **truth, + "thumb_mcp": 0.0, + "ring_mcp_pitch": truth["pinky_mcp_pitch"], + } + fit = replace(fit, zero_offsets_rad=offsets, full_hand_zero_result=invalid_zero) + error = O12SpatialZeroError("unreliable geometry", { + "passed": False, "stage": "spatial_solve", "result": asdict(invalid_zero)}) + error.review_fit = fit + def fail(*args, **kwargs): + raise error + monkeypatch.setattr(pipeline, "fit_o12_session", fail) + previous = tmp_path / "previous" + previous.mkdir() + pointer = tmp_path / "latest_passed" + pointer.symlink_to(previous, target_is_directory=True) + session = tmp_path / "new" + with pytest.raises(O12SpatialZeroError, match="仅供复核"): + pipeline.finalize_o12_session(session_dir=session, serial_number="FULL", + source_urdf=SOURCE_URDF, protected_inputs={}, records=[], publish=True, + timestamp="20260909_000000") + assert pointer.resolve() == previous + assert not list(session.glob("*.urdf")) + assert not list(session.rglob("*calibration.json")) + manifest = json.loads((session / "review_only/review_manifest.json").read_text()) + assert not manifest["publication_allowed"] + assert not manifest["spatial_validation"]["result"]["passed"] + assert "REVIEW_ONLY" in manifest["urdf"] + assert (session / "review_only" / manifest["urdf"]).is_file() + with pytest.raises(ValueError): + build_o12_runtime_payload(serial_number="FULL", source_urdf=SOURCE_URDF, + result=fit, protected_inputs={}, passed=True) diff --git a/src/linkerhand_calibration/test/test_o12_observation_resolution.py b/src/linkerhand_calibration/test/test_o12_observation_resolution.py new file mode 100644 index 0000000..8ae39d9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_observation_resolution.py @@ -0,0 +1,117 @@ +import copy +import numpy as np +import pytest +from scipy.spatial.transform import Rotation as R + +from linkerhand_calibration.models.o12 import observations as module +from linkerhand_calibration.pnp import SquareTagPose + + +def build_data(monkeypatch, *, wrong_holdout=False): + """Ambiguous initial poses: the wrong distal branch has lower image error.""" + rows=[]; pose_lookup={}; idx=0 + camera=R.from_euler('xyz',[.2,-.3,.4]) + mounts=[R.from_euler('xyz',angles) for angles in ([.1,.3,.2],[-.3,.5,-.4],[.2,.6,-.7])] + def payload(rotation,t,error): + return dict(quaternion_xyzw=(camera*rotation).as_quat().tolist(), + translation_xyz_m=(camera.apply(t)+[0,0,1]).tolist(),reprojection_error_px=error) + for cycle in range(4): + for theta in np.linspace(0,.9,24): + idx+=1 + driver=R.from_euler('z',theta) + good=[payload(mounts[0],[0,0,0],.05), + payload(driver*mounts[1],driver.apply([.03,0,0]),.05), + payload(R.from_euler('z',2*theta)*mounts[2],driver.apply([.04,0,0])+R.from_euler('z',2*theta).apply([.02,0,0]),.08)] + bad=payload(R.from_euler('x',2*theta)*R.from_euler('y',.8)*mounts[2],[0,0,0],.01) + bad['translation_xyz_m']=good[2]['translation_xyz_m'] + # Holdout is allowed to disagree; it may never choose the training branch. + if wrong_holdout and cycle==3: + good[2]['quaternion_xyzw']=bad['quaternion_xyzw'] + evidence={'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','cycle':cycle,'direction':'decreasing','attempt':1, + 'image_stamp_ns':idx,'camera_matrix':np.eye(3).tolist(), + 'camera_matrix_source':module.MATRIX_SOURCE,'tag_size_m':.016,'roles':{}} + for j,role in enumerate(module.THUMB_ROLES): + marker=idx*10+j + candidates=[good[j],good[j]] if j<2 else [bad,good[j]] + pose_lookup[marker]=[SquareTagPose(tuple(p['quaternion_xyzw']),tuple(p['translation_xyz_m']),p['reprojection_error_px']) for p in candidates] + evidence['roles'][role]={'corners_xy':[[marker,0]]*4,'selected':candidates[0], + 'maximum_reprojection_error_px':1.5} + rows.append(evidence) + for j,name in enumerate(('thumb_mcp','thumb_dip')): + pa=evidence['roles'][module.THUMB_ROLES[j]]['selected'] + ch=evidence['roles'][module.THUMB_ROLES[j+1]]['selected'] + q,t=module._relative(pa,ch) + rows.append({'kind':'o12_joint_sample','task_name':'thumb_mcp_dip_front', + 'joint':name,'cycle':cycle,'direction':'decreasing','attempt':1, + 'image_stamp_ns':idx,'parent_pose_common':pa,'child_pose_common':ch, + 'relative_quaternion_xyzw':q.tolist(),'relative_translation_xyz_m':t.tolist()}) + monkeypatch.setattr(module,'solve_square_tag_ippe',lambda xy,**kw:pose_lookup[xy[0][0]]) + return rows + + +def test_complete_training_can_reconsider_wrong_initial_branch(monkeypatch): + rows=build_data(monkeypatch);original=copy.deepcopy(rows) + resolved,report=module.resolve_thumb_observations(rows) + assert rows==original + assert report['status']=='resolved' and report['selected_branches'][2]==1 + assert report['training_frames']==72 and report['holdout_frames']==24 + assert report['changed_joint_rows']>0 + assert resolved!=rows + + +def test_holdout_cannot_change_training_branch_or_scores(monkeypatch): + _,a=module.resolve_thumb_observations(build_data(monkeypatch)) + _,b=module.resolve_thumb_observations(build_data(monkeypatch,wrong_holdout=True)) + assert a['selected_branches']==b['selected_branches'] + assert a['hypotheses']==b['hypotheses'] + + +def test_legacy_projection_is_not_silently_relabelled(monkeypatch): + rows=build_data(monkeypatch) + for r in rows:r.pop('camera_matrix_source',None) + result,report=module.resolve_thumb_observations(rows) + assert report['status']=='legacy_projection_unverified' and result==rows + assert module.resolve_thumb_observations([])[1]['status']=='legacy_no_corners' + + +def test_missing_evidence_cannot_be_silently_dropped(monkeypatch): + rows=build_data(monkeypatch) + rows=[r for r in rows if not(r['kind']=='o12_pnp_candidate_frame' and r['image_stamp_ns']==20)] + with pytest.raises(ValueError,match='lacks evidence'):module.resolve_thumb_observations(rows) + + +def test_other_task_evidence_is_not_mixed_into_thumb(monkeypatch): + rows=build_data(monkeypatch) + rows.append({'kind':'o12_pnp_candidate_frame','task_name':'pinky_chain_side','image_stamp_ns':1}) + assert module.resolve_thumb_observations(rows)[1]['status']=='resolved' + + +def test_partial_external_projection_cannot_finalize_whole_hand(tmp_path): + from linkerhand_calibration.models.o12.pipeline import finalize_o12_session + with pytest.raises(ValueError,match='diagnostic-only'): + finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf='unused', + protected_inputs={},records=[{'projection_reprocessing_scope':'thumb_only_external_projection'}],publish=False) + + +def test_known_unverified_projection_cannot_publish_even_if_fit_passes(monkeypatch,tmp_path): + from dataclasses import dataclass,field + from linkerhand_calibration.models.o12 import pipeline + from linkerhand_calibration.models.o12.zero import O12SpatialZeroError + @dataclass + class Spatial: + passed: bool=True + failure_reasons: dict=field(default_factory=dict) + @dataclass + class Fit: + full_hand_zero_result: Spatial=field(default_factory=Spatial) + zero_offsets_rad: dict=field(default_factory=dict) + monkeypatch.setattr(pipeline,'fit_o12_session',lambda *a,**k:Fit()) + def no_review(**kwargs): + raise RuntimeError('test omits review geometry') + monkeypatch.setattr(pipeline,'write_o12_corrected_urdf',no_review) + with pytest.raises(O12SpatialZeroError) as error: + 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 error.value.diagnostics['stage']=='camera_projection' + assert not error.value.review_fit.full_hand_zero_result.passed + assert not (tmp_path/'latest_passed').exists() diff --git a/src/linkerhand_calibration/test/test_o12_recorded_replay.py b/src/linkerhand_calibration/test/test_o12_recorded_replay.py new file mode 100644 index 0000000..e12cdf4 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_recorded_replay.py @@ -0,0 +1,49 @@ +"""Optional local-data replay: deterministic recovery, NOT physical certification. + +Set O12_REPLAY_RAW and O12_REPLAY_REFERENCE to use a captured session and an +independently reviewed model. The test never publishes or launches hardware. +""" + +import os +from pathlib import Path + +import pytest + +from linkerhand_calibration.models.o12.pipeline import finalize_o12_session, load_o12_raw_samples +from linkerhand_calibration.models.o12.zero import O12SpatialZeroError +from linkerhand_calibration.product import sha256_file +from linkerhand_calibration.storage import atomic_write_json +from linkerhand_calibration.urdf_comparison import compare_urdfs +from test_o12_right_profile import SOURCE_URDF + + +@pytest.mark.skipif(not os.environ.get("O12_REPLAY_RAW") or not os.environ.get("O12_REPLAY_REFERENCE"), + reason="local raw recording/reference not supplied") +def test_recorded_replay_matches_reviewed_geometry_without_overriding_quality(tmp_path): + raw = Path(os.environ["O12_REPLAY_RAW"]).resolve(strict=True) + reference = Path(os.environ["O12_REPLAY_REFERENCE"]).resolve(strict=True) + output = Path(os.environ.get("O12_REPLAY_OUTPUT", str(tmp_path / "replay"))).resolve() + assert not output.exists(), "use a new output directory" + hashes = {str(p): sha256_file(p) for p in (raw, reference, SOURCE_URDF)} + try: + _, fit, correction = finalize_o12_session( + session_dir=output, serial_number="O12_RIGHT_001", source_urdf=SOURCE_URDF, + protected_inputs=hashes, records=load_o12_raw_samples(raw), publish=False, + ) + candidate = correction.path + assert fit.full_hand_zero_result.passed + except O12SpatialZeroError as error: + assert error.review_fit is not None + assert not error.review_fit.full_hand_zero_result.passed + assert error.review_fit.full_hand_zero_result.failure_reasons + candidate = Path(error.diagnostics["review_urdf"]) + assert "REVIEW_ONLY" in candidate.name + assert not list(output.glob("*calibration.json")) + report = compare_urdfs(reference, candidate) + atomic_write_json(output / "reference_geometry_comparison.json", report) + assert report["maximum_origin_distance_mm"] < 1.e-5 + assert report["maximum_orientation_difference_deg"] < 1.e-5 + assert report["link_elements_identical"] + for ranges in report["active_joint_ranges_rad"].values(): + assert ranges["reference"] == pytest.approx(ranges["candidate"], abs=1.e-10) + assert all(sha256_file(p) == digest for p, digest in hashes.items()) diff --git a/src/linkerhand_calibration/test/test_o12_right_profile.py b/src/linkerhand_calibration/test/test_o12_right_profile.py index 309eec1..f7f0691 100644 --- a/src/linkerhand_calibration/test/test_o12_right_profile.py +++ b/src/linkerhand_calibration/test/test_o12_right_profile.py @@ -1,29 +1,96 @@ from __future__ import annotations +from dataclasses import replace +import json import math from pathlib import Path +import threading from types import SimpleNamespace import xml.etree.ElementTree as ET +from collections import deque import numpy as np import pytest from scipy.spatial.transform import Rotation from linkerhand_calibration.calibrated_joint_state_bridge import CalibratedCommandMapper -from linkerhand_calibration.models.o12.artifacts import build_o12_runtime_payload -from linkerhand_calibration.models.o12.fitting import fit_o12_session -from linkerhand_calibration.models.o12.motion import cosine_position_trajectory_rad +from linkerhand_calibration.models.o12.artifacts import ( + build_o12_runtime_payload, + validate_o12_runtime_payload, + validate_o12_runtime_payload_against_urdf, +) +from linkerhand_calibration.models.o12.fitting import ( + _uniform_curve_holdout_errors, + fit_o12_session, +) +from linkerhand_calibration.models.o12.health import ( + assess_o12_error_report, + decoded_faults, + historical_communication_latch_confirmed, +) +from linkerhand_calibration.models.o12.motion import ( + cosine_position_trajectory_rad, + cosine_ramp_velocity_trajectory_rad, +) +from linkerhand_calibration.models.o12.kinematics import ( + evaluate_vendor_passive_joint, +) +from linkerhand_calibration.models.o12.quality import ( + QUALITY_POLICY_VERSION, + evaluate_o12_observation_quality, +) +from linkerhand_calibration.models.l6.node import ( + L6ThreeCameraCalibrationNode, + MotionStep, +) +from linkerhand_calibration.models.o6.node import O6ThreeCameraCalibrationNode from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode -from linkerhand_calibration.models.o12.runner import render_o12_progress_zh +from linkerhand_calibration.models.o12.node import MotionStep as O12MotionStep +from linkerhand_calibration.pnp import SquareTagPose +from linkerhand_calibration.models.o12.runner import ( + _log_exception_summary, + render_o12_progress_zh, +) +from linkerhand_calibration.models.o12 import runner as o12_runner from linkerhand_calibration.models.l6.runner import _launch_command +from linkerhand_calibration.models.o12.resume import ( + automatic_resume_candidate, + build_resume_checkpoint_from_rows, + ordered_resume_units, +) from linkerhand_calibration.models.o12.profile import ( CALIBRATED_ACTIVE_JOINTS, + CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD, + CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION, COMMAND_NAMES, + EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION, + ENDPOINT_ANCHOR_BY_JOINT, + FEEDBACK_DOMAIN_MARGIN_RAD, + FEEDBACK_LOWER_RAD, + FEEDBACK_UPPER_RAD, + FORMAL_SPEED_CAP_RAD_S, + INITIAL_FEEDBACK_SPAN_FRACTION, + INDEX_CLEARANCE_MCP_RAD, + MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, MEASURED_PASSIVE_JOINTS, + NORMALIZED_SWEEP_BIN_COUNT, + OUTER_CLEARANCE_FRACTION, PARK_FINGER_RAD, + PARK_MIDDLE_MCP_RAD, + PARK_MIDDLE_PIP_RAD, + PARK_PINKY_MCP_RAD, + PARK_RING_MCP_RAD, + ROLL_CROSS_VIEW_BY_TASK, + ROLL_CLEARANCE_MCP_RAD, + SAFE_UPPER_RAD, SDK_TO_URDF_JOINT, + SDK_TO_URDF_SIGN, build_typed_profile, ) +from linkerhand_calibration.runtime import ( + ACQUISITION_POLICY_VERSION, + CalibrationEngine, +) from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf @@ -31,6 +98,14 @@ ROOT = Path(__file__).resolve().parents[1] SOURCE_URDF = ROOT / "urdf/o12_right/linkerhand_o12_t3_right-0703.urdf" +def test_o12_motion_extensions_are_not_part_of_legacy_hand_contracts() -> None: + assert "relative_feedback_delta" not in MotionStep.__dataclass_fields__ + assert "relative_feedback_delta" in O12MotionStep.__dataclass_fields__ + assert not L6ThreeCameraCalibrationNode._uses_isolated_motion_callbacks() + assert not O6ThreeCameraCalibrationNode._uses_isolated_motion_callbacks() + assert O12ThreeCameraCalibrationNode._uses_isolated_motion_callbacks() + + def _synthetic_records(): profile = build_typed_profile() task_by_joint = { @@ -54,6 +129,20 @@ def _synthetic_records(): for name in CALIBRATED_ACTIVE_JOINTS: task = task_by_joint[name] travel[name] = abs(task.end_value - task.start_value) + # SDK coordinates and physical URDF joint travel are not interchangeable + # on O12. Use the reviewed CAD/mechanical travel in this fixture, just as + # the real Tag curves identify it independently of the commanded range. + travel.update({ + "thumb_cmc_roll": 0.720, + "thumb_cmc_yaw": 0.987, + "thumb_cmc_pitch": 0.588, + "thumb_mcp": 1.184, + "index_mcp_pitch": 1.290, + "index_pip": 1.635, + "middle_mcp_pitch": 1.318, + "middle_pip": 1.604, + "pinky_mcp_pitch": 1.472, + }) travel["pinky_pip"] = ratios["pinky_pip"] * travel["pinky_mcp_pitch"] travel["pinky_dip"] = ratios["pinky_dip"] * travel["pinky_pip"] for name in ("thumb_dip", "index_dip", "middle_dip"): @@ -91,9 +180,47 @@ def test_o12_fixed_mapping_tags_and_radian_domain() -> None: ) assert profile.vision.tag_ids == frozenset(range(16)) assert len(profile.motion.tasks) == 11 + assert profile.motion.speed_parameters["command_rate_hz"] == 50.0 + assert NORMALIZED_SWEEP_BIN_COUNT == 64 + assert OUTER_CLEARANCE_FRACTION == pytest.approx(1.0) + assert PARK_RING_MCP_RAD == pytest.approx(SAFE_UPPER_RAD[10]) + assert PARK_PINKY_MCP_RAD == pytest.approx(SAFE_UPPER_RAD[11]) + assert PARK_FINGER_RAD == pytest.approx(PARK_PINKY_MCP_RAD) + assert PARK_MIDDLE_MCP_RAD == pytest.approx(SAFE_UPPER_RAD[8]) + assert PARK_MIDDLE_PIP_RAD == pytest.approx(SAFE_UPPER_RAD[9]) + assert INITIAL_FEEDBACK_SPAN_FRACTION == pytest.approx(0.99) + assert EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION == pytest.approx(0.90) + assert MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX == pytest.approx(2.0) + assert profile.vision.extrinsics_quality_limits[ + "reprojection_rms_px" + ] == pytest.approx(2.0) + assert FORMAL_SPEED_CAP_RAD_S[4] == pytest.approx(0.12) + assert FORMAL_SPEED_CAP_RAD_S[7] == pytest.approx(0.12) + assert ENDPOINT_ANCHOR_BY_JOINT == {} assert [task.formal_speed for task in profile.motion.tasks] == [ 0.03, 0.04, 0.08, 0.04, 0.08, 0.04, 0.08, 0.08, 0.04, 0.08, 0.08 ] + index_mcp_pitch = profile.measurement.measurements["index_mcp_pitch"] + assert ( + index_mcp_pitch.view, + index_mcp_pitch.parent_role, + index_mcp_pitch.child_role, + ) == ("side", "side_base", "index_dip") + side_tag_ids = { + tag.role: tag.tag_id + for view in profile.vision.views + if view.name == "side" + for tag in view.tags + } + assert side_tag_ids["side_base"] == 4 + assert side_tag_ids["index_dip"] == 11 + assert { + key: (value.view, value.parent_role, value.child_role) + for key, value in ROLL_CROSS_VIEW_BY_TASK.items() + } == { + "middle_roll_front": ("side", "side_base", "middle_pip"), + "index_roll_front": ("side", "side_base", "index_pip"), + } def test_o12_cosine_speed_is_bounded() -> None: @@ -107,6 +234,28 @@ def test_o12_cosine_speed_is_bounded() -> None: assert measured <= speed + 1.0e-6 +def test_o12_fast_trajectory_has_bounded_speed_and_zero_endpoint_velocity() -> None: + speed = 0.10 + _, _, old_duration = cosine_position_trajectory_rad( + 0.0, -0.827286065, 0.0, speed + ) + _, _, duration = cosine_ramp_velocity_trajectory_rad( + 0.0, -0.827286065, 0.0, speed, 0.4 + ) + times = np.linspace(0.0, duration, 5001) + samples = np.asarray([ + cosine_ramp_velocity_trajectory_rad( + 0.0, -0.827286065, t, speed, 0.4 + )[0] + for t in times + ]) + velocity = np.diff(samples) / np.diff(times) + assert duration < 0.70 * old_duration + assert np.max(np.abs(velocity)) <= speed + 1.0e-6 + assert abs(velocity[0]) < 1.0e-4 + assert abs(velocity[-1]) < 1.0e-4 + + def test_o12_hcan_launch_does_not_emit_empty_socketcan_argument(tmp_path: Path) -> None: profile = build_typed_profile() config = SimpleNamespace( @@ -134,6 +283,18 @@ def test_o12_hcan_launch_does_not_emit_empty_socketcan_argument(tmp_path: Path) ) assert not any(item.startswith("can_interface:=") for item in command) + resumed = _launch_command( + config, + tmp_path / "new_session", + record_bag=False, + commands_enabled=False, + resume_from=tmp_path / "old_session", + ) + assert ( + f"resume_raw_samples_path:={tmp_path / 'old_session/raw_samples.jsonl'}" + in resumed + ) + def test_o12_vendor_node_is_launched_in_documented_namespace() -> None: launch_text = (ROOT / "launch/three_camera_calibration.launch.py").read_text() @@ -142,6 +303,18 @@ def test_o12_vendor_node_is_launched_in_documented_namespace() -> None: assert 'namespace="o12"' in vendor +def test_o12_vendor_stream_is_not_artificially_throttled() -> None: + sdk_config = ( + ROOT.parent + / "agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node" + / "config/omnihand_pro_2025_node.yaml" + ).read_text() + active_right = sdk_config.split("right_hand:", 1)[1].split( + "# Alternative", 1 + )[0] + assert "request_interval_ms: 0" in active_right + + def test_o12_temperature_fallback_requires_verified_error_channel(tmp_path: Path) -> None: warnings: list[str] = [] fake = SimpleNamespace( @@ -174,6 +347,118 @@ def test_o12_temperature_fallback_requires_verified_error_channel(tmp_path: Path assert '"fallback_protection":"joint_error_states_bit1_overheat"' in raw +def test_o12_error_health_separates_active_faults_from_historical_comm_latch( +) -> None: + communication = assess_o12_error_report([0, 16, *([0] * 10)]) + assert communication.communication_only + assert communication.communication_channels == (1,) + assert not communication.active_fault_channels + assert historical_communication_latch_confirmed( + assessment=communication, + matching_report_count=3, + observation_seconds=2.0, + feedback_hz=49.0, + feedback_age_seconds=0.01, + minimum_feedback_hz=15.0, + ) + assert not historical_communication_latch_confirmed( + assessment=communication, + matching_report_count=3, + observation_seconds=2.0, + feedback_hz=49.0, + feedback_age_seconds=1.1, + minimum_feedback_hz=15.0, + ) + + active = assess_o12_error_report([0, 16 | 2, *([0] * 10)]) + assert not active.communication_only + assert active.active_fault_channels == (1,) + assert decoded_faults(active.codes)[0]["flags"] == ( + "overheat", "commu_except" + ) + + +def test_o12_historical_comm_latch_requires_repeated_live_evidence( + tmp_path: Path, +) -> None: + pauses: list[str] = [] + warnings: list[str] = [] + fake = SimpleNamespace( + latest_errors=(), + error_verified=False, + error_health_classification="awaiting_error_report", + error_report_count=0, + error_report_first_matching_at=0.0, + error_report_matching_count=0, + error_report_matching_codes=(), + confirmed_historical_communication_channels=(), + error_health_audit_written=False, + started=False, + state_receive_times=deque([0.0, 1.0]), + minimum_feedback_hz=15.0, + command_names=COMMAND_NAMES, + raw_path=tmp_path / "raw.jsonl", + _feedback_hz=lambda: 50.0, + _feedback_age_seconds=lambda _now: 0.01, + _update_error_health=lambda now: O12ThreeCameraCalibrationNode._update_error_health( + fake, now + ), + _pause=pauses.append, + get_logger=lambda: SimpleNamespace(warning=warnings.append), + ) + report = SimpleNamespace(data=[0, 16, *([0] * 10)]) + O12ThreeCameraCalibrationNode._error_callback(fake, report) + O12ThreeCameraCalibrationNode._error_callback(fake, report) + assert not fake.error_verified + fake.error_report_first_matching_at -= 2.0 + O12ThreeCameraCalibrationNode._error_callback(fake, report) + assert fake.error_verified + assert fake.error_health_classification == "historical_communication_latch" + assert fake.confirmed_historical_communication_channels == (1,) + assert not pauses + assert warnings + audit = (tmp_path / "raw.jsonl").read_text() + assert '"classification":"historical_communication_latch"' in audit + assert '"communication_channels":["thumb_abad"]' in audit + + fake.started = True + O12ThreeCameraCalibrationNode._error_callback( + fake, SimpleNamespace(data=[16, 16, *([0] * 10)]) + ) + assert not pauses + assert fake.error_health_classification == "confirming_historical_communication" + assert not fake.error_verified + assert '"while_started":true' in (tmp_path / "raw.jsonl").read_text() + + +def test_o12_default_health_query_does_not_send_unsupported_temperature_request( +) -> None: + class Publisher: + def __init__(self) -> None: + self.messages = [] + + def publish(self, message) -> None: + self.messages.append(message) + + mode = Publisher() + error = Publisher() + temperature = Publisher() + fake = SimpleNamespace( + commands_enabled=True, + control_mode_publisher=mode, + error_query_publisher=error, + temperature_query_publisher=temperature, + temperature_report_required=False, + temperature_verified=False, + last_temperature_query_at=0.0, + last_health_query_at=0.0, + ) + O12ThreeCameraCalibrationNode._query_health(fake) + assert len(mode.messages) == 1 + assert len(error.messages) == 1 + assert not temperature.messages + + def test_o12_required_temperature_report_disables_fallback(tmp_path: Path) -> None: fake = SimpleNamespace( temperature_report_required=True, @@ -193,23 +478,1585 @@ def test_o12_motion_plan_has_collision_clearance_and_safe_return() -> None: profile = build_typed_profile() fake = SimpleNamespace( profile=profile, - motion_speed_scale=2.0, + motion_speed_scale=4.0, _full_target=O12ThreeCameraCalibrationNode._full_target, ) steps = O12ThreeCameraCalibrationNode._build_steps(fake) clearance = [step for step in steps if step.phase.startswith("clearance")] - assert clearance[0].target_command[10:] == (PARK_FINGER_RAD, PARK_FINGER_RAD) - assert clearance[0].speed_u8 == 0.15 + assert clearance[0].target_command[10:] == ( + PARK_RING_MCP_RAD, PARK_PINKY_MCP_RAD + ) + assert clearance[0].required_endpoint_indices == (10, 11) + assert clearance[0].speed_u8 == 0.20 assert clearance[1].target_command[4] == pytest.approx(-math.radians(10.0)) - assert clearance[1].speed_u8 == 0.08 + assert clearance[1].target_command[5] == pytest.approx( + INDEX_CLEARANCE_MCP_RAD + ) + assert clearance[1].speed_u8 == 0.12 + index_clearance = next( + step for step in clearance if step.phase == "clearance" + ) + assert index_clearance.target_command[7] == pytest.approx(0.0) + assert index_clearance.target_command[8] == pytest.approx(SAFE_UPPER_RAD[8]) + assert index_clearance.target_command[9] == pytest.approx(SAFE_UPPER_RAD[9]) + assert index_clearance.target_command[10] == pytest.approx(SAFE_UPPER_RAD[10]) + assert index_clearance.target_command[11] == pytest.approx(SAFE_UPPER_RAD[11]) + assert index_clearance.required_endpoint_indices == (7, 8, 9, 10, 11) assert [step.phase for step in steps[-3:]] == [ "return_splay_zero", "return_middle_open", "return_outer_open" ] + assert not [step for step in steps if step.phase == "prepare"] for task in profile.motion.tasks: probes = [step for step in steps if step.task_key == task.key and step.phase == "preflight"] assert max(abs(step.target_u8 - task.start_value) for step in probes) <= math.radians(3.0) + 1e-12 + relative = [ + step for step in probes + if step.relative_feedback_delta is not None + ] + assert len(relative) == 1 + assert relative[0].relative_feedback_delta == pytest.approx( + relative[0].target_u8 - task.start_value + ) sweeps = [step for step in steps if step.task_key == task.key and step.phase == "sweep"] - assert {step.speed_u8 for step in sweeps} == {2.0 * task.formal_speed} + expected_speed = min( + 4.0 * task.formal_speed, + FORMAL_SPEED_CAP_RAD_S[task.command_index], + ) + assert {step.speed_u8 for step in sweeps} == {expected_speed} + thumb_pitch_sweeps = [ + step for step in steps + if step.task_key == "thumb_pitch_front" and step.phase == "sweep" + ] + assert {step.speed_u8 for step in thumb_pitch_sweeps} == {0.10} + middle_roll = next( + task for task in profile.motion.tasks + if task.key == "middle_roll_front" + ) + index_roll = next( + task for task in profile.motion.tasks + if task.key == "index_roll_front" + ) + assert dict(middle_roll.auxiliary_commands)[5] == pytest.approx( + INDEX_CLEARANCE_MCP_RAD + ) + assert dict(middle_roll.auxiliary_commands)[8] == pytest.approx( + ROLL_CLEARANCE_MCP_RAD + ) + assert dict(index_roll.auxiliary_commands)[5] == pytest.approx( + ROLL_CLEARANCE_MCP_RAD + ) + assert dict(index_roll.auxiliary_commands)[7] == pytest.approx(0.0) + assert dict(index_roll.auxiliary_commands)[8] == pytest.approx( + SAFE_UPPER_RAD[8] + ) + assert dict(index_roll.auxiliary_commands)[9] == pytest.approx( + SAFE_UPPER_RAD[9] + ) + for task in profile.motion.tasks: + if not task.key.startswith("index_"): + continue + clearance_target = dict(task.auxiliary_commands) + assert clearance_target[7] == pytest.approx(0.0) + assert clearance_target[8] == pytest.approx(SAFE_UPPER_RAD[8]) + assert clearance_target[9] == pytest.approx(SAFE_UPPER_RAD[9]) + assert clearance_target[10] == pytest.approx(SAFE_UPPER_RAD[10]) + assert clearance_target[11] == pytest.approx(SAFE_UPPER_RAD[11]) + + +def test_o12_clearance_waits_until_all_required_fingers_are_parked( + monkeypatch, +) -> None: + delegated: list[bool] = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_tick_radian_motion", + lambda _node, _step, _now: delegated.append(True), + ) + target = [0.0] * 12 + target[8] = PARK_MIDDLE_MCP_RAD + target[9] = PARK_MIDDLE_PIP_RAD + target[10] = PARK_RING_MCP_RAD + target[11] = PARK_PINKY_MCP_RAD + step = O12MotionStep( + "clearance", None, None, 0.0, 0.20, + target_command=tuple(target), + required_endpoint_indices=(7, 8, 9, 10, 11), + ) + node = object.__new__(O12ThreeCameraCalibrationNode) + node.step_trajectory_phase = 1.0 + node.command_count = 12 + node.command_names = COMMAND_NAMES + node.latest_state_u8 = tuple(target) + node.step_start_state_u8 = (0.0,) * 12 + node.step_start_feedback_u8 = (0.0,) * 12 + node.step_last_distance_u8 = 0.0 + node.step_last_progress_at = 10.0 + node.step_hold_since = 10.0 + node.motor_stall_timeout_seconds = 2.0 + node._target_command = lambda active_step: tuple( + active_step.target_command + ) + node._radian_feedback_travel = lambda _step: 1.0 + measured_travel = { + 8: target[8], + 9: 1.522766434, + 10: target[10], + 11: target[11], + } + node._o12_measured_channel_travel_rad = measured_travel.get + paused: list[str] = [] + node._pause = paused.append + + incomplete = list(target) + incomplete[8] = ( + 0.5 * CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION * target[8] + ) + node.latest_state_u8 = tuple(incomplete) + O12ThreeCameraCalibrationNode._tick_radian_motion(node, step, 10.5) + assert delegated == [] + assert paused == [] + assert node.step_hold_since is None + + # A stable SDK feedback endpoint may differ from the command endpoint; + # reaching most of the requested physical travel must still be accepted. + biased_endpoint = list(target) + biased_endpoint[8] = 0.90 * measured_travel[8] + # Exact field regression: full command is 1.815 rad, but simultaneous + # MCP/PIP clearance settles at 1.415 rad after the isolated scan measured + # 1.523 rad. This is 92.9% of physical travel and must not be compared to + # the command endpoint (78.0%). + biased_endpoint[9] = 1.415 + node.latest_state_u8 = tuple(biased_endpoint) + O12ThreeCameraCalibrationNode._tick_radian_motion(node, step, 10.6) + assert delegated == [True] + + # The same rule is symmetric when a parked finger returns toward zero. + return_step = O12MotionStep( + "return_middle_open", None, None, 0.0, 0.20, + target_command=(0.0,) * 12, + required_endpoint_indices=(8, 9), + ) + node.step_start_state_u8 = tuple(target) + node.step_start_feedback_u8 = tuple(biased_endpoint) + returned = list(biased_endpoint) + returned[8] = 0.0 + returned[9] = 0.0 + node.latest_state_u8 = tuple(returned) + node.step_last_distance_u8 = 0.0 + node.step_last_progress_at = 11.0 + O12ThreeCameraCalibrationNode._tick_radian_motion( + node, return_step, 11.1 + ) + assert delegated == [True, True] + + +def test_o12_clearance_has_no_second_command_scale_travel_gate() -> None: + node = object.__new__(O12ThreeCameraCalibrationNode) + step = O12MotionStep( + "clearance", None, None, 0.0, 0.20, + target_command=(0.0,) * 12, + required_endpoint_indices=(8, 9, 10, 11), + ) + assert node._minimum_radian_feedback_travel(step) == pytest.approx(0.0) + + +def test_o12_automatic_rescan_preserves_o12_motion_step_type( + tmp_path: Path, +) -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_pip_dip_side" + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.end_value, + 0.32, 0, "decreasing", 1, + ) + node = SimpleNamespace( + retry_counts={}, + calibration_engine=CalibrationEngine(profile), + profile=profile, + steps=[step], + step_index=0, + raw_path=tmp_path / "raw.jsonl", + _task=lambda _key: task, + ) + + assert L6ThreeCameraCalibrationNode._retry_step( + node, step, "maximum_gap=6" + ) + assert all(isinstance(item, O12MotionStep) for item in node.steps) + assert node.steps[1].phase == "retry_prepare" + assert node.steps[1].required_endpoint_indices == () + + +def test_o12_radian_tick_accepts_legacy_generated_step(monkeypatch) -> None: + delegated: list[MotionStep] = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_tick_radian_motion", + lambda _node, active_step, _now: delegated.append(active_step), + ) + step = MotionStep( + "retry_prepare", "middle_pip_dip_side", 9, 0.0, 0.32, + 0, attempt=2, + ) + node = object.__new__(O12ThreeCameraCalibrationNode) + node.step_trajectory_phase = 1.0 + node.latest_state_u8 = (0.0,) * 12 + node.command_count = 12 + + node._tick_radian_motion(step, 10.0) + + assert delegated == [step] + + +def test_o12_log_exception_summary_reports_child_traceback( + tmp_path: Path, +) -> None: + log_path = tmp_path / "calibration.log" + log_path.write_text( + "Traceback (most recent call last):\n" + "[node-1] AttributeError: missing motion metadata\n" + "[ERROR] [node-1]: process has died\n", + encoding="utf-8", + ) + assert _log_exception_summary(log_path) == ( + "AttributeError: missing motion metadata" + ) + + +def test_o12_preflight_resolves_three_degree_probe_from_measured_feedback() -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "thumb_yaw_top" + ) + step = O12MotionStep( + "preflight", + task.key, + task.command_index, + task.start_value - math.radians(3.0), + 0.06, + relative_feedback_delta=-math.radians(3.0), + ) + node = object.__new__(O12ThreeCameraCalibrationNode) + node.command_unit = "rad" + node.command_count = 12 + node.profile = profile + node.command_lower = profile.command.minimum_values + node.command_upper = profile.command.maximum_values + node.baseline_command = profile.command.baseline_values + node.latest_state_u8 = (0.0, -0.023) + (0.0,) * 10 + node.last_published_command_u8 = profile.command.baseline_values + node.commanded_speed = step.speed_u8 + node.step_speed_ready_at = 0.0 + node.step_command_sent = False + node.resolved_probe_target_by_step = {} + node.probe_feedback_origin_by_step = {} + node.non_target_motion_tolerance_u8 = 0.015 + node.step_data_lock = threading.RLock() + node.vision_callbacks_inflight = 0 + node._current_step = lambda: step + node._task = lambda _key: task + node._advance_step_trajectory = lambda _step, _now: None + node._radian_trajectory_fraction = lambda *_args: (0.0, 0.0, 1.0) + + O12ThreeCameraCalibrationNode._begin_step(node, step) + + assert node.probe_feedback_origin_by_step[id(step)] == pytest.approx(-0.023) + assert node.resolved_probe_target_by_step[id(step)][1] == pytest.approx( + -0.023 - math.radians(3.0) + ) + assert node.resolved_probe_target_by_step[id(step)][1] < step.target_u8 + assert abs( + node.resolved_probe_target_by_step[id(step)][1] + - node.probe_feedback_origin_by_step[id(step)] + ) == pytest.approx(math.radians(3.0)) + + +def test_o12_locked_front_base_is_used_only_for_occluded_finger_tasks() -> None: + profile = build_typed_profile() + pose = SquareTagPose( + quaternion_xyzw=(0.0, 0.0, 0.0, 1.0), + translation_xyz_m=(0.0, 0.0, 0.5), + reprojection_error_px=0.2, + ) + node = object.__new__(O12ThreeCameraCalibrationNode) + node.profile = profile + node.locked_base_pose_by_view = {"front": pose} + + middle = SimpleNamespace(task_key="middle_roll_front") + index = SimpleNamespace(task_key="index_roll_front") + thumb = SimpleNamespace(task_key="thumb_roll_front") + assert node._locked_reference_poses_for_capture("front", middle) == { + "front_base": pose + } + assert node._locked_reference_poses_for_capture("front", index) == { + "front_base": pose + } + assert node._locked_reference_poses_for_capture("front", thumb) == {} + assert node._locked_reference_poses_for_capture("side", middle) == {} + + +def test_o12_mapping_probe_accepts_feedback_when_live_tag_is_occluded( + monkeypatch, tmp_path: Path +) -> None: + delegated: list[O12MotionStep] = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_finish_step", + lambda _node, active_step: delegated.append(active_step), + ) + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "thumb_roll_front" + ) + delta = math.radians(3.0) + step = O12MotionStep( + "preflight", task.key, task.command_index, delta, 0.06, + relative_feedback_delta=delta, + ) + node = object.__new__(O12ThreeCameraCalibrationNode) + node.profile = profile + node.command_names = COMMAND_NAMES + node.preflight_maximum_rotation_by_task = {} + node.measured_direction_axis_by_task = {} + node.probe_feedback_origin_by_step = {id(step): 0.0} + node.latest_state_u8 = (0.04,) + (0.0,) * 11 + node.step_data_lock = threading.RLock() + node.vision_callbacks_inflight = 0 + node.step_command_sent = True + node.state = "PAUSED" + node.raw_path = tmp_path / "raw.jsonl" + node._task = lambda _key: task + node._target_command = lambda _step: (delta,) + (0.0,) * 11 + node._radian_feedback_travel = lambda _step: 0.04 + paused: list[str] = [] + node._pause = paused.append + node._current_step = lambda: None + + node._finish_step(step) + + assert paused == [] + assert delegated == [step] + row = json.loads(node.raw_path.read_text(encoding="utf-8")) + assert row["visual_mapping_verified"] is False + assert row["verification_basis"] == "feedback_only_live_tag_unavailable" + assert row["projected_feedback_travel_rad"] == pytest.approx(0.04) + + +def _resume_rows(profile, count: int) -> list[dict]: + units = ordered_resume_units(profile)[:count] + task_by_key = {task.key: task for task in profile.motion.tasks} + rows: list[dict] = [] + for task_name, cycle, direction in units: + task = task_by_key[task_name] + for sample in range(40): + rows.append({ + "kind": "o12_joint_sample", + "task_name": task_name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "joint": task.joints[0], + "feedback_rad": float(sample), + }) + rows.append({ + "kind": "o12_sweep_observation_quality", + "task_name": task_name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "valid_frames": 40, + "total_frames": 40, + "feedback_bins": 32, + "maximum_bin_gap": 1, + "tag_detection_rate": 1.0, + "failures": [], + }) + return rows + + +def test_o12_resume_accepts_legacy_one_frame_counter_boundary() -> None: + profile = build_typed_profile() + rows = _resume_rows(profile, 1) + quality = next( + row for row in rows + if row["kind"] == "o12_sweep_observation_quality" + ) + quality["valid_frames"] = 41 + quality["total_frames"] = 40 + + checkpoint = build_resume_checkpoint_from_rows( + profile, Path("legacy_frame_boundary"), rows, compatibility="test" + ) + + assert len(checkpoint.completed_units) == 1 + + +def test_o12_resume_uses_recorded_observability_decision() -> None: + profile = build_typed_profile() + rows = _resume_rows(profile, 1) + quality = next( + row for row in rows + if row["kind"] == "o12_sweep_observation_quality" + ) + quality.update({ + "quality_policy_version": QUALITY_POLICY_VERSION, + "passed": True, + "tag_detection_rate": 0.928, + "maximum_bin_gap": 5, + "warnings": [ + "tag_rate_target[middle_pip]=0.928", + "maximum_gap_target=5", + ], + }) + checkpoint = build_resume_checkpoint_from_rows( + profile, Path("observability_v2"), rows, compatibility="test" + ) + assert len(checkpoint.completed_units) == 1 + + quality["passed"] = False + quality["failures"] = ["maximum_gap=9"] + with pytest.raises(ValueError, match="no contiguous passed scan unit"): + build_resume_checkpoint_from_rows( + profile, Path("observability_v2"), rows, compatibility="test" + ) + + +def test_o12_continuous_feedback_uses_64_cells_for_32_bin_gate( + tmp_path: Path, +) -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_roll_front" + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.end_value, + 0.16, 0, "decreasing", 1, + ) + feedback = np.linspace(-0.228753502, 0.244652041, 125) + fake = SimpleNamespace( + raw_records=[{ + "task_name": task.key, "cycle": 0, + "direction": "decreasing", "attempt": 1, + "joint": task.joints[0], "feedback_rad": float(value), + } for value in feedback], + profile=profile, + command_unit="rad", + normalized_sweep_bin_count=NORMALIZED_SWEEP_BIN_COUNT, + minimum_state_span_u8=0.90, + minimum_sweep_frames=40, + minimum_sweep_bins=32, + maximum_bin_gap=2, + minimum_detection_rate=0.95, + minimum_joint_frame_rate=0.85, + minimum_feedback_hz=25.0, + step_total_frames=125, + raw_path=tmp_path / "quality.jsonl", + sweep_quality_kind="o12_sweep_observation_quality", + _task=lambda _key: task, + _step_observation_metrics=lambda: { + "tag_detection_rate": 1.0, + "joint_frame_rate": 1.0, + "tag_detection_rate_by_role": { + "front_base": 1.0, "middle_roll": 1.0, + }, + "tag_seen_rate_by_role": { + "front_base": 1.0, "middle_roll": 1.0, + }, + "all_tags_quality_rate": 1.0, + "pnp_valid_rate": 1.0, + "state_sync_rate": 1.0, + "rejection_counts": {}, + }, + _feedback_hz=lambda: 48.0, + ) + L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) + quality = json.loads(fake.raw_path.read_text().strip()) + assert quality["feedback_bins"] >= 32 + assert quality["maximum_bin_gap"] <= 2 + assert quality["failures"] == [] + + +def test_o12_quality_accepts_dense_fit_data_after_short_tag_occlusion( + tmp_path: Path, +) -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_pip_dip_side" + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.end_value, + 0.16, 1, "decreasing", 3, + ) + retained_bins = [ + value for value in range(3, 61) if value not in {30, 31, 32, 33} + ] + normalized = [ + (retained_bins[index % len(retained_bins)] + 0.25) + / NORMALIZED_SWEEP_BIN_COUNT + for index in range(324) + ] + feedback = [ + profile.command.denormalize(task.command_index, value) + for value in normalized + ] + observation = { + "tag_detection_rate": 0.928, + "joint_frame_rate": 0.928, + "tag_detection_rate_by_role": { + "side_base": 1.0, "middle_pip": 0.928, + "middle_dip": 1.0, + }, + "tag_seen_rate_by_role": { + "side_base": 1.0, "middle_pip": 0.928, + "middle_dip": 1.0, + }, + "all_tags_quality_rate": 0.928, + "pnp_valid_rate": 0.928, + "state_sync_rate": 0.928, + "rejection_counts": {"tag:middle_pip:missing": 25}, + } + fake = SimpleNamespace( + raw_records=[{ + "kind": "o12_joint_sample", + "task_name": task.key, + "cycle": 1, + "direction": "decreasing", + "attempt": 3, + "joint": task.joints[0], + "feedback_rad": float(value), + } for value in feedback], + profile=profile, + normalized_sweep_bin_count=NORMALIZED_SWEEP_BIN_COUNT, + minimum_sweep_frames=40, + minimum_sweep_bins=32, + maximum_bin_gap=2, + minimum_detection_rate=0.95, + minimum_joint_frame_rate=0.85, + minimum_feedback_hz=35.0, + step_total_frames=349, + raw_path=tmp_path / "quality.jsonl", + sweep_quality_kind="o12_sweep_observation_quality", + _task=lambda _key: task, + _required_radian_feedback_span_fraction=( + lambda _task, _step, _feedback: 0.85 + ), + _normalize_o12_feedback=( + lambda _task, _step, _feedback: np.asarray(normalized) + ), + _step_observation_metrics=lambda: observation, + _feedback_hz=lambda: 50.0, + ) + O12ThreeCameraCalibrationNode._qualify_o12_primary_recording_step( + fake, step + ) + quality = json.loads(fake.raw_path.read_text().strip()) + assert quality["quality_policy_version"] == QUALITY_POLICY_VERSION + assert quality["valid_frames"] == 324 + assert quality["feedback_bins"] == 54 + # Four consecutive normalized bins are unobserved; the metric reports + # missing bins, not the five-index distance between their neighbours. + assert quality["maximum_bin_gap"] == 4 + assert quality["allowed_maximum_bin_gap"] == 4 + assert quality["failures"] == [] + assert quality["passed"] is True + assert "tag_rate=0.928" in quality["warnings"] + + severe_gap = evaluate_o12_observation_quality( + [value for value in normalized if not 0.35 < value < 0.55], + observation, + normalized_bin_count=NORMALIZED_SWEEP_BIN_COUNT, + required_feedback_span=0.85, + minimum_sweep_frames=40, + minimum_sweep_bins=32, + minimum_joint_frame_rate=0.85, + minimum_feedback_hz=35.0, + feedback_hz=50.0, + target_detection_rate=0.95, + target_maximum_bin_gap=2, + ) + assert any( + failure.startswith("maximum_gap=") + for failure in severe_gap["failures"] + ) + + +def test_o12_feedback_scale_offset_is_not_counted_as_an_internal_gap( + tmp_path: Path, +) -> None: + """Reproduce the 1.815-command/1.541-feedback middle-PIP sweep.""" + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_pip_dip_side" + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.end_value, + 0.32, 0, "decreasing", 2, + ) + feedback = np.linspace(0.017880790, 1.540647224, 201) + fake = SimpleNamespace( + raw_records=[{ + "kind": "o12_joint_sample", + "task_name": task.key, + "cycle": 0, + "direction": "decreasing", + "attempt": 2, + "joint": task.joints[0], + "feedback_rad": float(value), + } for value in feedback], + profile=profile, + normalized_sweep_bin_count=NORMALIZED_SWEEP_BIN_COUNT, + step_total_frames=211, + raw_path=tmp_path / "quality.jsonl", + sweep_quality_kind="o12_sweep_observation_quality", + _task=lambda _key: task, + _required_radian_feedback_span_fraction=( + lambda _task, _step, _feedback: 0.99 + ), + _normalize_o12_feedback=( + lambda _task, _step, values: + (np.asarray(values) - np.min(values)) / np.ptp(values) + ), + _step_observation_metrics=lambda: { + "tag_detection_rate": 1.0, + "joint_frame_rate": 201 / 211, + "tag_detection_rate_by_role": { + "side_base": 1.0, "middle_pip": 1.0, + "middle_dip": 1.0, + }, + "tag_seen_rate_by_role": { + "side_base": 1.0, "middle_pip": 1.0, + "middle_dip": 1.0, + }, + "all_tags_quality_rate": 201 / 211, + "pnp_valid_rate": 201 / 211, + "state_sync_rate": 201 / 211, + "rejection_counts": {}, + }, + _feedback_hz=lambda: 50.0, + ) + + O12ThreeCameraCalibrationNode._qualify_o12_primary_recording_step( + fake, step + ) + + quality = json.loads(fake.raw_path.read_text(encoding="utf-8")) + assert quality["feedback_span"] == pytest.approx(1.0, abs=1e-6) + assert quality["feedback_bins"] == 64 + assert quality["maximum_bin_gap"] == 0 + assert quality["gap_scope"] == "observed_feedback_span" + assert quality["failures"] == [] + assert quality["passed"] is True + + +def test_o12_feedback_span_repeats_cycle_zero_effective_travel() -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_roll_front" + ) + cycle_zero_feedback = np.linspace(-0.233773934, 0.234931874, 126) + current_feedback = np.linspace(-0.229859, 0.234997, 307) + fake = SimpleNamespace( + profile=profile, + raw_records=[{ + "kind": "o12_joint_sample", + "task_name": task.key, + "cycle": 0, + "direction": "increasing", + "attempt": 1, + "joint": task.joints[0], + "feedback_rad": float(value), + } for value in cycle_zero_feedback], + ) + fake._o12_cycle_zero_feedback_span_fraction = ( + lambda selected_task, direction: + O12ThreeCameraCalibrationNode + ._o12_cycle_zero_feedback_span_fraction( + fake, selected_task, direction + ) + ) + fake._o12_cycle_zero_feedback_bounds = ( + lambda selected_task, direction: + O12ThreeCameraCalibrationNode._o12_cycle_zero_feedback_bounds( + fake, selected_task, direction + ) + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.start_value, + 0.12, 1, "increasing", 1, + ) + + reference = ( + O12ThreeCameraCalibrationNode + ._o12_cycle_zero_feedback_span_fraction( + fake, task, "increasing" + ) + ) + required = ( + O12ThreeCameraCalibrationNode + ._required_radian_feedback_span_fraction( + fake, task, step, current_feedback + ) + ) + normalized_current = ( + O12ThreeCameraCalibrationNode._normalize_o12_feedback( + fake, task, step, current_feedback + ) + ) + actual = float(np.ptp(normalized_current)) + + assert reference == pytest.approx(1.0, abs=1.0e-6) + assert required == pytest.approx( + EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION * reference + ) + assert actual == pytest.approx( + float(np.ptp(current_feedback)) / float(np.ptp(cycle_zero_feedback)), + abs=1.0e-6, + ) + assert actual >= required + assert actual / reference >= EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION + + +def test_o12_feedback_span_gate_uses_cycle_zero_measured_reference() -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_roll_front" + ) + lower = profile.command.minimum_values[task.command_index] + upper = profile.command.maximum_values[task.command_index] + cycle_zero_feedback = np.linspace(lower, lower + 0.98 * (upper - lower), 64) + fake = SimpleNamespace( + profile=profile, + raw_records=[{ + "kind": "o12_joint_sample", + "task_name": task.key, + "cycle": 0, + "direction": "decreasing", + "attempt": 1, + "joint": task.joints[0], + "feedback_rad": float(value), + } for value in cycle_zero_feedback], + ) + fake._o12_cycle_zero_feedback_span_fraction = ( + lambda selected_task, direction: + O12ThreeCameraCalibrationNode + ._o12_cycle_zero_feedback_span_fraction( + fake, selected_task, direction + ) + ) + fake._o12_cycle_zero_feedback_bounds = ( + lambda selected_task, direction: + O12ThreeCameraCalibrationNode._o12_cycle_zero_feedback_bounds( + fake, selected_task, direction + ) + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.end_value, + 0.12, 2, "decreasing", 1, + ) + required = ( + O12ThreeCameraCalibrationNode + ._required_radian_feedback_span_fraction( + fake, task, step, np.asarray([]) + ) + ) + assert required == pytest.approx(0.90) + + +def test_o12_resume_requires_side_view_for_every_roll_unit() -> None: + profile = build_typed_profile() + rows = _resume_rows(profile, 41) + checkpoint = build_resume_checkpoint_from_rows( + profile, Path("pre_multiview_roll"), rows, compatibility="test" + ) + assert len(checkpoint.completed_units) == 40 + + task_name, cycle, direction = ordered_resume_units(profile)[40] + observer = ROLL_CROSS_VIEW_BY_TASK[task_name] + rows.extend({ + "kind": "o12_roll_cross_view_sample", + "task_name": task_name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "joint": observer.source_joint, + "model_joint": observer.model_joint, + "feedback_rad": float(sample), + } for sample in range(40)) + rows.append({ + "kind": "o12_roll_cross_view_quality", + "task_name": task_name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "valid_frames": 40, + "total_frames": 40, + "feedback_bins": 32, + "maximum_bin_gap": 1, + "tag_detection_rate": 1.0, + "failures": [], + }) + checkpoint = build_resume_checkpoint_from_rows( + profile, Path("multiview_roll"), rows, compatibility="test" + ) + assert len(checkpoint.completed_units) == 41 + assert len([ + row for row in checkpoint.imported_records + if row["kind"] == "o12_roll_cross_view_sample" + ]) == 40 + + +def test_o12_complete_resume_finalizes_without_starting_hardware( + tmp_path: Path, monkeypatch, +) -> None: + profile = build_typed_profile() + config = SimpleNamespace( + calibration_contract=SimpleNamespace(typed_profile=profile), + serial_number="O12_TEST", + source_urdf=SOURCE_URDF, + session_root=tmp_path, + source_urdf_sha256="source", + camera_extrinsics_sha256="extrinsics", + calibration_config_sha256="calibration", + tag_config_sha256="tags", + sdk_config_sha256="sdk", + ) + records = ({"kind": "passed_observation"},) + resume = SimpleNamespace( + completed_units=ordered_resume_units(profile), + imported_records=records, + ) + captured = {} + + def finalize(**kwargs): + captured.update(kwargs) + correction = SimpleNamespace(path=tmp_path / "result.urdf") + return {"quality": {"passed": True}}, object(), correction + + monkeypatch.setattr(o12_runner, "finalize_o12_session", finalize) + assert o12_runner._finalize_completed_resume( + config, resume, tmp_path + ) == 0 + assert captured["records"] == list(records) + assert captured["publish"] is True + + +def test_o12_resume_audit_is_not_inserted_into_live_sample_rows( + tmp_path: Path, monkeypatch, +) -> None: + from linkerhand_calibration.models.o12 import node as o12_node + from linkerhand_calibration.storage import append_jsonl_many as write_many + profile = build_typed_profile() + serial_root = tmp_path / "O12_RIGHT_001" + source_session = serial_root / "old" + current_session = serial_root / "new" + source_session.mkdir(parents=True) + current_session.mkdir() + protected = { + key: f"{key}_value" + for key in profile.artifacts.protected_input_fields + } + rows = [{ + "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "sample_schema_version": profile.artifacts.output_schema_version, + "profile_id": profile.key.profile_id, + "serial_number": "O12_RIGHT_001", + **protected, + }, *_resume_rows(profile, 1)] + task_name, cycle, direction = ordered_resume_units(profile)[0] + rows.append({ + "kind": "o12_pnp_candidate_frame", + "task_name": task_name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "image_stamp_ns": 123, + }) + source_raw = source_session / "raw_samples.jsonl" + source_raw.write_text( + "".join(json.dumps(row) + "\n" for row in rows), + encoding="utf-8", + ) + + node = object.__new__(O12ThreeCameraCalibrationNode) + node.profile = profile + node.serial_number = "O12_RIGHT_001" + node.resume_raw_samples_path = source_raw + node.session_dir = current_session + node.raw_path = current_session / "raw_samples.jsonl" + node.protected_inputs = protected + node.raw_records = [] + + batches = [] + def capture_batch(path, payloads): + batch = list(payloads) + batches.append(batch) + write_many(path, batch) + monkeypatch.setattr(o12_node, "append_jsonl_many", capture_batch) + + O12ThreeCameraCalibrationNode._restore_resume_checkpoint(node) + + assert len(batches) == 1 + assert batches[0][0]["kind"] == "o12_resume_checkpoint_import" + assert any( + row.get("kind") == "o12_pnp_candidate_frame" + for row in batches[0] + ) + assert node.raw_records + assert all(row.get("kind") == "o12_joint_sample" for row in node.raw_records) + written = node.raw_path.read_text(encoding="utf-8") + assert '"kind":"o12_resume_checkpoint_import"' in written + + +def test_o12_resume_reuses_only_contiguous_passed_units_and_safe_plan( + tmp_path: Path, +) -> None: + profile = build_typed_profile() + rows = _resume_rows(profile, 10) + # A later isolated PASS cannot be used across the first missing unit. + later = ordered_resume_units(profile)[11] + task = next(item for item in profile.motion.tasks if item.key == later[0]) + rows.extend(_resume_rows(profile, 12)[-41:]) + checkpoint = build_resume_checkpoint_from_rows( + profile, tmp_path / "old", rows, compatibility="test" + ) + assert checkpoint.completed_units == ordered_resume_units(profile)[:10] + assert checkpoint.completed_tasks == (profile.motion.tasks[0].key,) + assert len([ + row for row in checkpoint.imported_records + if row["kind"] == "o12_joint_sample" + ]) == 400 + assert task.key == profile.motion.tasks[1].key + + fake = SimpleNamespace( + profile=profile, + motion_speed_scale=4.0, + _full_target=O12ThreeCameraCalibrationNode._full_target, + resumed_unit_keys=frozenset(checkpoint.completed_units), + resumed_task_keys=frozenset(checkpoint.completed_tasks), + resume_skipped_step_count=0, + ) + steps = O12ThreeCameraCalibrationNode._build_steps(fake) + assert steps[0].phase == "baseline" + assert not [ + step for step in steps + if step.task_key == profile.motion.tasks[0].key + ] + second = [ + step for step in steps + if step.task_key == profile.motion.tasks[1].key + ] + assert len([step for step in second if step.phase == "preflight"]) == 3 + assert len([step for step in second if step.recording]) == 6 + assert fake.resume_skipped_step_count == 13 + + increasing_checkpoint = build_resume_checkpoint_from_rows( + profile, + tmp_path / "old_increasing", + _resume_rows(profile, 9), + compatibility="test", + ) + increasing_fake = SimpleNamespace( + profile=profile, + motion_speed_scale=4.0, + _full_target=O12ThreeCameraCalibrationNode._full_target, + _task=lambda key: next( + item for item in profile.motion.tasks if item.key == key + ), + resumed_unit_keys=frozenset(increasing_checkpoint.completed_units), + resumed_task_keys=frozenset(increasing_checkpoint.completed_tasks), + resume_skipped_step_count=0, + ) + increasing_steps = O12ThreeCameraCalibrationNode._build_steps( + increasing_fake + ) + first_scan_index = next( + index for index, step in enumerate(increasing_steps) if step.recording + ) + assert increasing_steps[first_scan_index].direction == "increasing" + assert increasing_steps[first_scan_index - 1].phase == "resume_prepare" + assert increasing_steps[first_scan_index - 1].target_u8 == pytest.approx( + profile.motion.tasks[1].end_value + ) + + +def test_o12_truncated_resume_import_cannot_shadow_complete_source() -> None: + profile = build_typed_profile() + rows = _resume_rows(profile, 10) + rows.insert(0, { + "kind": "o12_resume_checkpoint_import", + "completed_unit_count": 11, + }) + with pytest.raises(ValueError, match="incomplete or truncated"): + build_resume_checkpoint_from_rows( + profile, + Path("truncated_resume"), + rows, + compatibility="test", + ) + + +def test_o12_automatic_resume_requires_exact_protected_hashes( + tmp_path: Path, +) -> None: + profile = build_typed_profile() + serial_root = tmp_path / "O12_TEST" + session = serial_root / "20260907_120000" + session.mkdir(parents=True) + hashes = { + "source_urdf_sha256": "a" * 64, + "camera_extrinsics_sha256": "b" * 64, + "calibration_config_sha256": "c" * 64, + "tag_config_sha256": "d" * 64, + "sdk_config_sha256": "e" * 64, + } + rows = [{ + "kind": "session_start", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "sample_schema_version": 7, + "profile_id": profile.key.profile_id, + "serial_number": "O12_TEST", + **hashes, + }, *_resume_rows(profile, 1)] + (session / "raw_samples.jsonl").write_text( + "".join(json.dumps(row) + "\n" for row in rows), + encoding="utf-8", + ) + config = SimpleNamespace( + session_root=serial_root, + profile_key=profile.key, + serial_number="O12_TEST", + calibration_contract=SimpleNamespace(typed_profile=profile), + source_urdf_sha256=hashes["source_urdf_sha256"], + camera_extrinsics_sha256=hashes["camera_extrinsics_sha256"], + calibration_config_sha256=hashes["calibration_config_sha256"], + tag_config_sha256=hashes["tag_config_sha256"], + sdk_config_sha256=hashes["sdk_config_sha256"], + ) + checkpoint = automatic_resume_candidate(config) + assert checkpoint is not None + assert checkpoint.source_session == session.resolve() + config.sdk_config_sha256 = "f" * 64 + assert automatic_resume_candidate(config) is None + + +def test_o12_radian_motion_keeps_command_and_feedback_domains_separate() -> None: + fake = SimpleNamespace( + command_count=12, + latest_state_u8=(0.034,) + (0.0,) * 11, + step_start_feedback_u8=(0.0, -0.017302) + (0.0,) * 10, + step_moving_indices=frozenset({0}), + step_initial_distance_u8=0.052, + ) + preflight = MotionStep("preflight", "thumb_roll_front", 0, 0.052, 0.06) + sweep = MotionStep( + "sweep", "thumb_roll_front", 0, 0.73, 0.16, + cycle=0, direction="decreasing", + ) + travel = O12ThreeCameraCalibrationNode._radian_feedback_travel + minimum = O12ThreeCameraCalibrationNode._minimum_radian_feedback_travel + assert travel(fake, preflight) == pytest.approx(0.034) + assert minimum(fake, preflight) == pytest.approx(0.004) + assert minimum(fake, sweep) == 0.0 + + baseline = MotionStep("baseline", None, None, 0.0, 0.20) + fake.step_initial_distance_u8 = 0.0 + fake.step_moving_indices = frozenset() + fake.latest_state_u8 = fake.step_start_feedback_u8 + # A stable -0.017302 rad feedback at command zero is a valid settled + # baseline observation, not an endpoint error or a stall. + assert travel(fake, baseline) == 0.0 + assert minimum(fake, baseline) == 0.0 + + clearance = MotionStep("clearance", None, None, 0.0, 0.20) + fake.step_initial_distance_u8 = 1.0 + # O12 clearance is checked per axis against measured travel; the generic + # aggregate command-scale gate must not be applied a second time. + assert minimum(fake, clearance) == pytest.approx(0.0) + + +def test_o12_republishes_steady_endpoint_to_refresh_request_driven_feedback() -> None: + profile = build_typed_profile() + published: list[list[float]] = [] + fake = SimpleNamespace( + command_unit="rad", + step_started_at=0.0, + step_start_state_u8=(0.0,) * 12, + step_last_command_u8=None, + step_trajectory_phase=0.0, + step_trajectory_blend=0.0, + step_requested_u8=0.0, + command_lower=profile.command.minimum_values, + command_upper=profile.command.maximum_values, + _target_command=lambda selected: tuple( + selected.target_u8 if index == selected.command_index else 0.0 + for index in range(12) + ), + _radian_trajectory_fraction=lambda _distance, _elapsed, _speed: ( + 1.0, 1.0, 1.0 + ), + _publish_command=lambda values: published.append(values), + ) + step = MotionStep( + "preflight", "thumb_yaw_top", 1, -math.radians(3.0), 0.06 + ) + O12ThreeCameraCalibrationNode._advance_step_trajectory(fake, step, 2.0) + O12ThreeCameraCalibrationNode._advance_step_trajectory(fake, step, 2.1) + assert len(published) == 2 + assert published[0] == pytest.approx(published[1]) + + +def test_o12_feedback_domain_contains_but_does_not_expand_command_domain() -> None: + command = build_typed_profile().command + assert command.minimum_values[2] == pytest.approx(-0.8272860654453121) + assert command.minimum_feedback_values == pytest.approx(FEEDBACK_LOWER_RAD) + assert command.maximum_feedback_values == pytest.approx(FEEDBACK_UPPER_RAD) + assert command.minimum_feedback_values[2] == pytest.approx( + command.minimum_values[2] - FEEDBACK_DOMAIN_MARGIN_RAD + ) + assert all( + feedback <= commanded + for feedback, commanded in zip( + command.minimum_feedback_values, command.minimum_values + ) + ) + assert all( + feedback >= commanded + for feedback, commanded in zip( + command.maximum_feedback_values, command.maximum_values + ) + ) + + +def test_o12_state_accepts_endpoint_tracking_inside_feedback_domain() -> None: + command = build_typed_profile().command + paused: list[str] = [] + fake = SimpleNamespace( + command_count=command.command_count, + profile=SimpleNamespace(command=command), + command_names=command.names, + feedback_lower=command.minimum_feedback_values, + feedback_upper=command.maximum_feedback_values, + state_history=[], + latest_state_u8=(), + state_receive_times=[], + _pause=paused.append, + _current_step=lambda: None, + ) + state = list(command.baseline_values) + state[2] = command.minimum_values[2] - 0.002 + message = SimpleNamespace( + name=[], + position=state, + header=SimpleNamespace(stamp=SimpleNamespace(sec=1, nanosec=0)), + ) + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert fake.latest_state_u8[2] == pytest.approx(state[2]) + + state[2] = command.minimum_feedback_values[2] - 0.001 + message.position = state + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert len(paused) == 1 + assert paused[0].startswith( + "feedback_outside_registered_feedback_domain:channel=thumb_mcp:" + ) + + +def test_o12_optional_temperature_report_is_not_a_motion_interlock() -> None: + paused: list[str] = [] + fake = SimpleNamespace( + latest_temperatures=(), + temperature_verified=True, + temperature_fallback_active=False, + maximum_temperature_c=70, + _pause=paused.append, + ) + O12ThreeCameraCalibrationNode._temperature_callback( + fake, SimpleNamespace(data=[]) + ) + assert paused == [] + assert fake.temperature_verified is False + + O12ThreeCameraCalibrationNode._temperature_callback( + fake, SimpleNamespace(data=[71] * 12) + ) + assert paused == ["o12_over_temperature"] + + +@pytest.mark.skip(reason="unified_engine_v1 removed theoretical live coupling gates") +def test_o12_roll_coupling_envelope_is_task_local(monkeypatch) -> None: + captured: list[frozenset[int]] = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_state_callback", + lambda node, _message: captured.append(node.step_moving_indices), + ) + step = O12MotionStep( + "retry_prepare", "middle_roll_front", 7, 0.26, 0.12 + ) + paused: list[str] = [] + command = [0.0] * 12 + command[8] = 0.17 + fake = SimpleNamespace( + command_count=12, + command_names=COMMAND_NAMES, + baseline_command=(0.0,) * 12, + step_start_feedback_u8=(0.0,) * 12, + step_start_state_u8=(0.0,) * 12, + step_last_command_u8=tuple(command), + step_trajectory_phase=1.0, + # Exercise the exact accepted-sweep -> retry_prepare boundary where + # the next step is selected but has not begun publishing yet. + step_command_sent=False, + step_moving_indices=frozenset({7}), + latest_state_u8=(), + _current_step=lambda: step, + _task=lambda _key: SimpleNamespace( + key="middle_roll_front", auxiliary_commands=((8, 0.17),) + ), + _target_command=lambda _step: tuple(command), + _pause=paused.append, + ) + message = SimpleNamespace(position=[0.0] * 12) + message.position[8] = 0.1793 + message.position[9] = 0.0128 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert captured == [frozenset({7, 8, 9})] + assert fake.step_moving_indices == frozenset({7}) + + message.position[9] = 0.0201 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused and paused[-1].startswith("o12_roll_coupling_exceeded:") + assert captured == [frozenset({7, 8, 9})] + + +@pytest.mark.skip(reason="auxiliary tracking is diagnostic-only in unified_engine_v1") +def test_o12_auxiliary_axis_lag_is_transition_then_timed_settle( + monkeypatch, +) -> None: + captured: list[frozenset[int]] = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_state_callback", + lambda node, _message: captured.append(node.step_moving_indices), + ) + clock = [100.0] + monkeypatch.setattr( + "linkerhand_calibration.models.o12.node.time.monotonic", + lambda: clock[0], + ) + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "index_roll_front" + ) + step = O12MotionStep( + "preflight", task.key, task.command_index, task.start_value, 0.06 + ) + target = list(profile.command.baseline_values) + for index, value in task.auxiliary_commands: + target[index] = value + target[task.command_index] = task.start_value + feedback = list(target) + # Reproduce the live log while the shared trajectory is still moving. + feedback[4] = 0.216 + feedback[5] = 0.139214 + command = list(target) + command[4] = 0.245 + command[5] = 0.160011 + paused: list[str] = [] + fake = SimpleNamespace( + command_count=12, + command_names=COMMAND_NAMES, + step_last_command_u8=tuple(command), + step_start_state_u8=tuple(profile.command.baseline_values), + step_trajectory_phase=0.92, + step_moving_indices=frozenset({4}), + latest_state_u8=(), + motor_stall_timeout_seconds=2.0, + o12_auxiliary_violation_since={}, + _current_step=lambda: step, + _task=lambda _key: task, + _target_command=lambda _step: tuple(target), + _pause=paused.append, + ) + message = SimpleNamespace(position=feedback) + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert fake.latest_o12_auxiliary_tracking["mode"] == "transitioning" + assert captured == [frozenset({4, 5, 6, 7, 8, 9, 10, 11})] + + # At the endpoint the same lag enters a timed settling state rather than + # causing an immediate false stop. + fake.step_trajectory_phase = 1.0 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert fake.latest_o12_auxiliary_tracking["mode"] == "holding" + assert fake.latest_o12_auxiliary_tracking["ready"] is False + + # Normal convergence clears the timer and allows endpoint holding. + clock[0] += 0.3 + message.position[5] = 0.168 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert fake.latest_o12_auxiliary_tracking["ready"] is True + assert 5 not in fake.o12_auxiliary_violation_since + + # A genuine persistent failure still stops after the configured timeout. + message.position[5] = 0.139214 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + clock[0] += 2.1 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused[-1].startswith( + "o12_auxiliary_settle_timeout:task=index_roll_front:" + ) + + +@pytest.mark.skip(reason="endpoint no longer waits for exact auxiliary tracking") +def test_o12_endpoint_hold_waits_for_auxiliary_axes(monkeypatch) -> None: + delegated = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_tick_radian_motion", + lambda _node, _step, _now: delegated.append(True), + ) + monkeypatch.setattr( + O12ThreeCameraCalibrationNode, + "_roll_cross_view_observer", + lambda _node, _step: None, + ) + step = O12MotionStep( + "preflight", "index_roll_front", 4, 0.26, 0.06 + ) + fake = object.__new__(O12ThreeCameraCalibrationNode) + fake.latest_o12_auxiliary_tracking = { + "active": True, + "task_name": "index_roll_front", + "mode": "holding", + "ready": False, + } + fake.step_trajectory_phase = 1.0 + fake.step_hold_since = 10.0 + O12ThreeCameraCalibrationNode._tick_radian_motion(fake, step, 11.0) + assert fake.step_hold_since is None + assert delegated == [] + + fake.latest_o12_auxiliary_tracking["ready"] = True + O12ThreeCameraCalibrationNode._tick_radian_motion(fake, step, 11.1) + assert delegated == [True] + + +@pytest.mark.skip(reason="vendor cross-drive is captured, not registered as a stop gate") +def test_o12_mcp_sweep_registers_vendor_pip_cross_drive(monkeypatch) -> None: + captured: list[frozenset[int]] = [] + monkeypatch.setattr( + L6ThreeCameraCalibrationNode, + "_state_callback", + lambda node, _message: captured.append(node.step_moving_indices), + ) + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_mcp_side" + ) + step = O12MotionStep( + "sweep", task.key, task.command_index, task.end_value, + 0.32, 0, "decreasing", 1, + ) + live_command = list(profile.command.baseline_values) + for index, value in task.auxiliary_commands: + live_command[index] = value + live_command[task.command_index] = 0.403 + feedback = list(live_command) + feedback[task.command_index] = 0.389 + # This value matches the vendor solver's expected non-diagonal + # MCP->PIP round trip and must not be treated as an unrelated motor. + feedback[9] = 0.027529 + paused: list[str] = [] + fake = SimpleNamespace( + command_count=12, + command_names=COMMAND_NAMES, + step_last_command_u8=tuple(live_command), + step_moving_indices=frozenset({8}), + latest_state_u8=(), + raw_records=[], + _current_step=lambda: step, + _task=lambda _key: task, + _target_command=lambda _step: tuple(live_command), + _pause=paused.append, + ) + message = SimpleNamespace(position=feedback) + + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert captured == [frozenset({4, 5, 8, 9, 10, 11})] + assert fake.step_moving_indices == frozenset({8}) + + # Vendor mismatch is training data, not an in-flight stop condition. + message.position[9] = 0.040 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused == [] + assert len(captured) == 2 + + message.position[9] = 0.086 + O12ThreeCameraCalibrationNode._state_callback(fake, message) + assert paused and paused[-1].startswith( + "o12_sdk_feedback_coupling_hard_limit:task=middle_mcp_side:" + ) + assert len(captured) == 2 + + +@pytest.mark.skip(reason="theoretical SDK coupling envelope was removed") +def test_o12_mcp_coupling_uses_three_training_cycles_and_holdout() -> None: + profile = build_typed_profile() + task = next( + item for item in profile.motion.tasks + if item.key == "middle_mcp_side" + ) + contract = SDK_FEEDBACK_COUPLING_BY_TASK[task.key] + raw_records = [] + offsets = (-0.002, 0.0, 0.002, 0.005) + for cycle, offset in enumerate(offsets): + for direction in ("decreasing", "increasing"): + source_values = np.linspace(0.0, 1.326, 80) + if direction == "increasing": + source_values = source_values[::-1] + for source in source_values: + state = [0.0] * 12 + state[8] = float(source) + state[9] = float( + 0.030 * math.sin(math.pi * source / 1.326) + offset + ) + raw_records.append({ + "kind": "o12_joint_sample", + "task_name": task.key, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "joint": task.joints[0], + "trajectory_command_rad": float(source), + "feedback_rad": float(source), + "state_rad": state, + }) + + training = feedback_coupling_metrics( + raw_records, task, contract, 0, "decreasing", attempt=1 + ) + assert training["decision"] == "training" + assert training["failures"] == [] + + holdout = feedback_coupling_metrics( + raw_records, task, contract, 3, "decreasing", attempt=1 + ) + assert holdout["decision"] == "holdout" + assert holdout["failures"] == [] + assert holdout["holdout_residual_p95_rad"] < 0.012 + + for row in raw_records: + if int(row["cycle"]) == 3: + row["state_rad"][9] += 0.030 + failed = feedback_coupling_metrics( + raw_records, task, contract, 3, "decreasing", attempt=1 + ) + assert failed["failures"] == ["coupling_holdout_repeatability"] + + +@pytest.mark.skip(reason="publication validates visual passive coupling instead") +def test_o12_publication_requires_holdout_for_both_mcp_tasks_and_directions( +) -> None: + profile = build_typed_profile() + tasks = { + task.key: task for task in profile.motion.tasks + if task.key in SDK_FEEDBACK_COUPLING_BY_TASK + } + raw_records = [] + for task_name, contract in SDK_FEEDBACK_COUPLING_BY_TASK.items(): + task = tasks[task_name] + for cycle, offset in enumerate((-0.002, 0.0, 0.002, 0.004)): + for direction in ("decreasing", "increasing"): + source_values = np.linspace(0.0, 1.326, 64) + if direction == "increasing": + source_values = source_values[::-1] + for source in source_values: + state = [0.0] * 12 + state[task.command_index] = float(source) + state[contract.coupled_channel_index] = float( + 0.025 * math.sin(math.pi * source / 1.326) + + offset + ) + raw_records.append({ + "kind": "o12_joint_sample", + "task_name": task_name, + "cycle": cycle, + "direction": direction, + "attempt": 1, + "joint": task.joints[0], + "trajectory_command_rad": float(source), + "feedback_rad": float(source), + "state_rad": state, + }) + + quality = session_feedback_coupling_metrics( + raw_records, profile, SDK_FEEDBACK_COUPLING_BY_TASK + ) + assert set(quality) == {"middle_mcp_side", "index_mcp_side"} + assert all( + metrics["passed"] + for task_metrics in quality.values() + for metrics in task_metrics.values() + ) + + raw_records[:] = [ + row for row in raw_records + if not ( + row["task_name"] == "index_mcp_side" + and row["cycle"] == 3 + and row["direction"] == "increasing" + ) + ] + with pytest.raises(ValueError, match="index_mcp_side MCP/PIP coupling"): + session_feedback_coupling_metrics( + raw_records, profile, SDK_FEEDBACK_COUPLING_BY_TASK + ) + + +def test_o12_status_tracking_error_uses_live_trajectory_command( +) -> None: + command = [0.0] * 12 + command[8] = 0.205 + state = list(command) + state[8] = 0.185 + state[9] = 0.016 + fake = SimpleNamespace( + command_count=12, + command_names=COMMAND_NAMES, + latest_state_u8=tuple(state), + step_last_command_u8=tuple(command), + ) + status = { + "target_state_rad": [0.0] * 8 + [1.33, 0.0, 1.38, 1.38], + "maximum_error_channel": "middle_mcp", + "maximum_error_rad": 1.145, + } + status = O12ThreeCameraCalibrationNode._apply_o12_tracking_diagnostics( + fake, status + ) + assert status["target_state_rad"][8] == pytest.approx(1.33) + assert status["maximum_error_channel"] == "middle_mcp" + assert status["maximum_error_rad"] == pytest.approx(0.020) + assert status["channel_errors_rad"][9] == pytest.approx(0.016) def test_o12_operator_progress_uses_unified_layout_and_radians() -> None: @@ -246,9 +2093,49 @@ def test_o12_operator_progress_uses_unified_layout_and_radians() -> None: assert "拇指 CMC pitch(正面 ID0→ID1)" in text assert "阶段:正式扫描(第 2/4 轮)" in text assert "命令/反馈:-0.400 rad/-0.395 rad" in text - assert "峰值 0.060 rad/s;本段 21.7 秒余弦轨迹" in text + assert "峰值 0.060 rad/s;本段 21.7 秒平滑限速轨迹" in text assert "温度策略=错误码 bit1 过热保护;速度倍率=2.0x" in text + roll_status = { + "state": "RUNNING", "serial_number": "O12_TEST", + "step_index": 50, "step_count": 190, "step_fraction": 0.2, + "task_name": "middle_roll_front", "phase": "sweep", + "cycle": 0, "direction": "decreasing", "attempt": 1, + "command_unit": "rad", "target_rad": -0.26, + "current_command_u8": 0.1, "actual_u8": 0.09, + "speed_rad_s": 0.12, "valid_frames": 50, + "total_frames": 50, "tag_detection_rate": 1.0, + "joint_frame_rate": 1.0, "recognized_tag_ids": [0, 12], + "unrecognized_tag_ids": [], + "roll_cross_view": { + "active": True, "recognized_tag_ids": [4, 8], + "unrecognized_tag_ids": [], "valid_frames": 48, + "total_frames": 50, "joint_frame_rate": 0.96, + }, + } + roll_text = render_o12_progress_zh(roll_status) + assert "正面 ID0→ID12 + 侧面 ID4→ID8" in roll_text + assert "侧摆双机位:侧面已识别 ID4/ID8" in roll_text + + roll_status["locked_reference_tag_ids"] = [0] + locked_text = render_o12_progress_zh(roll_status) + assert "固定基准:ID0 已锁定,避让遮挡期间复用" in locked_text + + coupling_status = dict(roll_status) + coupling_status["feedback_coupling"] = { + "active": True, + "coupled_channel": "middle_pip", + "reference_kind": "training_observation", + "displacement_rad": 0.028, + "residual_rad": 0.004, + "hard_displacement_limit_rad": 0.0855, + } + coupling_text = render_o12_progress_zh(coupling_status) + assert ( + "O12耦合:middle_pip 位移 0.028/0.086 rad;" + "训练采集;vendor偏差 0.004 rad(仅诊断)" + ) in coupling_text + def test_o12_synthetic_fit_schema7_bridge_and_ring_preservation(tmp_path: Path) -> None: result = fit_o12_session(SOURCE_URDF, _synthetic_records()) @@ -269,6 +2156,29 @@ def test_o12_synthetic_fit_schema7_bridge_and_ring_preservation(tmp_path: Path) mapper = CalibratedCommandMapper(payload) assert len(mapper.map_positions([0.0] * 12, ["wrong"] * 12)) == 19 + maximum_feedback = [ + SAFE_UPPER_RAD[index] if SDK_TO_URDF_SIGN[index] > 0.0 + else build_typed_profile().command.lower_bounds[index] + for index in range(12) + ] + mapped = dict(zip( + mapper.urdf_joint_names, + mapper.map_positions(maximum_feedback, ["wrong"] * 12), + )) + assert mapped["thumb_cmc_roll"] == pytest.approx(0.720) + assert mapped["thumb_cmc_yaw"] == pytest.approx(0.987) + assert mapped["thumb_cmc_pitch"] == pytest.approx(0.588) + assert mapped["thumb_mcp"] == pytest.approx(1.184) + assert mapped["index_mcp_pitch"] == pytest.approx(1.290) + assert mapped["index_pip"] == pytest.approx(1.635) + assert mapped["middle_mcp_pitch"] == pytest.approx(1.318) + assert mapped["middle_pip"] == pytest.approx(1.604) + assert mapped["ring_mcp_pitch"] == pytest.approx(1.38) + assert mapped["pinky_mcp_pitch"] == pytest.approx(1.472) + # Passive runtime comes from the O12 vendor polynomial rather than the + # planar Tag fit or the linear URDF visualization fallback. + assert mapped["pinky_dip"] == pytest.approx(1.2909331373, abs=1.0e-8) + correction = write_o12_corrected_urdf( source_urdf=SOURCE_URDF, output_directory=tmp_path, @@ -282,3 +2192,170 @@ def test_o12_synthetic_fit_schema7_bridge_and_ring_preservation(tmp_path: Path) by_name_after = {node.get("name"): node for node in after.findall("joint")} for name in ("ring_mcp_pitch", "ring_pip", "ring_dip"): assert ET.tostring(by_name_before[name]) == ET.tostring(by_name_after[name]) + # Rotation-only input has no static phase evidence. Travel differences + # must not create bent/uneven zero poses or a fabricated thumb origin. + assert all(value == 0.0 for value in correction.origin_offsets_rad.values()) + for name in CALIBRATED_ACTIVE_JOINTS: + assert ET.tostring(by_name_before[name].find("origin")) == ET.tostring( + by_name_after[name].find("origin") + ) + validate_o12_runtime_payload_against_urdf(payload, correction.path) + + +def test_o12_vendor_passive_solver_regression() -> None: + assert evaluate_vendor_passive_joint( + "thumb_dip", -1.29, sdk_to_urdf_sign=-1.0 + ) == pytest.approx(1.094445515, abs=1.0e-9) + assert evaluate_vendor_passive_joint( + "pinky_pip", 1.54, sdk_to_urdf_sign=1.0 + ) == pytest.approx(1.360545431, abs=1.0e-9) + assert evaluate_vendor_passive_joint( + "pinky_dip", 1.54, sdk_to_urdf_sign=1.0 + ) == pytest.approx(1.291363184, abs=1.0e-9) + + +def test_o12_rejects_direction_reversal_and_curve_urdf_mismatch( + tmp_path: Path, +) -> None: + result = fit_o12_session(SOURCE_URDF, _synthetic_records()) + hashes = { + name: "a" * 64 + for name in build_typed_profile().artifacts.protected_input_fields + } + payload = build_o12_runtime_payload( + serial_number="O12_TEST", + source_urdf=SOURCE_URDF, + result=result, + protected_inputs=hashes, + passed=True, + ) + payload["joints"]["thumb_cmc_roll"]["angle_rad"][-1] = 0.54 + with pytest.raises(ValueError, match="SDK/URDF direction"): + validate_o12_runtime_payload(payload) + + payload = build_o12_runtime_payload( + serial_number="O12_TEST", + source_urdf=SOURCE_URDF, + result=result, + protected_inputs=hashes, + passed=True, + ) + payload["joints"]["thumb_dip"]["angle_rad"][-1] = 2.47 + correction = write_o12_corrected_urdf( + source_urdf=SOURCE_URDF, + output_directory=tmp_path, + serial_number="O12_TEST", + result=result, + timestamp="20260904_120001", + ) + with pytest.raises(ValueError, match="above the URDF physical limit"): + validate_o12_runtime_payload_against_urdf(payload, correction.path) + + +def test_o12_fit_does_not_require_fake_u8_endpoints() -> None: + records = _synthetic_records() + profile = build_typed_profile() + task_by_joint = { + joint: task for task in profile.motion.tasks for joint in task.joints + } + for joint, rows in records.items(): + task = task_by_joint[joint] + for row in rows: + phase = ( + float(row["feedback_rad"]) - task.start_value + ) / (task.end_value - task.start_value) + observed_phase = 0.02 + 0.96 * phase + row["feedback_rad"] = float( + task.start_value + + observed_phase * (task.end_value - task.start_value) + ) + + result = fit_o12_session(SOURCE_URDF, records) + + assert result.curves + assert all(math.isfinite(value) for value in result.travels_rad.values()) + + +def test_o12_holdout_endpoint_dwell_has_one_curve_bin_weight() -> None: + rows = [ + {"direction": "decreasing", "command_u8": 0} + for _ in range(40) + ] + errors = [math.radians(2.3)] * 40 + for command in range(1, 65): + rows.append({"direction": "decreasing", "command_u8": command}) + errors.append(math.radians(0.2)) + + uniform = np.degrees(np.abs( + _uniform_curve_holdout_errors(rows, errors) + )) + + assert len(uniform) == 65 + assert np.percentile(uniform, 95) == pytest.approx(0.2) + assert np.max(uniform) == pytest.approx(2.3) + + +def test_o12_fit_rejects_a_session_that_did_not_scan_the_full_sdk_domain() -> None: + records = _synthetic_records() + for row in records["thumb_cmc_yaw"]: + if int(row["cycle"]) > 0: + row["feedback_rad"] = 0.8 * float(row["feedback_rad"]) + + with pytest.raises(ValueError, match="repeats only.*cycle-zero"): + fit_o12_session(SOURCE_URDF, records) + + +def test_o12_endpoint_zero_is_written_to_the_urdf_origin( + tmp_path: Path, +) -> None: + fitted = fit_o12_session(SOURCE_URDF, _synthetic_records()) + offsets = dict(fitted.zero_offsets_rad) + offsets["thumb_cmc_yaw"] = -0.1 + fitted = replace(fitted, zero_offsets_rad=offsets) + hashes = { + name: "a" * 64 + for name in build_typed_profile().artifacts.protected_input_fields + } + payload = build_o12_runtime_payload( + serial_number="O12_TEST", + source_urdf=SOURCE_URDF, + result=fitted, + protected_inputs=hashes, + passed=True, + ) + correction = write_o12_corrected_urdf( + source_urdf=SOURCE_URDF, + output_directory=tmp_path, + serial_number="O12_TEST", + result=fitted, + timestamp="20260904_120002", + ) + before = ET.parse(SOURCE_URDF).getroot() + after = ET.parse(correction.path).getroot() + before_joint = before.find("joint[@name='thumb_cmc_yaw']/origin") + after_joint = after.find("joint[@name='thumb_cmc_yaw']/origin") + assert before_joint is not None and after_joint is not None + assert before_joint.get("rpy") != after_joint.get("rpy") + assert correction.origin_offsets_rad["thumb_cmc_yaw"] == pytest.approx(-0.1) + validate_o12_runtime_payload_against_urdf(payload, correction.path) + + +def test_o12_synthetic_roll_cross_view_is_independently_fitted() -> None: + records = _synthetic_records() + cross_view = { + joint: records[joint] + for joint in ("middle_mcp_roll", "index_mcp_roll") + } + result = fit_o12_session( + SOURCE_URDF, + records, + cross_view_records_by_joint=cross_view, + require_cross_view=True, + ) + assert set(result.cross_view_roll_metrics) == { + "middle_mcp_roll", "index_mcp_roll" + } + assert all( + metrics["holdout_max_rad"] < 1.0e-6 + for metrics in result.cross_view_roll_metrics.values() + ) diff --git a/src/linkerhand_calibration/test/test_o12_static_geometry.py b/src/linkerhand_calibration/test/test_o12_static_geometry.py new file mode 100644 index 0000000..2439c07 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_static_geometry.py @@ -0,0 +1,156 @@ +"""Absolute zero and transfer tests, independent of curve-fit repeatability.""" + +from dataclasses import replace +import math +import xml.etree.ElementTree as ET + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.models.o12.fitting import ( + O12_THUMB_ROOT_AXIS_JOINTS, _fit_thumb_root_zero, fit_o12_session, +) +from linkerhand_calibration.models.o12.profile import build_typed_profile +from linkerhand_calibration.models.o12.artifacts import ( + build_o12_runtime_payload, validate_o12_runtime_payload, + validate_o12_runtime_payload_against_urdf, +) +from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf +from test_o12_right_profile import SOURCE_URDF, _synthetic_records + + +def _transform(xyz, rpy): + t = np.eye(4) + t[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + t[:3, 3] = xyz + return t + + +def _pose(t): + return {"translation_xyz_m": t[:3, 3].tolist(), + "quaternion_xyzw": Rotation.from_matrix(t[:3, :3]).as_quat().tolist()} + + +@pytest.fixture(scope="module") +def rotation_fit(): + return fit_o12_session(SOURCE_URDF, _synthetic_records()) + + +def _geometry_records(fit, offsets, camera_rpy): + model = UrdfKinematicModel(SOURCE_URDF) + tasks = {j: t for t in build_typed_profile().motion.tasks for j in t.joints} + palm = _transform([.02, -.01, .7], camera_rpy) + records = {} + for i, name in enumerate(O12_THUMB_ROOT_AXIS_JOINTS): + task = tasks[name] + mounting = _transform([.012, -.009, .02], [.13 * i, -.24, .31]) + parent = palm @ _transform([-.01, .007, .003], [-.12, .1 * i, .2]) + rows = [] + for cycle in range(4): + for direction, phases in (("decreasing", np.linspace(0, 1, 65)), + ("increasing", np.linspace(1, 0, 65))): + for phase in phases: + state = [0.] * 12 + state[task.command_index] = task.start_value + phase * (task.end_value - task.start_value) + child = palm @ model.link_transform( + name, zero_offsets=offsets, + joint_angles={name: float(phase * fit.travels_rad[name])}, + ) @ mounting + relative = np.linalg.inv(parent) @ child + rows.append({ + "cycle": cycle, "direction": direction, + "feedback_rad": state[task.command_index], "state_rad": state, + "relative_quaternion_xyzw": _pose(relative)["quaternion_xyzw"], + "relative_translation_xyz_m": relative[:3, 3].tolist(), + "parent_pose_common": _pose(parent), "child_pose_common": _pose(child), + "view_normal_common_xyz": [0., 0., 1.], + "camera_center_common_xyz_m": [0., 0., 0.], + }) + records[name] = rows + return records + + +@pytest.mark.parametrize("camera_rpy", [[.2, -.3, .4], [-.3, .5, -.6]]) +def test_root_zeros_recover_known_geometry_with_arbitrary_tag_mounts(rotation_fit, camera_rpy, tmp_path): + truth = {"thumb_cmc_roll": .08, "thumb_cmc_yaw": -.12} + rows = _geometry_records(rotation_fit, truth, camera_rpy) + solved = _fit_thumb_root_zero(SOURCE_URDF, rows, rotation_fit.curves, + rotation_fit.feedback_domains_rad) + assert solved.passed + assert set(solved.direct_offsets_rad) == set(truth) + for name, value in truth.items(): + assert solved.direct_offsets_rad[name] == pytest.approx(value, abs=1.e-5) + result = replace(rotation_fit, zero_offsets_rad={ + **rotation_fit.zero_offsets_rad, **solved.direct_offsets_rad, + }) + corrected = write_o12_corrected_urdf(source_urdf=SOURCE_URDF, + output_directory=tmp_path, serial_number="ROOT", result=result) + actual, original = UrdfKinematicModel(corrected.path), UrdfKinematicModel(SOURCE_URDF) + for state in ({}, {"thumb_cmc_roll": .72}, + {"thumb_cmc_roll": .36, "thumb_cmc_yaw": .4, "thumb_cmc_pitch": .3, "thumb_mcp": .5}): + assert actual.link_transform("thumb_mcp", zero_offsets={}, joint_angles=state) == pytest.approx( + original.link_transform("thumb_mcp", zero_offsets=truth, joint_angles=state), abs=1.e-5) + + +def test_root_holdout_rejects_changed_camera_pose(rotation_fit): + rows = _geometry_records(rotation_fit, {"thumb_cmc_roll": .08, "thumb_cmc_yaw": -.12}, [.2, -.3, .4]) + rotation = Rotation.from_rotvec([.15, .05, .1]) + for row in rows["thumb_cmc_yaw"]: + if row["cycle"] == 3: + for field in ("parent_pose_common", "child_pose_common"): + p = row[field] + p["translation_xyz_m"] = rotation.apply(p["translation_xyz_m"]).tolist() + p["quaternion_xyzw"] = (rotation * Rotation.from_quat(p["quaternion_xyzw"])).as_quat().tolist() + with pytest.raises(ValueError, match="spatial zero solve failed"): + _fit_thumb_root_zero(SOURCE_URDF, rows, rotation_fit.curves, + rotation_fit.feedback_domains_rad) + + +def test_rotation_only_records_cannot_pass_production_static_solve(): + with pytest.raises(ValueError, match="absolute zero requires"): + fit_o12_session(SOURCE_URDF, _synthetic_records(), require_thumb_root_spatial_zero=True) + + +def test_ring_transfers_nonzero_correction_without_copying_geometry(rotation_fit, tmp_path): + offsets = {**rotation_fit.zero_offsets_rad, "pinky_mcp_pitch": .06, "ring_mcp_pitch": .06} + result = replace(rotation_fit, zero_offsets_rad=offsets) + corrected = write_o12_corrected_urdf(source_urdf=SOURCE_URDF, + output_directory=tmp_path, serial_number="TRANSFER", result=result) + original = ET.parse(SOURCE_URDF).getroot() + actual = ET.parse(corrected.path).getroot() + for name in ("pinky_mcp_pitch", "ring_mcp_pitch"): + before = original.find(f"joint[@name='{name}']/origin") + after = actual.find(f"joint[@name='{name}']/origin") + assert before.get("xyz") == after.get("xyz") + rb = Rotation.from_euler("xyz", [float(x) for x in before.get("rpy").split()]) + ra = Rotation.from_euler("xyz", [float(x) for x in after.get("rpy").split()]) + assert (rb.inv() * ra).as_rotvec() == pytest.approx([0., .06, 0.], abs=1.e-10) + for name in ("ring_pip", "ring_dip"): + assert ET.tostring(original.find(f"joint[@name='{name}']")) == ET.tostring(actual.find(f"joint[@name='{name}']")) + hashes = {k: "a" * 64 for k in build_typed_profile().artifacts.protected_input_fields} + payload = build_o12_runtime_payload(serial_number="TRANSFER", source_urdf=SOURCE_URDF, + result=result, protected_inputs=hashes, passed=True) + ring = payload["joints"]["ring_mcp_pitch"] + pinky = payload["joints"]["pinky_mcp_pitch"] + # Equal feedback must have equal corrections before the ring saturates. + for i, value in enumerate(pinky["angle_rad"]): + assert ring["angle_rad"][i] == pytest.approx(min(1.38, value)) + validate_o12_runtime_payload_against_urdf(payload, corrected.path, source_urdf=SOURCE_URDF) + payload["joints"]["ring_mcp_pitch"]["static_urdf_origin_offset_rad"] = 0. + with pytest.raises(ValueError, match="static zero differs"): + validate_o12_runtime_payload(payload) + + +def test_static_frame_validator_catches_correct_limits_but_wrong_origin(rotation_fit, tmp_path): + corrected = write_o12_corrected_urdf(source_urdf=SOURCE_URDF, + output_directory=tmp_path, serial_number="FRAME", result=rotation_fit) + hashes = {k: "a" * 64 for k in build_typed_profile().artifacts.protected_input_fields} + payload = build_o12_runtime_payload(serial_number="FRAME", source_urdf=SOURCE_URDF, + result=rotation_fit, protected_inputs=hashes, passed=True) + tree = ET.parse(corrected.path) + tree.getroot().find("joint[@name='thumb_cmc_pitch']/origin").set("rpy", "0 0.9259 -1.5708") + tree.write(corrected.path) + with pytest.raises(ValueError, match="thumb_cmc_pitch static origin disagrees"): + validate_o12_runtime_payload_against_urdf(payload, corrected.path, source_urdf=SOURCE_URDF) diff --git a/src/linkerhand_calibration/test/test_o12_thumb_pnp.py b/src/linkerhand_calibration/test/test_o12_thumb_pnp.py new file mode 100644 index 0000000..d810799 --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_thumb_pnp.py @@ -0,0 +1,124 @@ +import math +from types import SimpleNamespace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation as R + +from linkerhand_calibration.models.o12.pnp import O12ThumbPoseTracker, THUMB_ROLES +from linkerhand_calibration.pnp import SquareTagPose + + +def pose(rotation, error=0.05): + return SquareTagPose(tuple(rotation.as_quat()), (0., 0., 1.), error) + + +@pytest.mark.parametrize('remount', [False, True]) +@pytest.mark.parametrize('passive_travel', [12, 28, 45]) +def test_parallel_branch_disambiguation_without_angle_ratio(remount, passive_travel): + tracker=O12ThumbPoseTracker() + mounts=[R.identity()]*3 if not remount else [ + R.from_euler('xyz', angles) for angles in + ([.3,-.7,.2],[-.5,.4,.8],[.6,.2,-.4])] + camera=R.from_euler('xyz',[.4,-.8,.7]) + baseline={role:(pose(camera*mount),) for role,mount in zip(THUMB_ROLES,mounts)} + for i in range(8): + selected,reason=tracker.select(baseline,stamp_ns=1_000_000_000+i*30_000_000) + assert selected is not None + driver=R.from_euler('z',20,degrees=True) + follower=R.from_euler('z',passive_travel,degrees=True) + wrong=R.from_euler('x',passive_travel,degrees=True) + good_pose=pose(camera*driver*follower*mounts[2],.08) + candidates={THUMB_ROLES[0]:baseline[THUMB_ROLES[0]], + THUMB_ROLES[1]:(pose(camera*driver*mounts[1]),), + THUMB_ROLES[2]:(pose(camera*driver*wrong*mounts[2]),good_pose)} + selected,reason=tracker.select(candidates,stamp_ns=1_250_000_000) + assert reason == '' + assert selected[THUMB_ROLES[2]] == good_pose + # Angles are observed candidates, not replaced with driver*mimic. + assert tracker.coupled_rotation_pairs == () + + +def test_no_credible_parallel_candidate_does_not_stop_or_invent_pose(): + tracker=O12ThumbPoseTracker() + baseline={role:(pose(R.identity()),) for role in THUMB_ROLES} + for i in range(8): + tracker.select(baseline,stamp_ns=i*30_000_000) + bad=pose(R.from_euler('zx',[20,45],degrees=True)) + selected,reason=tracker.select({THUMB_ROLES[0]:baseline[THUMB_ROLES[0]], + THUMB_ROLES[1]:(pose(R.from_euler('z',20,degrees=True)),), + THUMB_ROLES[2]:(bad,)},stamp_ns=250_000_000) + assert reason == '' + assert selected[THUMB_ROLES[2]] == bad + tracker.reset() + assert tracker.reference is None + + +def test_missing_tag_discards_observation_and_gap_reinitializes(): + tracker=O12ThumbPoseTracker() + baseline={role:(pose(R.identity()),) for role in THUMB_ROLES} + for i in range(8): + tracker.select(baseline,stamp_ns=i*30_000_000) + selected,reason=tracker.select({},stamp_ns=250_000_000) + assert selected is None and reason == 'group_missing_pose_candidates' + selected,_=tracker.select(baseline,stamp_ns=10_000_000_000) + assert selected is None and tracker.reference is None + + +def test_hook_only_applies_to_o12_thumb_task(): + from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode + from linkerhand_calibration.models.l6.node import L6ThreeCameraCalibrationNode + from linkerhand_calibration.models.o6.node import O6ThreeCameraCalibrationNode + assert not hasattr(L6ThreeCameraCalibrationNode,'_select_articulated_capture_poses') + assert not hasattr(O6ThreeCameraCalibrationNode,'_select_articulated_capture_poses') + hook=O12ThreeCameraCalibrationNode._select_articulated_capture_poses + assert hook(SimpleNamespace(),'side',None,(),{},0) is None + assert hook(SimpleNamespace(),'front',SimpleNamespace(task_key='thumb_pitch_front'),(),{},0) is None + + +def test_hook_records_corners_all_candidates_and_rejected_frames(monkeypatch, tmp_path): + import json + from linkerhand_calibration.models.o12 import node as module + from linkerhand_calibration.models.o12.profile import build_typed_profile + profile=build_typed_profile() + front=next(v for v in profile.vision.views if v.name == 'front') + good=pose(R.identity()) + high_error=pose(R.from_euler('y',.5),3.) + monkeypatch.setattr(module,'solve_square_tag_ippe',lambda *a,**k:[good,high_error]) + raw=tmp_path/'raw.jsonl' + node=SimpleNamespace(trackers={'front':SimpleNamespace(maximum_reprojection_error_px=1.5)}, + camera_matrices={'front':np.diag([500.,500.,1.])},tag_size_m=.016, + raw_path=raw,_view=lambda view:front) + step=SimpleNamespace(task_key='thumb_mcp_dip_front',cycle=0,direction='decreasing',attempt=1,phase='sweep') + corners={role:np.array([[1.,1.],[2.,1.],[2.,2.],[1.,2.]]) for role in THUMB_ROLES} + hook=module.O12ThreeCameraCalibrationNode._select_articulated_capture_poses + for i in range(8): + selected,reason=hook(node,'front',step,THUMB_ROLES,corners,i*30_000_000) + assert selected is not None + records=[json.loads(line) for line in raw.read_text().splitlines()] + assert len(records)==8 + assert records[0]['roles']['thumb_dip']['selected'] is None + assert len(records[-1]['roles']['thumb_dip']['candidates'])==2 + assert records[-1]['roles']['thumb_dip']['eligible_candidate_count']==1 + assert records[-1]['roles']['thumb_dip']['tag_id']==3 + assert records[-1]['camera_matrix']==node.camera_matrices['front'].tolist() + assert records[-1]['roles']['thumb_mcp']['corners_xy']==corners['thumb_mcp'].tolist() + + +def test_other_task_evidence_distinguishes_locked_reference_from_pixels(tmp_path): + import json + from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode + roles=('front_base','moving') + poses={r:pose(R.identity()) for r in roles} + node=SimpleNamespace(raw_path=tmp_path/'raw.jsonl',tag_size_m=.016, + camera_matrices={'front':np.eye(3)}, + trackers={'front':SimpleNamespace(last_candidates_by_role={r:(p,) for r,p in poses.items()},maximum_reprojection_error_px=1.5)}, + _view=lambda v:SimpleNamespace(tags=[SimpleNamespace(role=r,tag_id=i) for i,r in enumerate(roles)])) + step=SimpleNamespace(task_key='index_roll_front',cycle=0,direction='decreasing',attempt=1,phase='sweep') + corners={r:np.zeros((4,2)) for r in roles} + O12ThreeCameraCalibrationNode._record_capture_pose_evidence(node,'front',step,roles,corners,poses,1,locked_roles=('front_base',)) + record=json.loads(node.raw_path.read_text()) + assert record['camera_matrix_source']=='CameraInfo.P[:3,:3]' + assert record['roles']['front_base']['observation_source']=='locked_reference' + assert record['roles']['front_base']['candidates']==[] + assert record['roles']['moving']['observation_source']=='image' diff --git a/src/linkerhand_calibration/test/test_o6_right_profile.py b/src/linkerhand_calibration/test/test_o6_right_profile.py index 81b3fa0..8d1c665 100644 --- a/src/linkerhand_calibration/test/test_o6_right_profile.py +++ b/src/linkerhand_calibration/test/test_o6_right_profile.py @@ -208,7 +208,7 @@ def test_o6_motion_plan_uses_o6_specific_speed_tiers() -> None: steps = O6ThreeCameraCalibrationNode._build_steps(fake) assert steps[0].phase == "baseline" assert steps[0].speed_u8 == 80 - assert {step.speed_u8 for step in steps if step.phase == "preflight"} == {60} + assert not [step for step in steps if step.phase == "preflight"] assert { step.speed_u8 for step in steps diff --git a/src/linkerhand_calibration/test/test_rectified_camera_contract.py b/src/linkerhand_calibration/test/test_rectified_camera_contract.py new file mode 100644 index 0000000..71dd358 --- /dev/null +++ b/src/linkerhand_calibration/test/test_rectified_camera_contract.py @@ -0,0 +1,43 @@ +from types import SimpleNamespace +import numpy as np +import pytest +import cv2 +from scipy.spatial.transform import Rotation as R + +from linkerhand_calibration.core.geometry.camera import rectified_camera_matrix +from linkerhand_calibration.pnp import solve_square_tag_ippe, square_object_points + + +def test_rectified_corners_require_projection_not_raw_intrinsics(): + k=np.array([[3816.108989,0,620.129591],[0,3796.2534,570.26288],[0,0,1.]]) + p=np.array([[3792.732422,0,606.014017,0],[0,3825.369629,568.524418,0],[0,0,1,0.]]) + expected=R.from_euler('xyz',[.2,-.5,.3]) + t=np.array([.12,.05,1.]) + corners=cv2.projectPoints(square_object_points(.016),expected.as_rotvec(),t,p[:,:3],np.zeros(4))[0].reshape(4,2) + candidates=solve_square_tag_ippe(corners,tag_size_m=.016,camera_matrix=rectified_camera_matrix(p)) + errors=[(expected.inv()*R.from_quat(c.quaternion_xyzw)).magnitude() for c in candidates] + assert min(errors)<1e-6 + wrong=solve_square_tag_ippe(corners,tag_size_m=.016,camera_matrix=k) + assert min((expected.inv()*R.from_quat(c.quaternion_xyzw)).magnitude() for c in wrong)>.001 + + +@pytest.mark.parametrize('bad',[[],[0.]*12,[float('nan')]*12]) +def test_invalid_p_cannot_silently_fall_back_to_k(bad): + with pytest.raises(ValueError):rectified_camera_matrix(bad) + + +def test_shared_camera_callback_uses_p_saves_provenance_and_invalidates(tmp_path): + import json + from linkerhand_calibration.models.l6.node import L6ThreeCameraCalibrationNode + node=SimpleNamespace(camera_matrices={},image_sizes={},raw_path=tmp_path/'raw.jsonl') + p=[500.,0,320,0,0,510,240,0,0,0,1,0] + msg=SimpleNamespace(p=p,k=[600.,0,300,0,620,220,0,0,1],d=[.1]*5,r=np.eye(3).ravel().tolist(),width=640,height=480) + callback=L6ThreeCameraCalibrationNode._camera_info_callback + callback(node,'front',msg); callback(node,'front',msg) + np.testing.assert_allclose(node.camera_matrices['front'],np.array(p).reshape(3,4)[:,:3]) + records=[json.loads(l) for l in node.raw_path.read_text().splitlines()] + assert len(records)==1 and records[0]['raw_k']==msg.k + assert records[0]['matrix_source']=='CameraInfo.P[:3,:3]' + msg.p=[0.]*12 + callback(node,'front',msg) + assert 'front' not in node.camera_matrices diff --git a/src/linkerhand_calibration/test/test_three_camera_retry.py b/src/linkerhand_calibration/test/test_three_camera_retry.py index 3eeb51c..6d07df1 100644 --- a/src/linkerhand_calibration/test/test_three_camera_retry.py +++ b/src/linkerhand_calibration/test/test_three_camera_retry.py @@ -235,16 +235,14 @@ def test_refit_invalidates_all_derived_calibration_artifacts() -> None: def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None: plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4) - assert len(plan) == len(RIGHT_19_HAND_PROFILE.sweep_specs) * 10 + assert len(plan) == len(RIGHT_19_HAND_PROFILE.sweep_specs) * 8 for spec_index, spec in enumerate(RIGHT_19_HAND_PROFILE.sweep_specs): - task = plan[spec_index * 10 : (spec_index + 1) * 10] + task = plan[spec_index * 8 : (spec_index + 1) * 8] assert all(item.spec == spec for item in task) assert [ (item.precheck, item.cycle, item.direction) for item in task ] == [ - (True, -1, DIRECTION_DECREASING), - (True, -1, DIRECTION_INCREASING), *[ (False, cycle, direction) for cycle in range(4) @@ -259,8 +257,6 @@ def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None: for left, right in zip(task, task[1:]) ] assert transitions == [ - "immediate_reverse", - "immediate_reverse", "immediate_reverse", "cycle_reset", "immediate_reverse", @@ -271,7 +267,7 @@ def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None: ] task_boundaries = [ (plan[index], plan[index + 1]) - for index in range(9, len(plan) - 1, 10) + for index in range(7, len(plan) - 1, 8) ] assert all( _sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right) @@ -337,8 +333,8 @@ def test_right_19_complete_plan_has_no_unobserved_immediate_handoff() -> None: minimum_per_joint=1, ) - # 16 tasks × (precheck out/back, precheck->formal, four formal reversals). - assert immediate_boundaries == 16 * 6 + # Four formal decreasing/increasing pairs per task; no full-range precheck. + assert immediate_boundaries == 16 * 4 assert cycle_resets == 16 * 3 assert task_changes == 15 @@ -356,8 +352,7 @@ def test_right_19_initializes_pnp_once_per_normal_task() -> None: ] assert len(reset_items) == len(RIGHT_19_HAND_PROFILE.sweep_specs) - assert all(item.precheck for item in reset_items) - assert all(item.cycle == -1 for item in reset_items) + assert all(not item.precheck and item.cycle == 0 for item in reset_items) assert all( item.direction == DIRECTION_DECREASING for item in reset_items ) @@ -2960,7 +2955,7 @@ def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None assert event["cycles"] == [1] -def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> None: +def test_recoverable_sweep_failure_retries_once_at_same_speed(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) item = SweepItem(spec, 0, DIRECTION_INCREASING) transitions: list[str] = [] @@ -2975,8 +2970,8 @@ def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> _publish_hold_current=lambda: holds.append(True), _begin_return_baseline=lambda after: transitions.append(after), _pause=lambda reason: pauses.append(reason), - retry_speed_scales=(0.8, 0.6, 0.5), - retry_endpoint_hold_seconds=(0.75, 1.0, 1.25), + retry_speed_scales=(1.0,), + retry_endpoint_hold_seconds=(0.5,), ) G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( @@ -2985,22 +2980,15 @@ def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( node, "sweep_bin_gap_too_large" ) - G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( - node, "sweep_bin_gap_too_large" - ) - G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( - node, "sweep_bin_gap_too_large" - ) - - assert transitions == ["retry_sweep", "retry_sweep", "retry_sweep"] + assert transitions == ["retry_sweep"] assert pauses == ["sweep_bin_gap_too_large"] assert holds == [] - assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 3 + assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 1 events = [ json.loads(line) for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines() ] - assert [event["retry"] for event in events] == [1, 2, 3] + assert [event["retry"] for event in events] == [1] def test_visibility_precheck_does_not_require_dense_feedback_bins(tmp_path) -> None: @@ -3209,7 +3197,7 @@ def test_formal_sweep_still_rejects_17_u8_feedback_gap(tmp_path) -> None: assert retries == ["sweep_bin_gap_too_large"] -def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None: +def test_first_provisional_fit_failure_stops_without_more_motion(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) transitions: list[str] = [] pauses: list[str] = [] @@ -3240,10 +3228,9 @@ def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None: [{"joint": "thumb_ip", "metric": "rotation_orthogonal_rms_deg"}], ) - assert prepared == [True] - assert transitions == ["resume_sweep"] - assert pauses == [] - assert node.paused_reason == "joint_fit_check_failed" + assert prepared == [] + assert transitions == [] + assert pauses == ["joint_fit_check_failed"] def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry( @@ -3290,10 +3277,7 @@ def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry( assert node.fit_failure["directions_to_rescan"] == 0 -def test_near_threshold_provisional_failure_rescans_in_place(tmp_path) -> None: - # In-band values are rejected by the final fit on the same records, so - # the first in-band result reschedules the task here instead of warning - # and deferring the rejection to the end of the session. +def test_near_threshold_provisional_failure_stops_without_rescan(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) node = SimpleNamespace( raw_path=tmp_path / "raw_samples.jsonl", @@ -3333,11 +3317,11 @@ def test_near_threshold_provisional_failure_rescans_in_place(tmp_path) -> None: for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines() ] kinds = [event["kind"] for event in events] - assert "provisional_fit_warning_rescan" in kinds + assert "provisional_fit_warning_retry_exhausted" in kinds assert "fit_failure" in kinds -def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None: +def test_motion_timeout_pauses_without_republishing(tmp_path) -> None: commands: list[list[int]] = [] pauses: list[str] = [] holds: list[bool] = [] @@ -3363,12 +3347,12 @@ def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None: node, "return_baseline_timeout", now ) - assert len(commands) == 2 + assert commands == [] assert holds == [] - assert pauses == ["return_baseline_timeout"] + assert pauses == ["return_baseline_timeout"] * 3 -def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None: +def test_sweep_start_tag_timeout_does_not_block_motion(tmp_path) -> None: spec = next( item for item in RIGHT_19_HAND_PROFILE.sweep_specs @@ -3379,6 +3363,8 @@ def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None: resets: list[tuple[object, bool]] = [] diagnostic_resets: list[object] = [] commands: list[list[int]] = [] + starts: list[float] = [] + pauses: list[str] = [] start_frames = [object()] node = SimpleNamespace( profile=RIGHT_19_HAND_PROFILE, @@ -3406,22 +3392,18 @@ def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None: _publish_speed_profile=lambda profile: None, _publish_command=lambda command: commands.append(command), _reset_motion_progress=lambda now, error: None, - _pause=lambda reason: pytest.fail(f"unexpected pause: {reason}"), + _pause=lambda reason: pauses.append(reason), + _begin_active_sweep=lambda now: starts.append(now), ) - G20ThreeCameraCalibrationNode._retry_motion_or_pause( - node, "sweep_start_tag_timeout", 20.0 + G20ThreeCameraCalibrationNode._handle_sweep_start_timeout( + node, 20.0, True ) - assert resets == [(runtime, True)] - assert diagnostic_resets == [runtime] - assert runtime.pnp_invalid_since is None - assert runtime.pnp_reset_count == 1 - assert start_frames == [] - assert len(commands) == 1 + assert starts == [20.0] + assert pauses == [] event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["pnp_trackers_reset"] == ["side"] - assert event["task_reference_preserved"] is True + assert event["kind"] == "sweep_start_vision_timeout_warning" def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None: @@ -3453,7 +3435,7 @@ def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None: assert event["context"] == "sweep_motor_0" -def test_slower_sweep_retry_extends_timeout_inversely() -> None: +def test_same_speed_sweep_retry_keeps_timeout() -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) item = SweepItem(spec, 0, DIRECTION_DECREASING) key = (spec.motor_index, 0, DIRECTION_DECREASING) @@ -3466,11 +3448,11 @@ def test_slower_sweep_retry_extends_timeout_inversely() -> None: assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds( node - ) == 112.5 + ) == 90.0 node.sweep_retry_counts[key] = 3 assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds( node - ) == 180.0 + ) == 90.0 def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None: @@ -3726,7 +3708,7 @@ def test_thumb_yaw_zero_outlier_retries_only_localized_source_cycle( "thumb_cmc_pitch_front", "thumb_cmc_roll_front", ] - assert calls == ["prepare", "resume_sweep"] + assert calls == ["pause:joint_fit_check_failed"] def test_previous_passed_joint_zero_offset_reads_formal_pointer( @@ -5327,7 +5309,7 @@ def _warning_band_node(tmp_path, attempts: dict) -> SimpleNamespace: return node -def test_warning_band_failure_rescans_once_in_place(tmp_path) -> None: +def test_warning_band_failure_stops_in_place(tmp_path) -> None: node = _warning_band_node(tmp_path, attempts={}) failures = [ { @@ -5343,22 +5325,19 @@ def test_warning_band_failure_rescans_once_in_place(tmp_path) -> None: node, node._spec, failures, allow_warning=True ) - # The final fit rejects 0.779 against 0.75 on the same records, so the - # first in-band result must rescan here instead of deferring the - # rejection to the end of the session. assert paused is True - assert "prepare_retry" in node._calls + assert node._calls == ["pause_joint_fit_check_failed"] rows = [ json.loads(line) for line in node.raw_path.read_text().splitlines() ] kinds = {row["kind"] for row in rows} - assert "provisional_fit_warning_rescan" in kinds + assert "provisional_fit_warning_retry_exhausted" in kinds assert "provisional_fit_warning" not in kinds assert "fit_failure" in kinds -def test_second_warning_band_result_retries_immediately(tmp_path) -> None: +def test_second_warning_band_result_also_stops_without_motion(tmp_path) -> None: spec = next( item for item in RIGHT_19_HAND_PROFILE.sweep_specs @@ -5382,17 +5361,17 @@ def test_second_warning_band_result_retries_immediately(tmp_path) -> None: ) assert paused is True - assert node._calls == ["prepare_retry", "return_resume_sweep"] + assert node._calls == ["pause_joint_fit_check_failed"] rows = [ json.loads(line) for line in node.raw_path.read_text().splitlines() ] assert [row["kind"] for row in rows] == [ - "provisional_fit_warning_rescan", + "provisional_fit_warning_retry_exhausted", "fit_failure", ] assert rows[0]["attempt"] == 2 - assert rows[0]["attempt_limit"] == 3 + assert rows[0]["attempt_limit"] == 1 def test_third_warning_band_result_stops_at_current_joint(tmp_path) -> None: @@ -5427,7 +5406,7 @@ def test_third_warning_band_result_stops_at_current_joint(tmp_path) -> None: "fit_failure", ] assert rows[0]["attempt"] == 3 - assert rows[0]["attempt_limit"] == 3 + assert rows[0]["attempt_limit"] == 1 def test_repeated_branch_clusters_stop_before_third_full_rescan(tmp_path) -> None: @@ -5459,7 +5438,7 @@ def test_repeated_branch_clusters_stop_before_third_full_rescan(tmp_path) -> Non allow_warning=True, ) assert paused is True - assert node._calls == ["prepare_retry", "return_resume_sweep"] + assert node._calls == ["pause_joint_fit_check_failed"] node._calls.clear() node.sweep_attempts[node._spec.key] = 2 diff --git a/src/linkerhand_calibration/test/test_unified_engine.py b/src/linkerhand_calibration/test/test_unified_engine.py new file mode 100644 index 0000000..cdc7e4b --- /dev/null +++ b/src/linkerhand_calibration/test/test_unified_engine.py @@ -0,0 +1,326 @@ +from __future__ import annotations + +import math +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from linkerhand_calibration.models import get_default_registry +from linkerhand_calibration.core import ( + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + QualityPolicy, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, +) +from linkerhand_calibration.core.urdf import ( + UrdfJointPatch, + UrdfPatchSet, + write_urdf_patches, +) +from linkerhand_calibration.runtime import ( + ACQUISITION_POLICY_VERSION, + CalibrationEngine, +) +from linkerhand_calibration.runtime.adapters import ProfileSdkAdapter +from linkerhand_calibration.core.geometry.rotation import fit_rotation_axis +from linkerhand_calibration.models.g20.profile import JointCurveFit +from linkerhand_calibration.models.l6.fitting import fit_coupling_model + + +def test_all_product_profiles_use_unified_engine_and_sdk_adapter() -> None: + registry = get_default_registry() + products = { + registered.profile.key.model: registered + for registered in registry + if registered.profile.key.layout + in {"g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"} + } + assert set(products) == {"G20", "L6", "O6", "O12"} + for registered in products.values(): + engine = registered.build_engine() + adapter = registered.build_sdk_adapter() + assert engine.checkpoint_token == ACQUISITION_POLICY_VERSION + assert adapter.command_layout is registered.profile.command + assert len(engine.scan_units()) == len(registered.profile.motion.tasks) * 8 + + +def test_only_o12_has_one_three_degree_mapping_probe() -> None: + for registered in get_default_registry(): + if registered.profile.key.layout not in { + "g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16" + }: + continue + engine = registered.build_engine() + task = registered.profile.motion.tasks[0] + probe = engine.mapping_probe_delta(task) + if registered.profile.key.model == "O12": + assert probe is not None + assert 0.0 < abs(probe) <= math.radians(3.0) + else: + assert probe is None + + +def test_short_vision_loss_and_low_ideal_rates_are_warnings() -> None: + profile = next( + item.profile for item in get_default_registry() + if item.profile.key.layout == "o12_right_16" + ) + result = CalibrationEngine(profile).evaluate_sweep( + [index / 63.0 for index in range(64)], + minimum_span=0.85, + total_frames=90, + joint_frame_rate=0.71, + feedback_hz=12.0, + detection_rate=0.72, + bin_count=64, + ) + assert result.passed + assert any(value.startswith("tag_rate=") for value in result.warnings) + assert any(value.startswith("joint_frame_rate=") for value in result.warnings) + + +def test_observability_failure_gets_only_one_same_speed_rescan() -> None: + profile = next( + item.profile for item in get_default_registry() + if item.profile.key.layout == "l6_right_8" + ) + engine = CalibrationEngine(profile) + failed = engine.evaluate_sweep( + [index / 255.0 for index in range(20)], + minimum_span=240.0 / 255.0, + total_frames=200, + joint_frame_rate=0.1, + feedback_hz=5.0, + detection_rate=0.1, + ) + assert not failed.passed + assert engine.retry_speed(40.0, 2) == 40.0 + with pytest.raises(ValueError, match="exactly one"): + engine.retry_speed(40.0, 3) + + +def test_o12_effective_endpoint_scale_is_not_an_internal_blind_spot() -> None: + profile = next( + item.profile for item in get_default_registry() + if item.profile.key.layout == "o12_right_16" + ) + result = CalibrationEngine(profile).evaluate_sweep( + [index / 255.0 for index in range(20, 256)], + minimum_span=0.85, + total_frames=236, + joint_frame_rate=1.0, + feedback_hz=50.0, + detection_rate=1.0, + ) + assert result.passed + assert result.metrics["gap_scope"] == "observed_feedback_span" + assert result.metrics["maximum_bin_gap"] == 0 + + +def test_rotation_axis_uses_observed_endpoints_for_physical_feedback() -> None: + commands = np.arange(25, 254, dtype=int) + physical_angle = (253.0 - commands.astype(float)) / 228.0 + expected_axis = np.asarray([-0.25, 0.1, -0.96], dtype=float) + expected_axis /= np.linalg.norm(expected_axis) + vectors = physical_angle[:, None] * expected_axis[None, :] + + axis = fit_rotation_axis(vectors, commands) + projections = vectors @ axis + + assert float(np.median(projections[commands <= 48])) > float( + np.median(projections[commands >= 230]) + ) + + +def test_repeatable_hysteresis_is_diagnostic_when_holdout_passes() -> None: + profile = next( + item.profile for item in get_default_registry() + if item.profile.key.layout == "o12_right_16" + ) + curve = SimpleNamespace( + maximum_hysteresis_rad=math.radians(4.2), + maximum_monotonic_correction_rad=math.radians(0.2), + ) + fit = SimpleNamespace( + curves={"joint": curve}, + zero_offsets_rad={"joint": 0.0}, + travels_rad={"joint": 1.0}, + mimic_fits={}, + holdout_errors_rad={"joint": (0.0, math.radians(0.5))}, + ) + + result = CalibrationEngine(profile).result_from_fit(fit) + + assert result.quality["passed"] + assert result.quality["fit_diagnostics_by_joint"]["joint"][ + "maximum_hysteresis_rad" + ] == pytest.approx(math.radians(4.2)) + + +def test_directional_lookup_does_not_force_passive_curve_into_polynomial() -> None: + source = tuple(float(value) for value in np.linspace(1.5, 0.0, 256)) + target = tuple(float(value) for value in 1.2 * np.asarray(source) ** 3) + + def curve(values: tuple[float, ...]) -> JointCurveFit: + return JointCurveFit( + angle_rad=values, + decreasing_rad=values, + increasing_rad=tuple(value + 0.05 for value in values), + circle={}, + maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=0.05, + quality={}, + ) + + fit = fit_coupling_model( + "source", "target", curve(source), curve(target), + model="direction_aware_knots", + minimum_multiplier=0.1, + maximum_multiplier=3.0, + ) + + assert fit.model == "direction_aware_knots" + assert fit.urdf_mimic_policy == "endpoint_linear_fallback" + assert fit.urdf_mimic_multiplier == pytest.approx(target[0] / source[0]) + assert fit.residual_max_rad == 0.0 + + +def test_o12_internal_blind_spot_larger_than_one_sixteenth_is_rejected() -> None: + profile = next( + item.profile for item in get_default_registry() + if item.profile.key.layout == "o12_right_16" + ) + values = [ + index / 255.0 for index in range(256) + if not 100 <= index <= 130 + ] + result = CalibrationEngine(profile).evaluate_sweep( + values, + minimum_span=0.85, + total_frames=len(values), + joint_frame_rate=1.0, + feedback_hz=50.0, + detection_rate=1.0, + ) + assert not result.passed + assert any(value.startswith("maximum_gap=") for value in result.failures) + + +def test_old_checkpoint_policy_is_intentionally_incompatible() -> None: + profile = next( + item.profile for item in get_default_registry() + if item.profile.key.layout == "g20_right_19" + ) + engine = CalibrationEngine(profile) + assert not engine.resume_compatible({"profile_id": profile.key.profile_id}) + assert engine.resume_compatible({ + "profile_id": profile.key.profile_id, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + }) + + +def test_removed_soft_stop_gates_are_not_in_live_nodes() -> None: + package = Path(__file__).resolve().parents[1] / "linkerhand_calibration/models" + live = "\n".join( + (package / relative).read_text(encoding="utf-8") + for relative in ("l6/node.py", "o12/node.py") + ) + for removed in ( + "non_target_motor_moved", + "o12_auxiliary_settle_timeout", + "o12_sdk_feedback_coupling_hard_limit", + "o12_roll_coupling_exceeded", + ): + assert removed not in live + + +def test_virtual_new_model_needs_only_adapter_profile_and_raw_urdf( + tmp_path: Path, +) -> None: + joint = "finger_joint" + profile = CalibrationProfile( + key=ProfileKey("VIRTUAL", "right", "one_joint"), + namespace="/virtual/right", + command=CommandLayout( + names=("finger",), + baseline_u8=(255,), + command_index_by_joint={joint: 0}, + urdf_joint_by_joint={joint: joint}, + ), + vision=VisionRigSpec( + views=(ViewSpec("front", ( + TagSpec("base", 0, True), TagSpec("finger", 1), + )),), + common_frame="front", + extrinsic_reference_view="front", + ), + motion=MotionPolicy(tasks=( + TaskSpec( + "finger_sweep", "front", 0, (joint,), + start_u8=255, end_u8=0, formal_speed_u8=40, + ), + )), + measurement=MeasurementPolicy(measurements={ + joint: MeasurementSpec( + joint, "rotation", "front", "base", "finger" + ), + }), + zero=ZeroSolvePolicy( + active_joints=frozenset({joint}), + passive_joints=frozenset(), + direct_zero_joints=(joint,), + axis_joints=(joint,), + mechanical_endpoint_joints=frozenset(), + post_solve_endpoint_joints=frozenset(), + mimic_source_by_joint={}, + cad_frozen_joints=frozenset(), + ), + quality=QualityPolicy( + (0, 1, 2), 3, frozenset({"holdout"}), isolated_holdout=True + ), + scope=ScopePolicy( + calibrate_joints={"full": frozenset({joint})}, + frozen_joints={"full": frozenset()}, + ), + artifacts=ArtifactPolicy( + 1, "virtual.json", "virtual.urdf", frozenset(), + publish_corrected_urdf=True, + ), + ) + engine = CalibrationEngine(profile) + adapter = ProfileSdkAdapter(profile.command) + assert len(engine.scan_units()) == 8 + assert adapter.parse_feedback(("finger",), (127.0,)) == (127.0,) + + source = tmp_path / "virtual_raw.urdf" + source.write_text( + '' + '' + '' + '' + '' + '', + encoding="utf-8", + ) + output = write_urdf_patches( + source_urdf=source, + destination_urdf=tmp_path / "virtual.urdf", + patches=UrdfPatchSet({ + joint: UrdfJointPatch(origin_rpy="0 0 0.01") + }), + ) + corrected = output.read_text(encoding="utf-8") + assert 'rpy="0 0 0.01"' in corrected + assert '' in corrected diff --git a/src/linkerhand_calibration/test/test_urdf_comparison.py b/src/linkerhand_calibration/test/test_urdf_comparison.py new file mode 100644 index 0000000..efb13ff --- /dev/null +++ b/src/linkerhand_calibration/test/test_urdf_comparison.py @@ -0,0 +1,59 @@ +import math +import xml.etree.ElementTree as ET + +import numpy as np +import pytest + +from linkerhand_calibration.urdf_comparison import compare_urdfs, _Model, main + + +def model(tmp_path, name, zero=0.): + path = tmp_path / name + path.write_text(f''' + + + + + + ''') + return path + + +def test_identical_geometry_has_zero_pose_difference(tmp_path): + a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf") + result = compare_urdfs(a, b) + assert result["maximum_origin_distance_mm"] == pytest.approx(0.) + assert result["maximum_orientation_difference_deg"] == pytest.approx(0.) + assert not result["is_accuracy_certificate"] + assert result["pose_count"] == 4 + assert result["scope"] == "same_urdf_joint_angles_not_sdk_feedback" + + +def test_reports_actual_link_frame_shift_not_just_parameter_delta(tmp_path): + a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf", .1) + result = compare_urdfs(a, b) + assert result["maximum_origin_distance_mm"] == pytest.approx(200 * math.sin(.05)) + assert result["maximum_orientation_difference_deg"] == pytest.approx(math.degrees(.1)) + + +def test_mimic_source_on_other_branch_is_resolved_recursively(tmp_path): + path = model(tmp_path, "a.urdf") + root = ET.parse(path).getroot() + ET.SubElement(root, "link", name="follower_link") + j = ET.SubElement(root, "joint", name="follower", type="revolute") + ET.SubElement(j, "parent", link="base") + ET.SubElement(j, "child", link="follower_link") + ET.SubElement(j, "axis", xyz="0 0 1") + ET.SubElement(j, "mimic", joint="motor", multiplier="0.5", offset="0.1") + ET.ElementTree(root).write(path) + poses = _Model(path).fk({"motor": .4}) + assert poses["follower_link"][0, 0] == pytest.approx(math.cos(.3)) + assert np.isfinite(poses["follower_link"]).all() + + +def test_does_not_overwrite_existing_files(tmp_path): + a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf", .1) + before = a.read_bytes() + with pytest.raises(SystemExit): + main(["--reference", str(a), "--candidate", str(b), "--output", str(a)]) + assert a.read_bytes() == before diff --git a/src/linkerhand_calibration/test/test_urdf_patch_engine.py b/src/linkerhand_calibration/test/test_urdf_patch_engine.py index 41d5260..6f3cc13 100644 --- a/src/linkerhand_calibration/test/test_urdf_patch_engine.py +++ b/src/linkerhand_calibration/test/test_urdf_patch_engine.py @@ -10,6 +10,7 @@ from linkerhand_calibration.core.urdf import ( UrdfJointPatch, UrdfPatchSet, apply_urdf_patch_text, + validate_urdf_mimic_ranges, write_urdf_patches, ) @@ -162,3 +163,37 @@ def test_patch_engine_copies_referenced_and_complete_mesh_bundle( assert ( destination.parent / "meshes/auxiliary.stl" ).read_bytes() == b"auxiliary" + + +def test_mimic_range_validation_resolves_full_chain(tmp_path: Path) -> None: + text = """ + + + +""" + valid = tmp_path / "valid.urdf" + valid.write_text(text, encoding="utf-8") + ranges = validate_urdf_mimic_ranges(valid) + assert ranges["b"] == pytest.approx((0.0, 2.0)) + assert ranges["c"] == pytest.approx((0.0, 1.0)) + + invalid = tmp_path / "invalid.urdf" + invalid.write_text(text.replace('upper="1.1"', 'upper="0.9"'), encoding="utf-8") + with pytest.raises(ValueError, match="c=0.100000000rad"): + validate_urdf_mimic_ranges(invalid) + + +def test_mimic_range_validation_never_enlarges_cad_excess( + tmp_path: Path, +) -> None: + reference = tmp_path / "reference.urdf" + reference.write_text(SOURCE_TEXT, encoding="utf-8") + validate_urdf_mimic_ranges(reference, reference_urdf=reference) + + worsened = tmp_path / "worsened.urdf" + worsened.write_text( + SOURCE_TEXT.replace('multiplier="1"', 'multiplier="1.1"', 1), + encoding="utf-8", + ) + with pytest.raises(ValueError, match="passive"): + validate_urdf_mimic_ranges(worsened, reference_urdf=reference) diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_134319.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_134319.urdf new file mode 100644 index 0000000..4b7887c --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_134319.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_141642.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_141642.urdf new file mode 100644 index 0000000..a668f76 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_141642.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_170656.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_170656.urdf new file mode 100644 index 0000000..6239942 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_170656.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_174116.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_174116.urdf new file mode 100644 index 0000000..eb235c8 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_174116.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_181314.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_181314.urdf new file mode 100644 index 0000000..3dbf097 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_181314.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_183332.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_183332.urdf new file mode 100644 index 0000000..980b36d --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_183332.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260909_144123.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260909_144123.urdf new file mode 100644 index 0000000..36a9e32 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260909_144123.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260908_195044.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260908_195044.urdf new file mode 100644 index 0000000..b27a069 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260908_195044.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_105543.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_105543.urdf new file mode 100644 index 0000000..b6b95dd --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_105543.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_112041.urdf b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_112041.urdf new file mode 100644 index 0000000..543f403 --- /dev/null +++ b/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_112041.urdf @@ -0,0 +1,1234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file