diff --git a/.gitignore b/.gitignore index 08438f4..b3d8edd 100644 --- a/.gitignore +++ b/.gitignore @@ -70,6 +70,7 @@ Thumbs.db # Device-specific robot descriptions derived from local calibration runs # Includes full/partial zero-calibration outputs and local copies. +/src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf /src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf /src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..a8e003d --- /dev/null +++ b/AGENTS.md @@ -0,0 +1 @@ +避免产生屎山代码，要求代码结构清晰可读性好 \ No newline at end of file diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py index 2dcfef1..8b9d21f 100644 --- a/src/gui_control/gui_control/config/constants.py +++ b/src/gui_control/gui_control/config/constants.py @@ -114,8 +114,6 @@ _HAND_CONFIGS: Dict[str, HandConfig] = { "动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255], "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], - "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], - "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], "末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0], "末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255], "末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0], @@ -309,6 +307,22 @@ _HAND_CONFIGS: Dict[str, HandConfig] = { 0, 650, 700, 0, 650, 800, 700, 700, ], }, + preset_action_overrides={ + "right": { + "拇指对食指": [ + 245, -766, -818, 0, 0, 1046, 0, 0, 0, 0, 0, 0, + ], + "拇指对中指": [ + 489, -843, -685, 0, 0, 0, 0, 0, 1147, 0, 0, 0, + ], + "拇指对无名指": [ + 699, -779, -827, 0, 0, 0, 0, 0, 0, 0, 690, 0, + ], + "拇指对小指": [ + 786, -1065, -827, 0, 0, 0, 0, 0, 0, 0, 0, 645, + ], + }, + }, position_scale=1000, position_unit="rad", ), diff --git a/src/gui_control/test/test_o12_config.py b/src/gui_control/test/test_o12_config.py index b616cc1..6978180 100644 --- a/src/gui_control/test/test_o12_config.py +++ b/src/gui_control/test/test_o12_config.py @@ -14,3 +14,33 @@ def test_o12_gui_uses_active_angle_order_and_milliradians() -> None: assert config.position_unit == "rad" assert len(config.init_pos) == 12 assert all(len(pose) == 12 for pose in config.preset_actions.values()) + + +def test_o12_right_fingertip_preset_actions_use_requested_radians() -> None: + config = HAND_CONFIGS["O12"] + actions = config.get_preset_actions("right") + + assert actions["拇指对食指"] == [ + 245, -766, -818, 0, 0, 1046, 0, 0, 0, 0, 0, 0, + ] + assert actions["拇指对中指"] == [ + 489, -843, -685, 0, 0, 0, 0, 0, 1147, 0, 0, 0, + ] + assert actions["拇指对无名指"] == [ + 699, -779, -827, 0, 0, 0, 0, 0, 0, 0, 690, 0, + ] + assert actions["拇指对小指"] == [ + 786, -1065, -827, 0, 0, 0, 0, 0, 0, 0, 0, 645, + ] + assert all(len(actions[name]) == 12 for name in ( + "拇指对食指", "拇指对中指", "拇指对无名指", "拇指对小指", + )) + + +def test_o12_fingertip_preset_actions_are_right_hand_only() -> None: + actions = HAND_CONFIGS["O12"].get_preset_actions("left") + + assert "拇指对食指" not in actions + assert "拇指对中指" not in actions + assert "拇指对无名指" not in actions + assert "拇指对小指" not in actions diff --git a/src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md b/src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md new file mode 100644 index 0000000..1e413cd --- /dev/null +++ b/src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md @@ -0,0 +1,169 @@ +# 统一标定需求代码审查（2026-09-10） + +## 结论与审查范围 + +审查当前工作区（包含未提交的统一重构、精简 JSON v2 和本会话启动参数修复）， +不是只审查 Git HEAD，也没有把历史 PASS 当作当前实机验收。 +通用采集、拟合、URDF 修正、最终文件验收和发布架构已经存在，但仍有迁移残留和精度验证缺口。 +本轮修正可由代码及隔离测试确认的问题，没有改变原始 CAD、机械限位、SDK、贴 Tag 配置或相机外参。 +没有启动 SDK、相机或发送机械手运动指令。 + +## 已修复的问题 + +| 问题 | 影响 | 修改 | +| --- | --- | --- | +| `vendor_sdk_config:=` 等空 launch 实参 | O6/L6/G20 可能在节点启动前退出 | runner 省略空参数，使用 ROS CLI 的实际解析器覆盖四型号 | +| 在线 IO 使用固定 front/side/top 外参加载器 | 新 Profile 即使静态校验通过，在线仍可能不能使用其他机位 | 直接用通用外参加载器，视图和参考机位来自 Profile | +| launch 固定三机位、型号文件名回退和过期参数 | 新型号需修改启动代码；部分开关没有实际作用 | 按 Profile 创建机位；要求受保护 Profile/原始 URDF/Tag 路径；移除旧诊断、速度、scope 和输出目录覆盖参数 | +| MVS 无图像时只显示“等待设备” | 操作者无法区分 SDK、相机或检测链路故障 | 统一显示每机位有效内参/检测消息等待项、SDK 条件和内参不匹配原因 | +| READY 后设备断流仍保留可开始状态 | 启动请求可能使用已失效的就绪条件 | 开始前可撤销 READY；Start 再次检查完整设备条件 | +| 实时 CameraInfo 只比较 P 的部分字段 | K、D、R 改变但 P 未变时，整流图可能与原外参不一致 | 实时校验宽高及 K/D/R/P 指纹；开始后不匹配按原有坐标变化故障停止继续采集 | +| 每个稳态采样点清空整方向统计 | 显示有效帧和覆盖率归零，误导进度判断 | 分离方向数据与当前稳态点数据，重扫/换方向才重置方向统计 | +| 标题只显示序列号 | 序列号不含型号时无法确认当前型号 | 所有型号统一显示 model、side、serial_number | +| 正式 finalizer 仍允许旧格式绕过指令拟合 | 新型号配置旧版本可能没有所需的 SDK 指令→rad 产物 | 正式 Profile 与 finalizer 均只接受统一输出版本 2，取消旧输出分派 | +| 完成回调读取精简 JSON 已删除的 `quality` | 文件可能已经发布，但节点在结束时抛 KeyError | 删除没有消费者的 `final_quality` 赋值，增加无 quality 字段的完成回调回归 | +| 视觉暂停与控制定时器之间的状态竞争 | 定时器等待锁期间已暂停，取得锁后仍可能继续动作 | 在控制锁内再次检查终止状态 | + +开始前等待相机消息的 2 秒窗口不用于扫描中的实时暂停；空检测数组算视觉链路存活。 +短时 Tag 丢失、低检测率等仍按方向结束的数据质量策略处理，同速重扫一次后仍不足才暂停。 + +清理的在线残留包括无人读取的回调计数、命令频率缓存/方法、型号质量标签、旧 tracker reset hook， +以及每次反馈回调不必要构造的备用 Adapter。没有删除 SDK 协议接口的抽象方法。 + +五个过渡期写出器已移出生产包并保留原文件： +`calibration_output/code_review_retired_serializers_umhxlepj/`。 +包括 `select.py`、`generic_v1.py`、`legacy_v4.py`、`legacy_v6.py`、`native_v7.py`。 +历史读取器和历史诊断工具仍在；此归档不包含或改写用户的历史标定产物。 + +## 视频复核后的补充修复：共用姿态分支筛选 + +视频复核发现旧配置把 `pnp_reprojection_tie_px` 和姿态拒绝阈值均设为 `1.5 px`， +导致通过质量门槛的 IPPE 候选都参加时间连续性比较，旧镜像分支可能压过明确更好的图像拟合。 +用现有候选姿态复现时，旧分支 `0.24 px` 会压过另一分支 `0.05 px`。 + +公共模块统一默认近似同误差阈值为 `0.03 px`；ROS 与离线采集继承该值。 +五份旧 YAML 的重复覆盖已移除，四产品的 `calibration_config_sha256` 已同步更新。 +新增校验拒绝非有限、负值或不小于图像拒绝阈值的分支容差，防止旧的 `1.5/1.5` 配置再次生效。 +保持实际近似同误差时的连续性选择、原有倾角/图像质量限制和方向结束重扫策略。 +因此前文“配置保持不变”仅描述首轮结构审查；这次补充修复修改了上述算法配置及其哈希。 + +覆盖五份配置和四型号默认采集的九个复现用例，在修复前全部失败，修复后全部通过。 +第一组姿态、配置、采集与加载实际参数文件的四型号隔离 ROS host 检查共 51 项通过。 +第二组公共采集/拟合与发布、O12 姿态解析、来源保护、四型号配置及启动检查共 141 项通过， +耗时 114.53 秒。两组为本次修改的定向回归，没有宣称本次又执行过全包测试。 +本次 `colcon build --packages-select linkerhand_calibration --symlink-install` 通过； +安装环境读取四产品配置、校验受保护哈希及公共默认值均通过，`git diff --check` 通过。 +这不代表所有平面双解已消除，也不替代下述相机时序和实机重复性验证。 + +## 仍不能由本轮软件审查保证的事项 + +### P1：真实采集时间与传输积压 + +`hikrobot_camera.py::_publish_frame` 使用主机取到帧时的 ROS 时间戳，未将设备曝光时间映射到统一时钟。 +SDK 帧结构有设备时间戳和帧号，但设备时钟单位、偏移、漂移与主机时钟同步还没有在当前硬件验证。 +`runtime/ros/io.py::_state_callback` 对无时间戳反馈也采用接收时间。 +因此，USB/驱动缓存可能把旧图像与新反馈配对；50 ms 的消息时间戳检查和“只取最新帧”不能排除该问题。 +本会话已经实测到三台相机共享 480 Mbps Hub 上行，且正面/上方取帧超时。 + +后续应在恢复 USB 3.x 链路后，核验设备时间戳、帧号、曝光与反馈时序，并用独立运动数据检查配对误差。 +不能猜测设备时钟单位、直接给时间戳减一个常数，或放宽质量阈值作为修复。 + +### P1：重复性和实机/仿真验收尚未完成 + +前三轮训练、第四轮独立验证、冻结 Tag 安装和双文件重读校验已接入。 +但同手多次独立重采、重新摆放和重新贴 Tag、不同保持姿态/多关节组合动作的实际验收仍缺证据。 +L6/O6 部分关节由小指参数迁移，不属于所有关节独立实测。 +发布报告仍应如实保留 `arbitrary_multiaxis_validated: false`。 + +外部仿真要加载本次修正 URDF，用同一 SDK 指令话题及配套 JSON 驱动；用实机反馈直接驱动仿真， +只能显示反馈对应姿态，不能证明指令映射正确。桥本身是消息转换器，不是独立精度测量工具。 + +### P2：数学模块和旧配置仍有维护负担 + +`core/fitting/spatial.py` 约 3,656 行，包含轴线拟合、基座位姿、零位优化、可观测性和验证， +其中 `solve_urdf_zero_offsets` 从约第 1,921 行开始，嵌套函数及共享局部状态较多。 +它是当前生产算法，不是可直接删除的废代码。后续拆分应按这些数学职责进行， +用固定输入的参数、残差、失败原因和产物回归验证等价性，不应复制成各型号独立算法。 + +当前约 58 个兼容模块仍用于旧导入路径、历史回放/迁移工具和旧回归；不能仅因目录名称或静态引用少就删除。 +几行的公开兼容导出不是第二套拟合算法。旧型号诊断、旧 URDF plan 和部分旧 YAML 参数仍需明确退休范围后继续收缩。 +受保护的历史配置在本轮保持不变；旧 YAML 中一些已不被在线节点读取的参数，不应当作当前算法的生效开关。 + +原始 G20/O12 的 mimic/限位冲突和 CAD 保留策略仍需机械资料确认；当前通用 URDF 修正 +主要处理零位、限位和线性 mimic，不估计连杆长度、轴位置、mesh 或惯量。 + +## 按需求核对 + +| 需求 | 当前判断 | +| --- | --- | +| 多型号共用拟合和 URDF 修正 | 已实现统一生产链；SDK 新协议仍需 Adapter 和 ROS 启动绑定 | +| 新型号只关心 SDK、贴 Tag、避让 | 基本成立，但必须声明关节绑定、可观测性、迁移/保留策略和可修改字段，不能从未知安装的单轴 Tag 自动猜出 CAD 零位 | +| 统一 SDK 指令→rad JSON 与修正 URDF | 正式仅生成统一 v2 指令表；u8 为 256 项，rad 为显式节点；被动表由标准 mimic 推导并验收，详细证据保留在报告 | +| 开始前可移动、开始后固定 | 每次 Start 丢弃预览参考，正式锁定；可见固定基准有漂移监测；遮挡对象是否移动不能实时保证 | +| 正确、可重复 | 有独立验证和来源保护，但仍受采集时序与未完成实机验收限制 | +| 少暂停且原因明确 | 统一策略保留；开始前明确缺失设备，不用短时视觉丢失实时打断扫描 | +| 统一进度且明确型号 | 已修正型号、设备等待原因及方向统计显示 | +| 同源指令实机/仿真验证 | 有统一转换桥；实际动态/组合动作对比尚未完成 | + +## 本轮验证 + +- 审查中完整标定包回归：**544 通过、1 跳过，383.25 秒**。跳过项要求显式提供 + `O12_REPLAY_RAW` / `O12_REPLAY_REFERENCE`，没有用合成数据替代历史实测。 +- 随后补充的启动清理、单一正式输出及归档改动：启动/runner/正式 Profile 拟合/精简产物定向组 + **74 通过，108.78 秒**，包含四型号与虚拟型号的实际公共拟合和发布路径。 +- 最后完成回调、暂停竞争、ROS host、等待诊断、实时内参保护：**25 通过，0.98 秒**。 + 与上一组有重叠，不相加冒充一次全量结果；没有宣称最后所有变更又跑过一次全量。 +- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。 + 安装入口读取四产品的受保护配置、ROS launch 实参解析均通过；167 个生产/兼容 Python 文件 AST 通过； + `git diff --check` 通过。 +- 新 launch 测试实际构造四型号及改名机位的 launch actions，但不执行这些动作。 + 隔离 ROS 节点/标准加载器测试不启动 SDK 或相机，不代表实机验收。 + +## 数学核心与 ROS 组合重构的验收记录 + +本轮以包含公共 `0.03 px` 姿态分支修复的工作区为基线，只拆分职责、明确数据和并发边界。 +没有删除基线中仍存在的历史入口，没有修改配置、拟合公式、优化初值/顺序、残差权重或验收阈值。 + +- `core/fitting/spatial.py` 保留 45 个原定义符号的显式导出；实际计算在 `spatial_solver/`。 + `solve.py` 的入口顺序为输入准备、训练求解、统计、独立验证及结果装配。 + 数据通过 `ZeroProblem`、`TrainingProblem`、`TrainingFit` 和各验收结果类型传递。 + 训练接口只接收训练观测，训练几何中的掌部姿态按训练轮筛选。 +- `core/geometry/pnp.py` 保留 14 个原定义符号；实现拆为 IPPE、单 Tag 跟踪、刚性组和轨迹选择。 + 跟踪默认值集中在 `tag_pose/parameters.py`，ROS 参数只负责加载和 deg/rad 转换。 +- `UnifiedCalibrationNode` 直接继承 ROS `Node`，组合消息 IO 与 `CalibrationCoordinator`。 + 协调器不加载 ROS 库，复用 `SessionExecution`、采集、运动、固定基准和断点组件。 + 运行参数、观测输入和快照使用明确类型；阶段只有 `CalibrationSession.phase` 一个来源。 +- `FinalizationController` 组合原 worker 与发布器，协调器接收阶段事件并授权提交。 + 状态锁先于 worker 锁；PnP 在状态锁外计算，提交时检查会话版本、采集/运动对象及稳态边界。 + 暂停、中止与提交互斥；失败不触发自动恢复运动，成功只提交一次。 + SDK 绑定显式接收反馈新鲜度、时钟、健康订阅和发布接口。 + +固定输入记录在本次工作区快照 `/tmp/calibration_structure_baseline_30qf3ozo/`。 +持久化的对比报告及压缩前后输出位于 `calibration_output/structure_refactor_20260910/`， +其中 `comparison.json` 记录输入摘要、差异、产物摘要和拒绝原因，`protected_inputs.json` 保存输入哈希。 +这些记录是合成数据的结构等价证据，不是实机精度验收。 + +| 固定输入 | 比较浮点值数量 | 最大绝对差异 | JSON/拟合结果/阶段事件及 URDF 文本 | +| --- | ---: | ---: | --- | +| G20 | 46,046 | 0 | 一致 | +| L6 | 15,304 | 0 | 一致 | +| O6 | 15,304 | 0 | 一致 | +| O12 | 14,494 | 0 | 一致 | +| 虚拟型号（任务重排） | 15,304 | 0 | 一致 | + +对五组输入分别移除第四轮或注入第四轮 Tag 姿态滑移，10 个拒绝原因和阶段事件与基线一致。 +167 个配置、Profile、原始 URDF/资源文件的哈希不变。 +另一个虚拟型号同时重排 SDK 通道、机位名称和扫描任务，经过真实公共采集、finalizer、 +最终 JSON/URDF 验收和发布，再检查同一 SDK 指令经过模拟传输、查表和标准 mimic 图的一致性。 + +验证结果： + +- 整个标定包：581 passed，1 skipped，337.43 s。 +- SDK 绑定时钟补充后，协调器、四型号 ROS 构造和安全策略回归：27 passed。 +- 跳过项是 `test_o12_recorded_replay.py`：未提供 `O12_REPLAY_RAW` 与独立参考模型；不计作实机通过。 +- 改动模块通过 Pyflakes，`git diff --check` 通过。 +- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。 +- 安装后的 12 个 console entry 可导入；两个产品 CLI 的 `--help` 正常。 + 两个 ROS 节点入口均通过四型号受保护 Profile 的转交检查；安装环境下四型号 ROS 构造/Start 回归 4 passed。 + +本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。 diff --git a/src/linkerhand_calibration/CALIBRATION_FLOW.md b/src/linkerhand_calibration/CALIBRATION_FLOW.md deleted file mode 100644 index 274b4f4..0000000 --- a/src/linkerhand_calibration/CALIBRATION_FLOW.md +++ /dev/null @@ -1,238 +0,0 @@ -# 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 38f175f..996d3a1 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -1,1341 +1,236 @@ -# LinkerHand 专业标定包 +# LinkerHand 多型号统一标定 -## 全型号安装与重复标定约定 +G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF +验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。 -O12 的 `thumb_mcp_dip_front` 使用 ID1/ID2/ID3 联合 IPPE 候选选择，不再逐 Tag -独立决定分支。复用通用组跟踪器，以实测平行转轴方向辅助消歧；不指定 Tag 安装角， -不将 CAD mimic 比例或 SDK 被动公式替换为视觉测量。方向证据不足时退回视觉连续性， -不新增实时停机条件；最终几何质量验证保持不变。其他型号的选解策略及断点逻辑不变。 -在线选择仅作暂定观测。最终求解用前三轮完整轨迹重新比较拇指候选分支，第四轮 -不参与分支评分；结果写入 `pose_selection_diagnostics.json`。不会把关节轨迹强行 -投影成无残差的刚性运动，也不会降低原几何门限。 +本包校准关节零位、有效行程及标准线性 mimic；保留 CAD 连杆尺寸、轴位置、mesh、惯量。 +JSON 描述 SDK 输入与修正 URDF 角度的转换，不能补偿错误的 URDF。 +软件仿真通过不等于实机精度通过；当前原始 G20/O12 存在需要确认的 mimic/限位冲突。 +详见 [执行与验收记录](CALIBRATION_REFACTOR_PROGRESS.md)。 -三相机检测来自 `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> -``` +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +colcon build --packages-select linkerhand_calibration --symlink-install +source install/setup.bash -它检查相同 **URDF 关节角** 下的连杆原点位置、方向及主动行程，不修改文件、不控制 -硬件、不影响标定拟合或发布。坐标不是原始 SDK 弧度，连杆原点也不是指尖接触点； -离线探测姿态不可直接发送给实机。报告不等于精度 PASS。参考结果只作对照，不作为 -固定零位输入，也不强制新数据拟合成旧参数。 +# 无运动、无 SDK 启动的配置检查 +ros2 run linkerhand_calibration calibrate_hand --config \ + src/linkerhand_calibration/config/o12_right_product.yaml --validate-only -## O12 右手 16-Tag 完整标定 - -O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口，固定订阅 -`/o12/right/joint_states`、发布 `/o12/right/joint_cmd`。标定程序不会使用 -0–2000 混合控制接口，也不会按可能错位的 `JointState.name[]` 重排数据；12 路 -`position[]` 顺序及 `thumb_roll → thumb_cmc_roll`、 -`thumb_abad → thumb_cmc_yaw` 映射由产品 Profile 固定。 - -把 [o12_right_product.yaml](config/o12_right_product.yaml) 中的 -`serial_number` 改成实物串号后，只需运行： - -```bash +# 正式启动：自动启动 SDK、相机、检测及标定，READY 后自动开始运动 ros2 run linkerhand_calibration calibrate_hand --config \ src/linkerhand_calibration/config/o12_right_product.yaml ``` -runner 会自动加载仓库内 vendor Jazzy overlay，启动 HCAN device 0/channel 0 -节点、三相机、AprilTag 与标定节点；READY 后自动开始。启动前会确认 12 路 -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 -ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/o12_right_product.yaml --validate-only -``` - -## O6 右手局部标定（o6_right_8/v1） - -O6 使用与 L6 相同的三机位八 Tag 观测拓扑，但保留 O6 自己的六通道协议与 -URDF 关节名：正面 ID0/1/2 标定 `rh_thumb_cmc_pitch` 与 -`rh_thumb_ip`，侧面 ID3/4/5 标定 `rh_pinky_mcp_pitch` 与 -`rh_pinky_dip`，上面 ID6/7 标定 `rh_thumb_cmc_yaw`。六路 baseline 均为 -`255`；每次只扫描通道 0、1 或 5，其他通道保持 255。经硬件确认，食指、中指、 -无名指与小指同机构，因此小指 MCP/DIP 的实测结果会以迁移来源标记后用于其余 -三指。O6 实测 IP/DIP 均存在稳定的非线性，因此使用通过独立 holdout 的双向 -运行曲线和二次耦合模型。标准 URDF 无法表示二次 mimic，因此修正 URDF 与 L6 -一样保留端点对齐的线性 ``，使普通 URDF/RViz 中五个被动关节能正常联动， -同时把被动关节 limit 更新为实测范围；中间行程的精确双向非线性轨迹由下述标定桥 -发布。 - -```bash -ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/o6_right_product.yaml --validate-only - -ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/o6_right_product.yaml -``` - -由于 O6 的 FRONT/TOP 光轴接近正交，同一平面棋盘的同步视角天然更倾斜。O6 外参 -允许最终批次 RMS 不超过 1.5 px，但仍保持 0.3° 旋转、1.5 mm 平移稳定性门限， -并在标定发布前额外用 20 mm 跨机位实体轴线 RMS 粗差门限拦截移动相机等明显错误： - -```bash -ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \ - output_file:=$PWD/config/o6_three_camera_extrinsics.yaml \ - checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \ - maximum_reprojection_rms_px:=1.5 -``` - -结果发布到 `calibration_output//latest_partial_passed`，运行时 JSON、 -修正 URDF 与 correction-input JSON 均使用 `o6_right_` 前缀。产品 YAML 内的实物 -串号、相机身份、外参和四项输入哈希必须在启动硬件前通过校验。 - -标定完成后，以修正 URDF 启动 robot state publisher，并用同一会话中的 JSON 把 -O6 六路反馈转换为 11 个 URDF 关节： - -```bash -ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \ - hand_type:=right \ - calibration_file:=$PWD/calibration_output/O6_RIGHT_001/latest_partial_passed/o6_right_O6_RIGHT_001_partial_calibration.json -``` - -## L6 右手局部标定（l6_right_8/v1） - -本版只发布 `rh_thumb_cmc_pitch`、`rh_thumb_cmc_roll`、 -`rh_pinky_mcp_pitch` 的静态零位与动态曲线，以及 `rh_thumb_dip`、 -`rh_pinky_dip` 的视觉动态曲线。拇指 DIP 通过线性 mimic；小指 DIP 使用实测 -双方向运行曲线和 MuJoCo 二次 equality，因为它的传动比会随屈曲角变化。生成的 -修正 URDF 保留 `rh_pinky_dip` 的 ``，因此在普通 URDF/RViz 中仍会跟随 -MCP 运动；该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由 -MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认，食指、中指、 -无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移；每指自己的 -`origin.xyz`、axis、mesh 和惯量保持 CAD，不把迁移结果标成 Tag 实测。四指 MCP -以反馈 255 的展开端作为 CAD lower/zero 锚点；实测行程不会再被强制压回较短的 -CAD upper，因此不会向四指 origin 写入系统性的负零偏。 -结果指针为 `latest_partial_passed`，不会被当作六路主动关节的完整标定。 - -拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比，以反馈 255 -保持源 CAD joint zero，不叠加端点推断的静态偏置；小指及三指迁移仍以反馈 0 -机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点，不改变视觉实测的双方向 -行程曲线。现场姿态对比必须同时记录对应的六路反馈值。 - -先启动 SDK 和 GUI 做手动检查时使用： - -```bash -ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \ - -p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6 - -ros2 run gui_control gui_control -``` - -正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点，不要同时 -运行上面的 SDK/GUI 控制命令： - -```bash -ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/l6_right_product.yaml -``` - -只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF： - -```bash -ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/l6_right_product.yaml --validate-only -``` - -离线回放与在线发布使用同一拟合/URDF写回路径： - -```bash -ros2 run linkerhand_calibration calibrate_hand --config \ - src/linkerhand_calibration/config/l6_right_product.yaml \ - --offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl -``` - -拟合通过后，程序先原子写入并重新校验 -`l6_right_<序列号>_urdf_correction_input.json`，验证串号、Profile 和源 URDF -SHA-256 后，才把其中的原精度零偏、行程和 DIP 耦合参数交给现有 URDF 写回器。 -面向运行时的 schema-v6 `*_partial_calibration.json` 及修正 URDF 的字段、数值和 -格式保持兼容；单独的 correction-input JSON 是可审计的 URDF 生成依据，不是运行桥 -的输入文件。 - -运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、 -roll、index、middle、ring、pinky；旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路 -兼容别名读取，新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。 -预检和正式扫描均使用 L6 硬件速度 `1` 作为上限，并由 100 Hz 余弦缓入缓出 -轨迹把完整 `255↔0` 行程固定为 `6 s`；短行程按距离同比缩短。SDK 会过滤 L6 -在同一 CAN ID 回送的位置命令回显，标定只使用状态查询返回的真实反馈。标定 -启动时还会检查重复 SDK/GUI 发布者，避免两个进程同时访问同一只手。 - -标定完成后，用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节 -`JointState`： - -```bash -ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \ - hand_type:=right \ - calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json -``` - -schema v6 默认订阅 `/l6/cb_right_hand_state`，发布 -`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `` 本身只支持线性关系， -所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似；需要实测轨迹时使用该桥 -或修正 URDF 内的 MuJoCo equality。 - -## G20右手正式一键标定 - -固定三相机和19张Tag安装完成后，用户只运行： - -```bash -ros2 run linkerhand_calibration calibrate_hand -``` - -旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名； -旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。 - -构建和正式调用统一为： - -```bash -colcon build --packages-select linkerhand_calibration -ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml> -``` - -## 代码边界 - -- `core/`：无 ROS、无具体型号，包含领域类型、PnP/旋转数学、拟合接口、统一样本 - 契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。其中 - `core/urdf/patch.py` 是所有型号共用的声明式 URDF patch engine，统一负责属性级 - 文本修改、MuJoCo equality、mesh 安全复制、禁止覆盖和原子发布。 -- `runtime/`：通用会话状态机与注册 Profile 分发；ROS 消息和硬件适配只能位于 - `runtime/nodes`、`runtime/adapters`。 -- `models/g20/`、`models/l6/`：只保留型号 Profile、拟合/零位策略以及把拟合结果 - 转成 `UrdfPatchSet` 的薄适配层，不再各自实现 XML/mesh 文件写入器。新增 O6 时 - 优先新增 Profile；只有测量链或传动模型不同的部分才增加小型拟合插件。 - 后续型号或左右手作为新的独立 Profile 加入 `models/`，不在通用层增加分支。 -- `compat/`：v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留 - 一版最小转发 shim，不包含算法副本。 - -产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、 -Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改；v2 配置使用 -`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明，通用层 -不要求 `front/side/top`，也不假设固定 20 个命令。 - -仓库中存在已审定硬件会话时，可执行只读金标准检查（不会覆盖任何产物）： - -```bash -ros2 run linkerhand_calibration validate_g20_goldens \ - calibration_output/G20_RIGHT_001 -``` - -它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话；完整 -整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。 - -完全独立地只标定大拇指4项任务时，使用： - -```bash -ros2 run linkerhand_calibration calibrate_hand \ - --scope thumb -``` - -该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机 -方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动 -关节，12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`，其JSON是 -拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。 - -如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用： - -```bash -ros2 run linkerhand_calibration calibrate_hand \ - --scope thumb \ - --base-session calibration_output/G20_RIGHT_001/latest_passed -``` - -合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对， -任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。 - -拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础， -只重新采集12项四指任务并合成完整整手URDF： - -```bash -ros2 run linkerhand_calibration calibrate_hand \ - --scope fingers \ - --base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳> -``` - -`fingers`模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指 -零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的 -独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写 -拇指结果。原来的默认`full`仍保留，用于需要16项全部重新采集的情况。 - -采样文件中的运动域是显式且不可混用的： -`requested_command_u8` 表示下发命令，`feedback_u8` 表示电机反馈。 -在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的 -`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为 -`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`，完成复核后才允许机械手运动。 - -提供`--base-session`时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD -URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准 -URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、 -`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务，其余任务的原始记录导入后仍按 -数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。 - -开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经 -完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的 -独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与 -最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据 -当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收 -失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记； -恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。 -因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因 -日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面 -主测量和侧面校验测量独立保留。侧面校验视角的任务级有效率只记录为诊断；逐帧 -识别率低于标称值， -但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整 -轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量 -门限保持不变。`unified_engine_v1`取消每任务低速全行程预检；相邻方向共享已验证 -端点和任务级PnP参考。采样不足只按原速度重扫当前方向一次，拟合或第四轮留出 -失败立即锁定发布并保持当前位置，不再通过反复运动碰门限。G20右手正式扫描固定使用 -产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态 -过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查：前三轮 -极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的 -紧密簇、仅另一轮越界，也只输出诊断并停止发布，不自动补采。与`latest_passed` -中上一正式结果相差超过0.75°时另写入 -`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装，但该历史差值 -不直接否决当前会话，也不会用旧结果约束新零位。默认恢复兼容断点，前提是安装未变； -使用 `--no-resume` 可从第一个关节重新采集。 -升级前已经分别完成的正面/侧面roll也会合并为 -一个完整同步任务断点；只有两边数据都完整时才复用。 - -命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、 -16个会话数据求解主动关节URDF零位修正、 -21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。 -终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。 - -四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag -平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。 -侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用 -静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。 -单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只 -限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧 -绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。 -终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。 -若联合候选仍然失败，`raw_samples.jsonl` 会按8个反馈计数的区间保存 -`group_pnp_candidate_event`，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参 -哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。 - -正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在 -JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证 -全部通过后更新。 - -## G20右手19-Tag底层调试入口 - -以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简 -schema v4 JSON，不再生成schema v5运行文件。 - -新布局用独立参数启用，原有左右手11-Tag流程仍默认使用 -`tag_layout:=legacy_11`，两套配置和结果schema互不覆盖： - -```bash -ros2 launch linkerhand_calibration three_camera_calibration.launch.py \ - hand_type:=right \ - tag_layout:=g20_right_19 \ - serial_number:=G20_RIGHT_001 \ - camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ - source_urdf_expected_sha256:= -``` - -当前产品布局共19张`tag36h11`，所有Tag的黑色码区边长均为16 mm；四指末节ID为 -`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`，侧面为 -`4,5,6,7,14,15,16,17,18`，上面为`8,9`。详细角色和逐ID尺寸以 -`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务， -以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、 -侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过 -各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、 -用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉 -拟合和质量门限。任务直接执行四轮双向正式扫描； - -基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的 -固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中 -允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不 -放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的 -实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考 -失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点 -零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计； -任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把 -采集质量良好的任务误判为可见性失败。 - -恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次 -相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端 -能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch -和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的 -平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的 -坐标上限以及相关被动关节的mimic坐标偏置， -保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和 -第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、 -四指MCP roll、MCP pitch及PIP，共16个。`thumb_mcp`与四指屈伸关节一样使用 -实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。 -侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出； -同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时 -运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP” -等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。 -`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。 -同一任务的四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出， -不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且 -当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机 -退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的 -"滚转先回中再展开""先滚开再弯曲"顺序保持不变。G20右手四轮正式速度始终使用 -产品配置的固定值，不会因本次帧率或识别密度而改变。 -四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理 -零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、 -四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。 -正式四轮使用产品审定速度；唯一一次采样重扫保持相同速度。 -正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈； -方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再 -替代静态保持姿态。 -前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。 -每个任务在首个正式方向起点执行PnP静态初始化；四轮正式扫描连续复用同一帧间 -分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮 -已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的 -IPPE镜像解。baseline标准接近和全部质量门限保持不变。 -电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中 -排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放 -`thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。 - -当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。 -URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个， -其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程 -与机械端点联合求解； -写出非零`thumb_mcp`零偏时同步平移其关节坐标上限，并更新被动`thumb_ip`的 -`mimic offset`，因此不会改变CAD定义的最大屈曲实体姿态； -`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容， -当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、 -轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中 -相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入 -数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测 -前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收， -正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中 -严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置， -避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是 -随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限； -两者线距仅记录在诊断中。侧视校验通道 -（`*_mcp_roll_side`）的 -姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自 -近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴 -仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断，组合轴线改用四轮 -位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。 -侧面端视roll的圆轨迹方向已经受 -姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限； -径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的 -二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮 -留出失败仍会拒绝发布。任一静态目标、第四轮留出、 -遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。 -8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔 -位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用 -`a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。 -现场需要区分某根手指的roll机构回差与单机位误差时，可设置 -`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次 -正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务， -双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果 -一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久 -锁定URDF发布，不能用`resume`转换成正式标定。 -schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅 -`/g20/cb_right_hand_state`，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动 -方向。尚未观察到运动方向时使用`255→127`标准分支，不使用两个机械分支的平均值。 -schema v4继续兼容旧 -命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。 - -## 三机位三维关节轴零位标定（schema v4） - -正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有 -`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要 -贴正；相机和Tag在一次标定中必须固定。默认绑定为： +其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。 +`calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。 +`--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。 +重新开始时先安全回基准，仍必须确保现场没有障碍物。 + +| Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 | +| --- | ---: | ---: | ---: | --- | +| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v2，256 项指令查表 | +| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v2，256 项指令查表 | +| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v2，256 项指令查表 | +| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v2，rad 输入节点查表 | + +任务数、主动关节数和实测关节数不必相等。一条运动可观测多个主动/被动关节。 +L6/O6 未贴 Tag 的同机构关节使用显式迁移，不能把迁移称为独立实测。 +每任务四个往返周期：前三轮训练，第四轮独立验证。 +每个方向先连续扫描，再单通道访问稳态点。训练每方向默认 9 点，第四轮为交错点加端点； +每点至少 3 个稳定同步图像，轨迹结束后最多等待 2 秒，不能用精确指令/反馈相等判断稳定。 +稳态点缺失与连续扫描不足统一在方向结束处理，最多同速重扫一次。 +字节拟合不再改写原始 command/feedback，也不将接近端点的读数伪装成 0/255。 + +## 基准、避让与恢复 + +- 开始前可以调整手掌、相机和 Tag；预览数据不会成为正式参考。 +- 开始后清空预览缓存，先安全回基准，再为每机位固定 Tag 收集至少 10 个有效帧。 + 缺少基准时保持并显示缺少的 ID，不因为等待时间长暂停。 +- 锁定后禁止人工移动手掌、相机、支架、Tag 粘贴位置；程序驱动关节和避让仍是允许的。 +- 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。 +- 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。 + 运动 Tag 的刚性和几何一致性仍须通过最终独立验证。 +- 断点版本为 `unified_engine_v4_dual_mapping`，不复用缺少稳态证据的旧 v1/v2/v3。 + 新断点检查配置哈希、基准及所复用 Tag 安装关系（5 px、2°、5 mm）。缺失或不一致即放弃复用， + 自动完整重采，旧会话不修改。恢复时重新执行准备与避让，不直接跳到旧姿态。 + +O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指前，中指 MCP/PIP 到最大弯曲指令， +中指侧摆为 0 rad。到位采用完成轨迹、正确方向、至少 80% 请求位移和短时稳定， +不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。 + +O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。 +只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。 +POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。 +字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。 + +## 哪些情况暂停 + +实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、 +物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。 +开始后相机投影参数变化也会拒绝继续使用混合坐标数据。 + +短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动 +不会实时中断轨迹。无效视觉帧直接丢弃。 + +所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。 +ROS 和离线采集使用相同默认值，型号 YAML 不重复声明该值。`1.5 px` 是图像质量拒绝阈值， +不能用它判断两种姿态是否同样可信；仅在误差差值不超过近似同误差阈值时使用时间连续性。 +当另一分支的图像拟合明确更好时允许纠正旧分支。覆盖参数必须有限、非负且小于图像质量拒绝阈值。 +这项修复不保证解决所有平面 Tag 双解，真实角点噪声、采集时序和最终运动精度仍需实测验证。 + +方向结束才检查 ≥40 个有效同步样本、≥32 分箱、内部空白 ≤行程的 1/16、规定的有效行程覆盖， +以及 Profile 声明的端点/双视角观测。数据不足只同速重扫一次；第二次失败暂停。 +拟合或最终文件验收失败不重新自动运动、不更新发布指针。 + +所有型号显示相同进度：阶段、任务/轮次/方向、分机位 Tag ID、命令/反馈/单位/速度、 +有效数据、基准、断点来源，以及中文原因、建议和原始诊断。耗时拟合在后台执行，不阻塞反馈与状态。 +标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件； +空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。 +收到开始请求时重新检查设备状态；相机有效内参必须与受保护外参中的指纹一致。 +开始前相机消息的新鲜度窗口为 2 秒，该窗口不用于扫描中的实时暂停。 + +## 唯一生产代码链 ```text -front = DB2163742，Tag 0/1/2/3/10 -side = DB2163749，Tag 4/5/6/7 -top = DB2163739，Tag 8/9 +linkerhand_calibration/ +├── config/profiles/ 唯一型号定义（受保护 YAML） +├── config/*_product.yaml 设备、序列号、相机、路径与哈希 +├── urdf/ 原始 CAD/mesh，不覆盖 +└── linkerhand_calibration/ + ├── profiles/ 加载、静态验证、Tag/CAD 观测关系 + ├── core/domain/ Profile、样本、CalibrationResult、状态 + ├── core/geometry/ 相机、变换、PnP 兼容导出 + │ └── tag_pose/ IPPE、连续跟踪、刚性组、轨迹分支选择 + ├── core/fitting/ 运动曲线、线性耦合、固定安装 + │ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收 + ├── core/urdf/ 授权修正、FK、范围与最终文件验收 + ├── runtime/runner.py 唯一产品启动器 + ├── runtime/coordinator.py 会话协调、采集提交、运动和收尾接线 + ├── runtime/parameters.py 已解析的运行参数；无 ROS 类型 + ├── runtime/inputs.py 相机/检测输入、时钟与发布接口 + ├── runtime/cameras.py 受保护相机身份与消息新鲜度 + ├── runtime/session.py 唯一在线状态机 + ├── runtime/execution.py Profile 任务与运动效果调度 + ├── runtime/motion_execution.py 平滑轨迹与位移到位判定 + ├── runtime/capture.py 多机位视觉/反馈采集 + ├── runtime/scan_quality.py 唯一方向数据门 + ├── runtime/reference_lock.py 开始后固定基准 + ├── runtime/resume.py 唯一断点证据检查 + ├── runtime/safety.py 最小实时保护 + ├── runtime/status.py 统一状态与中文显示 + ├── runtime/snapshot.py 类型化状态快照与旧状态映射 + ├── runtime/adapters/ SDK 协议，不包含标定业务 + ├── runtime/ros/ ROS 参数加载、消息转换、订阅/服务/定时器 + ├── runtime/artifacts/ 收尾控制、后台 finalizer、serializer、原子发布 + └── compat/ 历史格式、布局与离线诊断；不提供旧在线流程 ``` -11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致： +`calibrate_hand → runner → runtime/ros/entrypoint → UnifiedCalibrationNode → CalibrationCoordinator → SessionExecution` +是所有型号的生产调用链。节点直接继承 ROS `Node`，组合 `RosCalibrationIO` 和协调器， +不通过父子类业务回调执行标定。SDK 绑定接收明确的反馈新鲜度、时钟、健康订阅和发布接口。 -| ID | 机位 | 固定位置/运动件 | -|---:|---|---| -| 0 | 正面 | 正面掌壳固定基准 | -| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 | -| 2 | 正面 | 拇指 MCP 后连杆 | -| 3 | 正面 | 拇指 IP 后末节 | -| 4 | 侧面 | 掌壳侧面固定基准（最底下） | -| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 | -| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 | -| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 | -| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 | -| 9 | 上面 | 拇指 CMC yaw 运动件 | -| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 | +空间入口 `core/fitting/spatial.py` 保留显式兼容导出，计算顺序在 +`spatial_solver/solve.py`：输入准备 → 仅训练求解 → 可观测性/轮间统计 → 冻结结果的独立验证 → 结果装配。 +`TrainingProblem` 不包含第四轮观测，训练几何中的掌部姿态也按训练轮筛选。 +基座策略由 Profile 的几何约束选择；曲线、零位坐标约定、权重、阈值和原始 URDF 授权修正规则不变。 -ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见， -ID 9 必须在拇指横摆的完整行程中持续可见。 -贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。 +PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值， +ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。 +公共分支容差仍为 `0.03 px`，重投影质量、倾角、过滤原因和连续跟踪恢复策略保持一致。 -每台相机必须有独立内参文件： +`CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测； +耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。 +Start 更换预览采集对象并重新锁定基准，暂停/中止使尚未完成的旧观测失效。 +收尾由 `FinalizationController` 组合 worker 和发布器，worker 只上报阶段事件、准备候选文件； +协调器核实全部验收阶段后才能提交。锁顺序固定为状态锁 → worker 锁， +拟合失败维持当前姿态，中止后禁止提交，成功产物只提交一次。 + +老的单相机拇指/CMC 在线节点和 launch 已退出安装入口；需要局部测量时应增加经过验证的 Profile， +不能重新启用旧状态机。老版本离线诊断不是新的标准 URDF 发布证据。 + +## 新增型号 + +必需文件： ```text -~/.ros/camera_info/hikrobot_DB2163742.yaml -~/.ros/camera_info/hikrobot_DB2163749.yaml -~/.ros/camera_info/hikrobot_DB2163739.yaml +config/profiles/new_hand.yaml +config/new_hand_product.yaml +urdf/new_hand/raw.urdf +urdf/new_hand/meshes/* ``` -### 1. 一次性三相机外参 +Profile 声明： -三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用 -`8x5` 内角点、实测方格边长 `27 mm`、粘在硬质平板上的棋盘： +1. Adapter、SDK 通道顺序、单位、正负方向、命令/反馈范围、基准值、独立速度槽布局。 +2. Tag ID/尺寸/机位/安装 link，以及每次运动的父子观测关系；不明确的 link 不能靠名称猜。 +3. 扫描任务、速度、准备/避让/收尾 waypoint；必要时指定不可见 Tag 的恢复验证姿态。 +4. 哪些零位可观测、哪些固定保留 CAD、哪些参数同机构迁移；声明空间观测约束和拟合原语。 +5. 每关节允许修正的 `origin.rpy`、`limit.lower/upper`、`mimic.multiplier/offset`。 + +这仍需要正确的可观测性设计：单轴加一对未知安装角的 Tag 不能自动辨识绝对 CAD 零位。 +不能为了减少配置，默认把 SDK 零点或最大指令当成 CAD 零位。可参考最简单的 L6/O6 Profile， +删除不适用关系后重新声明；不要盲目复制另一型号的空间约束。 + +复用 SDK 不增加 Python；新协议只增加 `runtime/adapters/` 实现及协议工厂注册。 +没有型号节点、runner、状态机、质量/断点/发布器；新数学只能增加通用拟合原语。 +新型号使用 `artifacts.output_schema_version: 2` 选择统一指令查表格式；产物自身为 +`format: unified_calibration_v2, schema_version: 2`。详细双映射保留在独立标定报告中。 +旧 generic v1、v4/v6/v7 只保留历史兼容工具，不把反馈表改名为指令表来伪造兼容。 +正式 Profile 校验及 finalizer 只接受 `output_schema_version: 2`，不能用旧格式跳过指令映射拟合。 +配置错字、错误绑定、未知修正字段在启动前失败；修改受保护文件后应审查并更新对应 SHA256。 + +## 产物与离线验证 + +会话目录保存原始样本、冻结 Tag 安装、拟合诊断、标准 URDF 验收、JSON、URDF。 +有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。 +manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。 +`measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。 +主 JSON 以原始 URDF 关节名为键，每个关节仅包含 `sdk_channel` 和 `angle_rad`； +O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。 +字节型号固定 256 项：SDK 指令 `v` 对应 `angle_rad[v]`，只能在训练指令实际覆盖 0–255 时导出。 +输出均是修正 URDF 的 q_output，不再加零偏。顶层保留 `format/schema_version/profile_id/model/side/serial_number/input_unit`。 +被动关节的查表由导出 URDF 的标准 mimic 递归生成，绑定同一驱动通道，必须与 URDF 完全一致。 +单张查表取训练正反向指令曲线的平均值，再用实际导出表重放第四轮视觉数据；回差等因素导致超差时禁止发布， +不通过隐藏方向分支、外推、放宽精度或 JSON 非线性被动联动制造通过结果。 + +`calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、 +零位、来源、迁移和训练/holdout 指标。报告中的 `applicability` 记录保持姿态； +未采集多姿态证据不能宣称任意多轴工况已验证。主 JSON 用于指令查表，不能误作反馈转换。 +旧 unified v1 会话继续只读兼容，不自动覆盖或转换历史产物；新输出单表必须重新通过最终文件验收。 + +统一消费桥可直接读取通过验证的 manifest（纯消息转换，不会发送实机命令）： ```bash -mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config -ros2 launch linkerhand_calibration \ - three_camera_extrinsics.launch.py \ - output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ - checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 +ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \ + -p calibration_file:= -p input_kind:=command \ + -p input_topic:= -p output_topic:=/calibration/verification/joint_states ``` -启动后默认打开 `G20 Three-Camera Extrinsics` 交互窗口。可切换 -`FRONT + SIDE` 和 `FRONT + TOP`；窗口实时显示棋盘角点、单相机/组合 -RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、 -同步、RMS和姿态差异合格，`ADD CANDIDATE` 就会变绿；不再用单张 -PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒会自动 -采集，移到新姿态后再自动采下一组。 - -外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加 -一个候选；随后以相同方法采集正面/上面。程序使用固定内参的 -`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和 -配对相机的单帧RMS分别不得超过1.5 px，候选组合RMS不得超过1.5 px；界面中 -单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。 -新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组， -再在不低于15个内点的前提下有界裁剪联合误差最高的候选，只有最终批次组合RMS -不超过1.2 px才允许保存。两组均得到 -至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。 +需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。 +桥检查三文件哈希、拒绝越域输入， +被动角由实际导出 URDF 计算，不加第二次零偏。仿真输出不能与实机反馈话题同名。 +MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。 +外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。 ```bash -ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {} -ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {} -ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {} +# 同一算法离线回放新策略的完整采集；不启动硬件，默认不发布 +ros2 run linkerhand_calibration calibrate_hand --config \ + --offline-raw --offline-output <新的输出目录> + +# 只比较两份 URDF，不修改，不控制机械手 +ros2 run linkerhand_calibration compare_calibration_urdfs \ + --reference <参考.urdf> --candidate <本次.urdf> --output <新报告.json> ``` -采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终 -显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出 -`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过 -1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时 -绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式 -标定也不会运动。 - -### 2. 预检和正式标定 - -先使用禁止运动模式检查三个机位、外参、内参和标签： - -```bash -ros2 launch linkerhand_calibration \ - three_camera_calibration.launch.py \ - hand_type:=left \ - serial_number:=G20_LEFT_001 \ - camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ - commands_enabled:=false -``` - -分别查看正式流程的三个画面： - -```bash -ros2 run image_view image_view --ros-args \ - --remap image:=/g20_calibration/front/camera/image_rect -ros2 run image_view image_view --ros-args \ - --remap image:=/g20_calibration/side/camera/image_rect -ros2 run image_view image_view --ros-args \ - --remap image:=/g20_calibration/top/camera/image_rect -``` - -确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程： - -```bash -ros2 launch linkerhand_calibration \ - three_camera_calibration.launch.py \ - hand_type:=left \ - serial_number:=G20_LEFT_001 \ - camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ - can_interface:=can0 -``` - -原始URDF及其mesh随`linkerhand_calibration`安装，默认根据`hand_type`自动选择。 -如需调试其他CAD版本，仍可通过`source_urdf_path:=<绝对路径>`显式覆盖。 -右手使用同一入口，并自动选择右手SDK话题和原始URDF： - -```bash -ros2 launch linkerhand_calibration \ - three_camera_calibration.launch.py \ - hand_type:=right \ - serial_number:=G20_RIGHT_001 \ - camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ - can_interface:=can0 -``` - -状态显示三个机位均“就绪”后只调用一次： - -```bash -ros2 topic echo /g20_calibration/status_text -ros2 service call /g20_calibration/start std_srvs/srv/Trigger {} -``` - -收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始 -第一条轨迹扫描： - -```text -[255, 255, 255, 255, 255, 255, 127, 127, 127, 127, - 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] -``` - -左手依次扫描电机 `0/5/15/6/1/16/10`，右手依次扫描 -`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对 -四元数的旋转向量投影到三维拟合轴得到。 -轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立 -交叉检查并参与融合。轴线上一点则由整段 -相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接 -当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的 -光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令 -严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承 -给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立 -电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留 -原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。 - -零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定 -掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免 -稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节 -进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游 -轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差 -超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴 -Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过 -1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°； -小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。 - -`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过 -被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴 -偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手 -`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其 -`angle_rad[256]` 实测轨迹。 - -前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位 -差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的 -训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。 - -单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动 -耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差， -不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。 - -四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差 -仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用 -完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型 -之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承 -上游轴方向时不重复报告同一项跨轮轴失败。 - -四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD -的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成 -约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。 - -四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断， -但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测 -装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械 -端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或 -序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。 - -视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指 -pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点 -的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。 -原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5 -保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。 -偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL -会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索 -不会放宽正式结果的硬门限。 - -生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的 -`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终 -JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。 - -坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、 -重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%， -端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时 -连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并 -暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫 -完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色 -预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算 -（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到 -后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前 -关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序 -只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复 -失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试 -仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的 -PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。 - -每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。 -程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组 -分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免 -错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立 -重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。 -再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。 - -左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像， -将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为 -`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用 -电机1/16（左）或4/19（右），参考指速度10。 - -右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到 -255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部 -Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节 -的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手 -仍沿用原有正面机位和基准姿态。 - -标定 `thumb_cmc_yaw`（电机10）时，程序将 -`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag -ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持 -电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才 -恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员 -暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的 -`baseline_command_u8` 不变。 - -右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19 -的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍 -使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判 -为端点到达。 - -右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端 -使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为 -电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。 - -三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证 -始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。 -状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和 -验证完成、正式JSON与URDF成功写入之前不会显示100%。 - -上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag -和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求 -当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。 - -### 3. 输出 - -通过后生成精简JSON和一个新URDF： - -```text -calibration_output/G20_LEFT_001/<时间戳>/ - g20_left_G20_LEFT_001_calibration.json - g20_left_G20_LEFT_001_calibration_urdf_correction_input.json - linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf - meshes/*.STL - -calibration_output/G20_RIGHT_001/<时间戳>/ - g20_right_G20_RIGHT_001_calibration.json - g20_right_G20_RIGHT_001_calibration_urdf_correction_input.json - linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf - meshes/*.STL -``` - -`*_urdf_correction_input.json` 会在修正 URDF 之前落盘并重新读取，且绑定源 URDF -哈希、型号、侧别、layout 和序列号。公开 schema-v4 运行 JSON 仍保持原格式，原有 -G20 运行桥、金标准哈希和 URDF 写回数值不因该交接层改变。 - -文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、 -`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从 -指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或 -覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能 -改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和 -`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随 -上游主动关节坐标系变换作等价调整。 -未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到 -同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、 -图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。 - -完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机 -命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖： - -```bash -python3 -m linkerhand_calibration.offline_replay \ - calibration_output/G20_RIGHT_001/20260811_120146 \ - --output-tag AXIS_FRAME_V3 \ - --write -``` - -下面保留原有正面拇指独立标定说明和兼容入口。 - -### 4. 修正URDF的运行时关节映射 - -修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节 -`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点 -`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线 -用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节 -`JointState`（包括5个被动关节）： - -```bash -ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \ - hand_type:=right \ - calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json -``` - -schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅 -`/g20/cb_right_hand_control_cmd`。两者均发布 -`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个 -发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令， -因此不会静默退回旧标定。 - -该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、 -Linker Hand SDK 和标定状态机， -只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次 -终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置， -程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。 -完成 `255→0→255` 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点 -平均，只为每个关节保存一个 256 项 `angle_rad`。最后用 5 个随机静态命令复测精度。 - -当前默认使用 `trajectory_center_3d`。节点由四个亚像素角点和 `CameraInfo.P` -计算每张 Tag 的三维中心，但不把小尺寸平面 Tag 的 PnP 朝向直接当作关节角： - -- 根部扫描先减去掌心 T0 的位置，再用 T3/T4/T5 三条圆轨迹共同拟合 CMC 旋转轴； - 每帧三个角度取中位数。 -- 尖部扫描用 T4 相对 T3 的圆轨迹直接拟合 MCP。G20 只有电机 15 这一个尖部输入， - URDF 将被动 IP 定义为 `thumb_ip = 1.02 × thumb_mcp`，因此运行时 IP 曲线严格按 - 这个机械耦合生成。这样不会把不同相机角度下 T5 的平面 PnP 深度偏差误认为 IP - 真实运动。 -- 程序仍会按 MCP 角将 T5 反向旋转并拟合剩余小圆，但该结果只用于 - `trajectory_center_quality.tip` 中的观测一致性诊断，不参与最终 IP 数组。 -- 每条曲线都减去命令 255 的测量角，所以最终文件严格满足 - `angle_rad[255] == 0.0`；`angle_rad[0]` 是该关节相对零位的最大角度。 - -这种方法对固定的相机摆放角度、Tag 在同一刚性连杆上的固定位置和贴纸朝向更不敏感。 -但相机或贴纸在一次扫描过程中移动、Tag 翘起、角点严重抖动仍会破坏圆轨迹。程序会 -检查平面残差、圆残差、轨迹半径、实际弧长和根部三个轨迹点的角度一致性。 - -PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维中心及辅助质量检查，不再 -直接生成运行时角度。根部扫描用固定的 T3–T4、T4–T5 中心间距共同选择分支； -尖部扫描用固定的 T0–T3 中心间距约束非目标部分。中心间距漂移超过阈值仍会暂停， -避免错误中心进入圆拟合，但 Tag 的 PnP 朝向抖动不会触发该门限。 - -## 1. 标记和安全检查 - -- `T0` 必须保留并固定在掌壳，作为整体平移参考；`T3` 固定在拇指根部运动连杆，`T4` 固定在 MCP 后的连杆， - `T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。 -- 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改 - `config/front_tags.yaml` 里检测节点和标定节点的两组数组。 -- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为 - `16 mm`，因此配置为 `0.016`。 - 测量检测角点所围成的正方形边长，不包含外围白色留边。 -- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由 - 静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时， - 启动参数增加 `publish_debug_image:=true`，再订阅 - `/g20_thumb_calibration/debug_image`；正式长时间扫描建议保持默认关闭。 -- 执行全行程前清空拇指周围空间并准备断开电机电源。确认这只手的下标 0 和 15 - 均可安全走完整 `255→0→255`。标定节点发现命令话题上另有发布者时不会解锁扫描。 - -## 2. 安装与构建 - -```bash -sudo apt-get update -sudo apt-get install -y \ - ros-jazzy-image-pipeline \ - ros-jazzy-apriltag-ros \ - ros-jazzy-apriltag-msgs \ - ros-jazzy-camera-calibration \ - python3-yaml - -cd /home/lxp/projects/linkerhand_retarget_ros2 -source /opt/ros/jazzy/setup.bash -colcon build --symlink-install \ - --packages-select linker_hand_ros2_sdk linkerhand_calibration -source install/setup.bash -``` - -相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在： - -```text -/opt/MVS/lib/64/libMvCameraControl.so -/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py -``` - -正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是 -`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举 -顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。 - -`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。 -三相机标定 launch 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量 -加载 `config/fastdds_large_images.xml`，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz， -大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。 - -首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从 -`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时 -仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。 - -先单独启动相机（不会连接机械手，也不会发送关节命令）： - -```bash -ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \ - --remap __ns:=/camera/camera/color \ - -p serial_number:=DB2163742 \ - -p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml -``` - -测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz： - -```bash -ros2 topic hz /camera/camera/color/camera_info -ros2 topic hz /camera/camera/color/image_raw -``` - -使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按 -实际标定板修改： - -```bash -ros2 run camera_calibration cameracalibrator \ - --size 8x6 --square 0.020 \ - --camera_name hikrobot_front_DB2163742 \ - --ros-args \ - --remap image:=/camera/camera/color/image_raw \ - --remap camera/set_camera_info:=/camera/camera/color/set_camera_info -``` - -在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会 -原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、 -分辨率和 ROI；改变任何一项都要重新标定。 - -连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布 -`/g20/cb_left_hand_control_cmd` 的程序。 - -## 3. 启动和操作 - -首次使用时可先用 `commands_enabled:=false` 做预检；SDK 仍会设置速度/扭矩并读取状态， -但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描： - -```bash -ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \ - serial_number:=G20_LEFT_001 \ - camera_serial_number:=DB2163742 \ - commands_enabled:=false -``` - -确认 T0、T3、T4、T5 在根部和尖部全行程中不会被遮挡，且拇指运动不会碰撞后， -停止预检并启动一个新的正式会话。默认使用 AprilTag 内部 `decimate=1.5` 提升检测 -速度，并使用单终点连续运动： - -```bash -ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \ - serial_number:=G20_LEFT_001 \ - camera_serial_number:=DB2163742 \ - can_interface:=can0 \ - calibration_speed:=15 \ - continuous_motion_mode:=endpoint \ - angle_estimation_mode:=trajectory_center_3d \ - apriltag_decimate:=1.5 \ - use_roi:=false -``` - -默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag -的完整画面： - -```bash -ros2 run image_view image_view --ros-args \ - --remap image:=/camera/camera/color/image_rect -``` - -图像检测链路使用 `sensor_data`（BEST_EFFORT）QoS，只保留最新帧，避免完整分辨率 -下可靠队列积压反压相机；这不会裁剪图像，也不会降低相机分辨率。 - -若以后需要以帧率优先，可传入 `use_roi:=true`；默认 ROI 是原图中的 -`x=128, y=192, width=1024, height=528`，也可用 `roi_x`、`roi_y`、 -`roi_width`、`roi_height` 覆盖。 - -监控状态： - -```bash -ros2 topic echo /g20_thumb_calibration/status -``` - -预检通过后状态为 `WAIT_ROOT_CONFIRM`，`reason` 为 `call_start`。只需调用一次： - -```bash -ros2 service call /g20_thumb_calibration/start std_srvs/srv/Trigger {} -``` - -节点随后自动完成下标 0 的 `255→0→255`、下标 15 的 `255→0→255` 和 5 点随机复测， -正常结束状态为 `COMPLETE`，无需在根部和尖部之间再次确认。为安全起见，调用 `start` -前必须一次性确认两个关节的完整行程都已清空。原来的 -`confirm_root_full_range`、`confirm_tip_full_range` 服务仍保留用于兼容。 - -暂停、恢复和终止： - -```bash -ros2 service call /g20_thumb_calibration/pause std_srvs/srv/Trigger {} -ros2 service call /g20_thumb_calibration/resume std_srvs/srv/Trigger {} -ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {} -``` - -预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。PnP 有效率也必须 -至少 95%，每个候选解的重投影 RMS 不超过 1.5 px。中心轨迹模式以三组相对中心 -的静止 RMS 不超过 2 mm、5 mm 范围内位置内点不少于 90% 为硬判据；PnP 朝向抖动 -只作为诊断，不会阻止静态捕获。 -状态中的 -`pnp_rejections` 会指出当前是哪张 Tag 因丢失、重投影/倾角超限或姿态跳变而被拒绝， -`pnp_reprojection_error_px` 显示四张 Tag 最近一次有效解的误差。连续扫描要求 -图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、 -至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、 -90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 `PAUSED`。恢复时会先回到 -该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。`abort` 也只停止队列， -不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。 - -PnP 跟踪在整个会话中对四张 Tag 都优先保持同一个 IPPE 平面分支；最多 5 秒的短暂检测 -间隔不会重新初始化分支。随机复测只有在同步电机反馈与目标相差不超过 2、且稳定 -窗口与捕获窗口内三个相对中心的最大偏差都不超过 3 mm 时才会写入，否则继续等待并最终暂停， -不会再生成明知不可靠但字段完整的结果。 - -单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用 -实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待 -电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路， -不要降低到固件低速区。必须临时回退时可启动 -`continuous_motion_mode:=paced`，该模式按步长 8 到位即发下一段。 - -连续扫描中的主要状态字段： - -- `state_zh`/`reason_zh`/`action_zh`：当前阶段、失败原因和下一步操作的中文说明； - 原有 `state`/`reason` 英文机器码继续保留。 -- `tag_quality`：逐张显示 T0/T3/T4/T5 的边长、hamming、识别置信度、重投影误差、 - 是否有效和具体中文问题，不再需要手工解析 `/apriltag/detections`。 -- `/g20_thumb_calibration/status_text`：适合终端直接查看的多行中文状态。使用 - `ros2 topic echo --once /g20_thumb_calibration/status_text --field data` - 即可看到原因、建议及四张标签的质量。 -- `scan_progress`：4 个方向的完成比例，依次约为 0、0.25、0.5、0.75、1.0。 -- `sweep_valid_frames_seen`：当前连续方向已收到的同步有效帧数。 -- `sweep_state_span_u8`：当前方向实际覆盖的电机范围，接近 255 才算完整。 -- `active_phase`/`active_direction`：当前是根部或尖部、下降或上升方向。 -- `pnp_branch_corrections`：四张 Tag 联合跟踪为维持相邻关节姿态连续，而没有选择 - 单张 Tag 最小重投影分支的累计次数。 -- `pnp_trajectory_quality`：最近一个完整方向的整段分支修正帧数，以及相对整段稳健 - 参考的旋转、相对平移和中心间距漂移。中心轨迹模式只按欧氏中心间距判断： - P95 超过 3 mm 或单帧最大值超过 6 mm 时暂停；旋转及随 Tag 坐标轴表达的相对平移 - 只保留为诊断。 -- `trajectory_center_quality`：四个方向完成并拟合后，显示三维平面/圆残差、拟合半径、 - 实际弧长、T0/T3 锚点漂移和根部三个轨迹点的角度一致性。其中 - `tip.ip_observed_vs_constrained_*` 显示T5残余小圆与URDF被动耦合之间的差异； - 它用于发现T5识别误差、标签松动或机构异常，但不会改变最终IP曲线。 - -根部扫描中 T3/T4/T5 作为完整刚性组共同选择 IPPE 分支，不再把 T3 固定为在线解； -尖部扫描仍固定 T3，只用静止的 T0/T3 约束修正非目标根部姿态。 - -## 4. 中断恢复和输出 - -默认会话目录是启动命令当前目录下： - -```text -calibration_output/<序列号>/<时间戳>/ -``` - -恢复时必须显式复用原目录，否则会创建新会话： - -```bash -ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \ - serial_number:=G20_LEFT_001 \ - session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000 -``` - -恢复会校验序列号、Tag 配置、基准命令、扫描模式、采集参数和代码哈希； -任一项变化都会拒绝混用旧样本， -此时应新建会话。 - -目录内文件： - -- `raw_samples.jsonl`：连续帧按实际整数电机位置分箱后的 Tag 三维中心、姿态辅助统计及复测点；每完成一个 - 扫描方向后落盘。 -- `checkpoint.json`：当前状态和进度。 -- `session_manifest.json`：Tag、相机内参、SDK、代码哈希和会话信息。 -- `validation.json`：随机复测及全部质量判据。 -- `rosbag/`：仅在 `record_bag:=true` 时生成，用于保存相机、检测、命令和状态等诊断数据。 -- `g20_left_<序列号>_thumb_angle.json`：精简后的运行时标定文件。 - -最终文件使用 `schema_version: 2`。每个关节只包含： - -```json -{ - "motor_index": 0, - "angle_rad": ["按命令0～255索引的256个弧度值"] -} -``` - -`thumb_ip.angle_rad` 由 `thumb_mcp.angle_rad` 乘 -`ip_coupling.multiplier`（默认 `1.02`）得到，二者在命令255处都严格为零。 - -`thumb_ip` 另外包含 `"passive": true`。正反方向原始曲线不进入最终 JSON，但仍保留 -在 `raw_samples.jsonl` 中，并用于最大回差和质量判定。 - -零位和最大角度可直接读取： - -```python -import json -from pathlib import Path - -data = json.loads(Path("g20_left_G20_LEFT_001_thumb_angle.json").read_text()) -for name, joint in data["joints"].items(): - print(name, "zero(rad)=", joint["angle_rad"][255], - "max(rad)=", joint["angle_rad"][0]) -``` - -如果相机或 SDK 已由外部进程启动，可传 -`start_camera:=false` 或 `start_sdk:=false`。`camera_serial_number` 同时接受 MVS -序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。 - -海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益 -`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用 -`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定 -曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加 -`record_bag:=true`。 - -默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用 -`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。 -校正和 AprilTag 组件运行 -在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。 -可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认 -不订阅整幅图像，只订阅检测结果和 TF。 -若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式， -不影响 AprilTag 的 ROI 输入。 - -静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。 -当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短 -相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。 - -启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像 -订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制； -若用于正式可追溯验收，再启用并检查磁盘空间。 - -## 5. CMC Pitch 零位角测量 - -只测量命令 255 时 `thumb_cmc_pitch` 的画面水平投影零位角时，使用独立启动文件。 -它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改 -URDF： - -```bash -ros2 launch linkerhand_calibration \ - front_cmc_pitch_zero.launch.py \ - serial_number:=G20_LEFT_001 -``` - -该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。 -预检完成后查看中文状态： - -```bash -ros2 topic echo --once --full-length \ - /g20_thumb_cmc_pitch_zero/status_text \ - --field data -``` - -状态显示“等待开始”后启动三轮测量： - -```bash -ros2 service call \ - /g20_thumb_cmc_pitch_zero/start \ - std_srvs/srv/Trigger {} -``` - -查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线 -的调试画面： - -```bash -ros2 run image_view image_view --ros-args \ - --remap image:=/g20_thumb_cmc_pitch_zero/debug_image -``` - -画面底部红线是固定的相机水平与构图目标。程序会在画面下部自动寻找一条足够长、 -接近水平的物理桌边或高对比参考直线，并画成蓝线。开始前调整相机，使蓝线与红线 -重合；画面和 `status_text` 会实时显示红蓝线夹角及垂直偏差。`±0.5°` 和 -`±12 px` 只用于显示 `ALIGNED/ADJUST`，完全不参与预检或 `start` 服务判断。 -由操作人员确认相机位置后手动开始标定。参考直线应清晰、连续并尽量横跨画面； -该检测不使用 T0/T3 标签朝向。 - -一条二维直线只能确认相机滚转角和上下构图位置，不能单独证明相机的距离、俯仰、 -偏航或完整三维位置。若需要严格复现这些量，还应使用固定相机支架或专用标定板。 - -每轮只控制电机 0 执行一次 `255→64` 端点运动和一次 `64→255` 返回运动。运动期间 -连续采集 `T3中心−T0中心`，按机械手状态分箱后拟合图像平面圆；返回 255 后使用 -“T3零位中心→拟合圆心”的固定内向径向矢量计算角度。运动前和返回后各采集30帧静态零位， -三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布 -`/g20/cb_left_hand_control_cmd` 时，`start` 服务会拒绝启动。 - -T0中心用于消除相机或整只手的平移抖动。T0和T3标签自身的朝向与角点 `+x` -都不参与零位或行程计算；标签可以任意平面内旋转或反贴180°，只需标签平整、 -固定且中心始终可见。若轨迹跨度、圆弧、半径、径向RMS/P95或回零误差不合格， -节点暂停或写出 `quality.passed=false`。 - -完成后只生成： - -```text -calibration_output/G20_LEFT_001/<时间戳>/ - g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json -``` - -核心字段是： - -```text -zero_angles.table_projected_zero_rad -``` - -该值是内向径向零位矢量相对相机画面水平向右方向的角度。它不使用 T0 的方向， -但会用 T0 中心抵消平移；它仍不是真实三维桌面检测，因此会随相机滚转和机械手 -摆放改变。 - -## 6. CMC Roll 零位与行程标定 - -`thumb_cmc_roll` 复用上节的 T0 平移补偿、T3 中心轨迹分箱和稳健圆拟合， -但控制的是电机 5。每轮执行 `255→0→255`：在 255 零位、0 行程端点和返回 -255 后各静态采集 30 帧，因此可以同时测量零位角和完整 `0～255` 实际角行程。 -命令 0 是完整行程端点，开始前必须确认拇指没有机械碰撞或硬限位顶死风险。 -Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。 - -```bash -ros2 launch linkerhand_calibration \ - front_cmc_roll_calibration.launch.py \ - serial_number:=G20_LEFT_001 -``` - -预检通过后启动三轮标定： - -```bash -ros2 topic echo --once --full-length \ - /g20_thumb_cmc_roll_calibration/status_text \ - --field data - -ros2 service call \ - /g20_thumb_cmc_roll_calibration/start \ - std_srvs/srv/Trigger {} -``` - -调试画面： - -```bash -ros2 run image_view image_view --ros-args \ - --remap image:=/g20_thumb_cmc_roll_calibration/debug_image -``` - -Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用 -蓝线状态阻止 `start` 进入电机运动。 - -完成后生成： - -```text -calibration_output/G20_LEFT_001/<时间戳>/ - g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json -``` - -核心输出字段： - -```text -zero_angles.table_projected_zero_rad -travel.signed_rad -travel.range_rad -``` - -`travel.signed_rad` 是从命令 255 到 0 的有符号转角，`travel.range_rad` 是三轮 -行程大小的中值。只有轨迹圆质量、T0/T3 检出率、三轮零位/行程一致性、端点径向 -误差和回零误差全部通过时，`quality.passed` 才为 `true`。 +离线回放也检查原始会话策略、SDK 输入域、配置/相机证据、每方向完整数据及最终文件。 +旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。 +算法和坐标规则见 [CALIBRATION_FLOW.md](CALIBRATION_FLOW.md)，批准的原始方案保持在 +[CALIBRATION_REFACTOR_PLAN.md](CALIBRATION_REFACTOR_PLAN.md)。 + +当前代码审查、已修复问题与未完成的精度验收见 +[2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)。相机图像目前采用主机取帧时间； +USB/驱动积压情况下，图像与反馈的时间戳接近不能证明曝光时刻同步，不能据此宣称实机精度通过。 diff --git a/src/linkerhand_calibration/config/calibration.yaml b/src/linkerhand_calibration/config/calibration.yaml index 8b660ab..a5a66ca 100644 --- a/src/linkerhand_calibration/config/calibration.yaml +++ b/src/linkerhand_calibration/config/calibration.yaml @@ -60,11 +60,8 @@ g20_thumb_calibration: # 30-38 px tags are usable, but only if IPPE gives a tight image fit and # a pose continuous with the preceding frame. pnp_maximum_reprojection_error_px: 1.5 - # All four tags keep a temporally continuous IPPE solution throughout the - # complete session. With 30 px planar tags, tiny reprojection differences - # do not reliably identify the physical branch and previously caused - # stationary T0 to flip by about 25 deg between scan and validation. - pnp_reprojection_tie_px: 1.5 + # Independent IPPE selection inherits the shared near-tie tolerance from + # core/geometry/pnp.py; continuity cannot override a clear image-fit lead. pnp_maximum_pose_jump_deg: 35.0 pnp_maximum_translation_jump_m: 0.04 pnp_maximum_tag_tilt_deg: 75.0 diff --git a/src/linkerhand_calibration/config/g20_right_product.yaml b/src/linkerhand_calibration/config/g20_right_product.yaml index 936538c..c1f98bb 100644 --- a/src/linkerhand_calibration/config/g20_right_product.yaml +++ b/src/linkerhand_calibration/config/g20_right_product.yaml @@ -1,4 +1,7 @@ -schema_version: 1 +schema_version: 3 +profile_id: G20/right/g20_right_19/v1 +profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml +profile_config_sha256: 35cad4432a01b3e1af157f1e914c2054e71522a44c6786db2519e4c2a0966a46 model: G20 side: right tag_layout: g20_right_19 @@ -26,7 +29,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: afb323494140c88ab6368a061332fd80724a86831f44505dcfab147234e3c4be + calibration_config_sha256: 4927506d787d665c16f0209ee4d052654e648bb25beaa1913ed0d416f449d523 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 5229478..3ecfddc 100644 --- a/src/linkerhand_calibration/config/l6_right_product.yaml +++ b/src/linkerhand_calibration/config/l6_right_product.yaml @@ -1,5 +1,7 @@ -schema_version: 2 +schema_version: 3 profile_id: L6/right/l6_right_8/v1 +profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml +profile_config_sha256: b9654ce998b56c10d7094364e8b715663163d2101e1f70e077cb0723fc47f02e model: L6 side: right tag_layout: l6_right_8 @@ -28,7 +30,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: e25e7ab27f4fd918f7cab70717e070d52a52fff17266a367c136388384ad4baa + calibration_config_sha256: 090d82a5609e8b981c1b8c6ebeaf4c2f477323209ae3db3b9d8713ac6d6169b5 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 ca74874..35a6cf3 100644 --- a/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/l6_three_camera_calibration.yaml @@ -36,7 +36,6 @@ l6_calibration: minimum_decision_margin: 30.0 minimum_edge_pixels: 30.0 pnp_maximum_reprojection_error_px: 1.5 - pnp_reprojection_tie_px: 1.5 pnp_maximum_pose_jump_deg: 35.0 pnp_maximum_translation_jump_m: 0.04 pnp_maximum_tag_tilt_deg: 75.0 diff --git a/src/linkerhand_calibration/config/o12_right_product.yaml b/src/linkerhand_calibration/config/o12_right_product.yaml index 0e46a23..63c2d16 100644 --- a/src/linkerhand_calibration/config/o12_right_product.yaml +++ b/src/linkerhand_calibration/config/o12_right_product.yaml @@ -1,5 +1,7 @@ schema_version: 3 profile_id: O12/right/o12_right_16/v1 +profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml +profile_config_sha256: 0b5cd27168bc863e5c1aed5382c08e33ab4c02a23359a325dc9925fd30599e5f model: O12 side: right tag_layout: o12_right_16 @@ -8,7 +10,9 @@ serial_number: O12_RIGHT_001 output_root: calibration_output sdk: - driver: omnihand_pro_2025_node + driver: o12_sdk_bridge + python_package: src/agillink_omnihand_sdk/linux/x64/python/omnihand-1.1.8-cp312-cp312-linux_x86_64.whl + package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170 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 @@ -34,7 +38,7 @@ artifacts: 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: 962a16ce3a21fd2d7886caab17c39a9441ff4eb856c9bf8e86c5c303c2291553 + calibration_config_sha256: c2fcdd532e22c15013413e655311de2ca87107c23a96d5c043e8f340f232eebd 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 fe982ae..1ba194c 100644 --- a/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/o12_three_camera_calibration.yaml @@ -47,7 +47,6 @@ o12_calibration: 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 pnp_maximum_translation_jump_m: 0.04 pnp_maximum_tag_tilt_deg: 75.0 diff --git a/src/linkerhand_calibration/config/o6_right_product.yaml b/src/linkerhand_calibration/config/o6_right_product.yaml index 260c422..c9edf88 100644 --- a/src/linkerhand_calibration/config/o6_right_product.yaml +++ b/src/linkerhand_calibration/config/o6_right_product.yaml @@ -1,5 +1,7 @@ -schema_version: 2 +schema_version: 3 profile_id: O6/right/o6_right_8/v1 +profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml +profile_config_sha256: ab1d86cb0c3e514de5322a44c1444e99d9b1f1ce43f8a2451cd032ed9c31e4ff model: O6 side: right tag_layout: o6_right_8 @@ -28,7 +30,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: a7fe124a195fa0f491535e96bdc728d25ea3efe298cf905e499917d329e9026f + calibration_config_sha256: 15adaec462d0895dc68d19760e8f333c62e57786d124f22cfc481007a3d976a4 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 162d644..e92130d 100644 --- a/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/o6_three_camera_calibration.yaml @@ -33,7 +33,6 @@ o6_calibration: minimum_decision_margin: 30.0 minimum_edge_pixels: 30.0 pnp_maximum_reprojection_error_px: 1.5 - pnp_reprojection_tie_px: 1.5 pnp_maximum_pose_jump_deg: 35.0 pnp_maximum_translation_jump_m: 0.04 pnp_maximum_tag_tilt_deg: 75.0 diff --git a/src/linkerhand_calibration/config/profiles/g20_right_19.yaml b/src/linkerhand_calibration/config/profiles/g20_right_19.yaml new file mode 100644 index 0000000..84eef1d --- /dev/null +++ b/src/linkerhand_calibration/config/profiles/g20_right_19.yaml @@ -0,0 +1,1504 @@ +schema_version: 1 +profile_id: G20/right/g20_right_19/v1 +namespace: /g20_calibration +sdk_adapter: legacy_byte_sdk +command: + names: + - thumb_cmc_pitch + - index_mcp_pitch + - middle_mcp_pitch + - ring_mcp_pitch + - pinky_mcp_pitch + - thumb_cmc_roll + - index_mcp_roll + - middle_mcp_roll + - ring_mcp_roll + - pinky_mcp_roll + - thumb_cmc_yaw + - reserved_11 + - reserved_12 + - reserved_13 + - reserved_14 + - thumb_mcp + - index_pip + - middle_pip + - ring_pip + - pinky_pip + baseline_u8: + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 127 + - 127 + - 127 + - 127 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + command_index_by_joint: + thumb_cmc_pitch: 0 + thumb_cmc_roll: 5 + thumb_cmc_yaw: 10 + thumb_mcp: 15 + index_mcp_roll: 6 + index_mcp_pitch: 1 + index_pip: 16 + middle_mcp_roll: 7 + middle_mcp_pitch: 2 + middle_pip: 17 + ring_mcp_roll: 8 + ring_mcp_pitch: 3 + ring_pip: 18 + pinky_mcp_roll: 9 + pinky_mcp_pitch: 4 + pinky_pip: 19 + disabled_indices: + - 11 + - 12 + - 13 + - 14 + urdf_joint_by_joint: {} + feedback_name_aliases: {} + speed_slot_by_command_index: + 0: 0 + 1: 1 + 2: 2 + 3: 3 + 4: 4 + 5: 0 + 6: 1 + 7: 2 + 8: 3 + 9: 4 + 10: 0 + 15: 0 + 16: 1 + 17: 2 + 18: 3 + 19: 4 + unit: u8 + baseline: [] + lower_bounds: [] + upper_bounds: [] + feedback_lower_bounds: [] + feedback_upper_bounds: [] + feedback_by_index: false + sdk_to_joint_direction: + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 + - -1 +vision: + views: + - name: front + tags: + - role: front_base + fixed_reference: true + id: 0 + - role: thumb_cmc + fixed_reference: false + id: 1 + - role: thumb_mcp + fixed_reference: false + id: 2 + - role: thumb_ip + fixed_reference: false + id: 3 + - role: pinky_roll + fixed_reference: false + id: 10 + - role: ring_roll + fixed_reference: false + id: 11 + - role: middle_roll + fixed_reference: false + id: 12 + - role: index_roll + fixed_reference: false + id: 13 + - name: side + tags: + - role: side_base + fixed_reference: true + id: 4 + - role: ring_pip + fixed_reference: false + id: 5 + - role: pinky_pip + fixed_reference: false + id: 6 + - role: pinky_dip + fixed_reference: false + id: 7 + - role: ring_dip + fixed_reference: false + id: 14 + - role: middle_pip + fixed_reference: false + id: 15 + - role: middle_dip + fixed_reference: false + id: 16 + - role: index_pip + fixed_reference: false + id: 17 + - role: index_dip + fixed_reference: false + id: 18 + - name: top + tags: + - role: top_base + fixed_reference: true + id: 8 + - role: thumb_yaw + fixed_reference: false + id: 9 + common_frame: calibration_common + extrinsic_reference_view: front + extrinsics_quality_limits: + reprojection_rms_px: 1.2 + maximum_rotation_repeatability_deg: 0.3 + maximum_translation_repeatability_m: 0.0015 + minimum_capture_counts: + front_side_captures: 15 + front_top_captures: 15 +motion: + tasks: + - key: thumb_cmc_pitch_front + view: front + command_index: 0 + joints: + - thumb_cmc_pitch + auxiliary_commands: + - - 5 + - 255 + - - 10 + - 255 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 5 + - 10 + - 15 + - - 0 + - key: thumb_cmc_roll_front + view: front + command_index: 5 + joints: + - thumb_cmc_roll + auxiliary_commands: [] + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 10 + - 15 + - - 5 + - key: thumb_mcp_ip_front + view: front + command_index: 15 + joints: + - thumb_mcp + - thumb_ip + auxiliary_commands: [] + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - - 15 + - key: thumb_cmc_yaw_top + view: top + command_index: 10 + joints: + - thumb_cmc_yaw + auxiliary_commands: + - - 5 + - 145 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 15 + - - 10 + - key: pinky_roll_multiview + view: front + command_index: 9 + joints: + - pinky_mcp_roll + auxiliary_commands: + - - 4 + - 255 + - - 6 + - 255 + - - 7 + - 255 + - - 8 + - 255 + - - 19 + - 255 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 9 + - key: pinky_pitch_side + view: side + command_index: 4 + joints: + - pinky_mcp_pitch + auxiliary_commands: + - - 6 + - 255 + - - 7 + - 255 + - - 8 + - 255 + - - 9 + - 127 + - - 19 + - 255 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - - 0 + - 5 + - 10 + - 15 + - - 4 + - key: pinky_pip_side + view: side + command_index: 19 + joints: + - pinky_pip + - pinky_dip + auxiliary_commands: + - - 4 + - 255 + - - 6 + - 255 + - - 7 + - 255 + - - 8 + - 255 + - - 9 + - 127 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 19 + - key: ring_roll_multiview + view: front + command_index: 8 + joints: + - ring_mcp_roll + auxiliary_commands: + - - 3 + - 255 + - - 4 + - 0 + - - 6 + - 255 + - - 7 + - 255 + - - 9 + - 127 + - - 18 + - 255 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 8 + - key: ring_pitch_side + view: side + command_index: 3 + joints: + - ring_mcp_pitch + auxiliary_commands: + - - 4 + - 0 + - - 6 + - 255 + - - 7 + - 255 + - - 8 + - 127 + - - 9 + - 127 + - - 18 + - 255 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 3 + - key: ring_pip_side + view: side + command_index: 18 + joints: + - ring_pip + - ring_dip + auxiliary_commands: + - - 3 + - 255 + - - 4 + - 0 + - - 6 + - 255 + - - 7 + - 255 + - - 8 + - 127 + - - 9 + - 127 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 18 + - key: middle_roll_multiview + view: front + command_index: 7 + joints: + - middle_mcp_roll + auxiliary_commands: + - - 2 + - 255 + - - 3 + - 0 + - - 4 + - 0 + - - 6 + - 255 + - - 8 + - 127 + - - 9 + - 127 + - - 17 + - 255 + - - 18 + - 0 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 7 + - key: middle_pitch_side + view: side + command_index: 2 + joints: + - middle_mcp_pitch + auxiliary_commands: + - - 3 + - 0 + - - 4 + - 0 + - - 6 + - 255 + - - 7 + - 127 + - - 8 + - 127 + - - 9 + - 127 + - - 17 + - 255 + - - 18 + - 0 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 2 + - key: middle_pip_side + view: side + command_index: 17 + joints: + - middle_pip + - middle_dip + auxiliary_commands: + - - 2 + - 255 + - - 3 + - 0 + - - 4 + - 0 + - - 6 + - 255 + - - 7 + - 127 + - - 8 + - 127 + - - 9 + - 127 + - - 18 + - 0 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 17 + - key: index_roll_multiview + view: front + command_index: 6 + joints: + - index_mcp_roll + auxiliary_commands: + - - 1 + - 255 + - - 2 + - 0 + - - 3 + - 0 + - - 4 + - 0 + - - 7 + - 127 + - - 8 + - 127 + - - 9 + - 127 + - - 16 + - 255 + - - 17 + - 0 + - - 18 + - 0 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 6 + - key: index_pitch_side + view: side + command_index: 1 + joints: + - index_mcp_pitch + auxiliary_commands: + - - 2 + - 0 + - - 3 + - 0 + - - 4 + - 0 + - - 6 + - 127 + - - 7 + - 127 + - - 8 + - 127 + - - 9 + - 127 + - - 16 + - 255 + - - 17 + - 0 + - - 18 + - 0 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 16 + - 17 + - 18 + - 19 + - - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 1 + - key: index_pip_side + view: side + command_index: 16 + joints: + - index_pip + - index_dip + auxiliary_commands: + - - 1 + - 255 + - - 2 + - 0 + - - 3 + - 0 + - - 4 + - 0 + - - 6 + - 127 + - - 7 + - 127 + - - 8 + - 127 + - - 9 + - 127 + - - 17 + - 0 + - - 18 + - 0 + - - 19 + - 0 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: null + formal_speed_u8: null + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_groups: + - - 6 + - 7 + - 8 + - 9 + - - 17 + - 18 + - 19 + - - 1 + - 2 + - 3 + - 4 + - - 0 + - 5 + - 10 + - 15 + - - 16 + preparation_waypoints_u8: [] + safe_return_waypoints_u8: [] + speed_parameters: {} + precheck_sweeps: false + steady_command_checkpoints: false + return_groups: + - - 6 + - 7 + - 8 + - 9 + - - 1 + - 2 + - 3 + - 4 + - - 16 + - 17 + - 18 + - 19 + - - 0 + - 5 + - 10 + - 15 +measurement: + measurements: + thumb_cmc_pitch: + joint: thumb_cmc_pitch + kind: urdf_axis_chain + view: front + parent_role: front_base + child_role: thumb_cmc + validation_source: null + pose_axis_line_required: true + thumb_cmc_roll: + joint: thumb_cmc_roll + kind: urdf_axis_chain + view: front + parent_role: front_base + child_role: thumb_cmc + validation_source: null + pose_axis_line_required: true + thumb_cmc_yaw: + joint: thumb_cmc_yaw + kind: urdf_axis_chain + view: top + parent_role: top_base + child_role: thumb_yaw + validation_source: null + pose_axis_line_required: true + thumb_mcp: + joint: thumb_mcp + kind: urdf_axis_chain + view: front + parent_role: thumb_cmc + child_role: thumb_mcp + validation_source: null + pose_axis_line_required: true + thumb_ip: + joint: thumb_ip + kind: curve + view: front + parent_role: thumb_mcp + child_role: thumb_ip + validation_source: null + pose_axis_line_required: false + index_mcp_roll: + joint: index_mcp_roll + kind: urdf_axis_chain + view: front + parent_role: front_base + child_role: index_roll + validation_source: index_mcp_roll_side + pose_axis_line_required: true + index_mcp_pitch: + joint: index_mcp_pitch + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: index_pip + validation_source: null + pose_axis_line_required: true + index_pip: + joint: index_pip + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: index_pip + validation_source: null + pose_axis_line_required: true + index_dip: + joint: index_dip + kind: curve + view: side + parent_role: index_pip + child_role: index_dip + validation_source: null + pose_axis_line_required: true + middle_mcp_roll: + joint: middle_mcp_roll + kind: urdf_axis_chain + view: front + parent_role: front_base + child_role: middle_roll + validation_source: middle_mcp_roll_side + pose_axis_line_required: true + middle_mcp_pitch: + joint: middle_mcp_pitch + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: middle_pip + validation_source: null + pose_axis_line_required: true + middle_pip: + joint: middle_pip + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: middle_pip + validation_source: null + pose_axis_line_required: true + middle_dip: + joint: middle_dip + kind: curve + view: side + parent_role: middle_pip + child_role: middle_dip + validation_source: null + pose_axis_line_required: true + ring_mcp_roll: + joint: ring_mcp_roll + kind: urdf_axis_chain + view: front + parent_role: front_base + child_role: ring_roll + validation_source: ring_mcp_roll_side + pose_axis_line_required: true + ring_mcp_pitch: + joint: ring_mcp_pitch + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: ring_pip + validation_source: null + pose_axis_line_required: true + ring_pip: + joint: ring_pip + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: ring_pip + validation_source: null + pose_axis_line_required: true + ring_dip: + joint: ring_dip + kind: curve + view: side + parent_role: ring_pip + child_role: ring_dip + validation_source: null + pose_axis_line_required: true + pinky_mcp_roll: + joint: pinky_mcp_roll + kind: urdf_axis_chain + view: front + parent_role: front_base + child_role: pinky_roll + validation_source: pinky_mcp_roll_side + pose_axis_line_required: true + pinky_mcp_pitch: + joint: pinky_mcp_pitch + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: pinky_pip + validation_source: null + pose_axis_line_required: true + pinky_pip: + joint: pinky_pip + kind: urdf_axis_chain + view: side + parent_role: side_base + child_role: pinky_pip + validation_source: null + pose_axis_line_required: true + pinky_dip: + joint: pinky_dip + kind: curve + view: side + parent_role: pinky_pip + child_role: pinky_dip + validation_source: null + pose_axis_line_required: true + pinky_mcp_roll_side: + joint: pinky_mcp_roll_side + kind: axis_cross_view_validation + view: side + parent_role: side_base + child_role: pinky_pip + validation_source: null + pose_axis_line_required: true + ring_mcp_roll_side: + joint: ring_mcp_roll_side + kind: axis_cross_view_validation + view: side + parent_role: side_base + child_role: ring_pip + validation_source: null + pose_axis_line_required: true + middle_mcp_roll_side: + joint: middle_mcp_roll_side + kind: axis_cross_view_validation + view: side + parent_role: side_base + child_role: middle_pip + validation_source: null + pose_axis_line_required: true + index_mcp_roll_side: + joint: index_mcp_roll_side + kind: axis_cross_view_validation + view: side + parent_role: side_base + child_role: index_pip + validation_source: null + pose_axis_line_required: true + cross_view_sources: + pinky_mcp_roll: pinky_mcp_roll_side + ring_mcp_roll: ring_mcp_roll_side + middle_mcp_roll: middle_mcp_roll_side + index_mcp_roll: index_mcp_roll_side + image_curve_joints: + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - ring_mcp_roll + - ring_mcp_pitch + - ring_pip + - pinky_mcp_roll + - pinky_mcp_pitch + - pinky_pip + directional_zero: true + cross_view_roll_curve: true + stable_cross_view_cone_bias: true + input_domain: feedback_u8 +zero: + active_joints: + - index_mcp_pitch + - index_mcp_roll + - index_pip + - middle_mcp_pitch + - middle_mcp_roll + - middle_pip + - pinky_mcp_pitch + - pinky_mcp_roll + - pinky_pip + - ring_mcp_pitch + - ring_mcp_roll + - ring_pip + - thumb_cmc_pitch + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_mcp + passive_joints: + - index_dip + - middle_dip + - pinky_dip + - ring_dip + - thumb_ip + direct_zero_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - ring_mcp_roll + - ring_mcp_pitch + - ring_pip + - pinky_mcp_roll + - pinky_mcp_pitch + - pinky_pip + axis_joints: &id001 + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - thumb_ip + - index_mcp_roll + - index_mcp_pitch + - index_pip + - index_dip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - middle_dip + - ring_mcp_roll + - ring_mcp_pitch + - ring_pip + - ring_dip + - pinky_mcp_roll + - pinky_mcp_pitch + - pinky_pip + - pinky_dip + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: + index_dip: index_pip + middle_dip: middle_pip + ring_dip: ring_pip + pinky_dip: pinky_pip + thumb_ip: thumb_mcp + cad_frozen_joints: + - index_dip + - middle_dip + - pinky_dip + - ring_dip + - thumb_ip + endpoint_anchor_by_joint: {} + fitted_mimic_joints: + - index_dip + - middle_dip + - pinky_dip + - ring_dip + - thumb_ip + coupling_model_by_joint: + index_dip: linear_mimic + middle_dip: linear_mimic + pinky_dip: linear_mimic + ring_dip: linear_mimic + thumb_ip: linear_mimic + spatial: + base_pose_strategy: full_hand + parallel_root_pattern: true + root_anchor_joints: + - thumb_cmc_roll + - index_mcp_roll + - middle_mcp_roll + - ring_mcp_roll + - pinky_mcp_roll + depth_free_axis_projection: true + axis_order: *id001 + axis_parent_joint: + thumb_cmc_yaw: thumb_cmc_roll + thumb_cmc_pitch: thumb_cmc_yaw + index_mcp_pitch: index_mcp_roll + middle_mcp_pitch: middle_mcp_roll + ring_mcp_pitch: ring_mcp_roll + pinky_mcp_pitch: pinky_mcp_roll + phase_parent_joint: + thumb_mcp: thumb_cmc_pitch + thumb_ip: thumb_mcp + index_pip: index_mcp_pitch + index_dip: index_pip + middle_pip: middle_mcp_pitch + middle_dip: middle_pip + ring_pip: ring_mcp_pitch + ring_dip: ring_pip + pinky_pip: pinky_mcp_pitch + pinky_dip: pinky_pip + offset_observer_joint: + thumb_cmc_roll: thumb_cmc_yaw + thumb_cmc_yaw: thumb_cmc_pitch + thumb_cmc_pitch: thumb_mcp + thumb_mcp: thumb_ip + 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 + ring_mcp_roll: ring_mcp_pitch + ring_mcp_pitch: ring_pip + ring_pip: ring_dip + pinky_mcp_roll: pinky_mcp_pitch + pinky_mcp_pitch: pinky_pip + pinky_pip: pinky_dip + same_view_axis_pair_by_offset: + thumb_cmc_yaw: + - thumb_cmc_roll + - thumb_cmc_pitch +quality: + training_cycles: + - 0 + - 1 + - 2 + holdout_cycle: 3 + hard_threshold_keys: + - maximum_axis_cycle_difference_rad + - maximum_pose_line_rms_m + - maximum_reprojection_error_px + - maximum_validation_error_rad + - minimum_detection_rate + retry_metric_scope: {} + isolated_holdout: true +scope: + calibrate_joints: + full: + - index_mcp_pitch + - index_mcp_roll + - index_pip + - middle_mcp_pitch + - middle_mcp_roll + - middle_pip + - pinky_mcp_pitch + - pinky_mcp_roll + - pinky_pip + - ring_mcp_pitch + - ring_mcp_roll + - ring_pip + - thumb_cmc_pitch + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_mcp + thumb: + - thumb_cmc_pitch + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_mcp + fingers: + - index_mcp_pitch + - index_mcp_roll + - index_pip + - middle_mcp_pitch + - middle_mcp_roll + - middle_pip + - pinky_mcp_pitch + - pinky_mcp_roll + - pinky_pip + - ring_mcp_pitch + - ring_mcp_roll + - ring_pip + frozen_joints: + full: [] + thumb: + - index_mcp_pitch + - index_mcp_roll + - index_pip + - middle_mcp_pitch + - middle_mcp_roll + - middle_pip + - pinky_mcp_pitch + - pinky_mcp_roll + - pinky_pip + - ring_mcp_pitch + - ring_mcp_roll + - ring_pip + fingers: + - thumb_cmc_pitch + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_mcp + default_scope: full +artifacts: + output_schema_version: 2 + calibration_filename: g20_{side}_{serial_number}_calibration.json + corrected_urdf_filename: linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf + protected_input_fields: + - calibration_config_sha256 + - camera_extrinsics_sha256 + - profile_config_sha256 + - source_urdf_sha256 + - tag_config_sha256 + publication_pointer: latest_passed + session_compatibility_tokens: + - cross_view_roll_curve + - directional_zero + - isolated_holdout + - measured_passive_dips + - palm_axis_relative_motion_v3 + - stable_cross_view_cone_bias + - steady_command_checkpoints + - urdf_zero_publication + publish_corrected_urdf: true +acquisition: + policy_version: unified_engine_v4_dual_mapping + mapping_probe_maximum_rad: 0.0 + automatic_rescan_limit: 1 + minimum_valid_samples: 40 + minimum_bins: 32 + maximum_unobserved_fraction: 0.0625 + legacy_minimum_span_01: 0.9411764705882353 + physical_first_cycle_minimum_span_01: 0.85 + physical_repeat_minimum_fraction: 0.9 + stall_timeout_seconds: 2.0 + feedback_stale_seconds: 1.0 + fixed_reference_minimum_frames: 10 + fixed_reference_maximum_drift_px: 5.0 + fixed_reference_confirmation_frames: 10 +urdf: + authorized_fields: + ring_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + index_pip: + - limit.lower + - limit.upper + - origin.rpy + index_mcp_roll: + - limit.lower + - limit.upper + - origin.rpy + thumb_cmc_yaw: + - limit.lower + - limit.upper + - origin.rpy + middle_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + index_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + ring_mcp_roll: + - limit.lower + - limit.upper + - origin.rpy + middle_mcp_roll: + - limit.lower + - limit.upper + - origin.rpy + ring_pip: + - limit.lower + - limit.upper + - origin.rpy + thumb_cmc_pitch: + - limit.lower + - limit.upper + - origin.rpy + thumb_cmc_roll: + - limit.lower + - limit.upper + - origin.rpy + pinky_pip: + - limit.lower + - limit.upper + - origin.rpy + middle_pip: + - limit.lower + - limit.upper + - origin.rpy + thumb_mcp: + - limit.lower + - limit.upper + - origin.rpy + pinky_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + pinky_mcp_roll: + - limit.lower + - limit.upper + - origin.rpy + thumb_ip: + - mimic.multiplier + - mimic.offset + index_dip: + - mimic.multiplier + - mimic.offset + middle_dip: + - mimic.multiplier + - mimic.offset + ring_dip: + - mimic.multiplier + - mimic.offset + pinky_dip: + - mimic.multiplier + - mimic.offset +joint_coverage: + index_mcp_pitch: measured_static_dynamic + index_mcp_roll: measured_static_dynamic + index_pip: measured_static_dynamic + middle_mcp_pitch: measured_static_dynamic + middle_mcp_roll: measured_static_dynamic + middle_pip: measured_static_dynamic + pinky_mcp_pitch: measured_static_dynamic + pinky_mcp_roll: measured_static_dynamic + pinky_pip: measured_static_dynamic + ring_mcp_pitch: measured_static_dynamic + ring_mcp_roll: measured_static_dynamic + ring_pip: measured_static_dynamic + thumb_cmc_pitch: measured_static_dynamic + thumb_cmc_roll: measured_static_dynamic + thumb_cmc_yaw: measured_static_dynamic + thumb_mcp: measured_static_dynamic + index_dip: measured_dynamic_cad_static + middle_dip: measured_dynamic_cad_static + pinky_dip: measured_dynamic_cad_static + ring_dip: measured_dynamic_cad_static + thumb_ip: measured_dynamic_cad_static diff --git a/src/linkerhand_calibration/config/profiles/l6_right_8.yaml b/src/linkerhand_calibration/config/profiles/l6_right_8.yaml new file mode 100644 index 0000000..675a626 --- /dev/null +++ b/src/linkerhand_calibration/config/profiles/l6_right_8.yaml @@ -0,0 +1,372 @@ +schema_version: 1 +profile_id: L6/right/l6_right_8/v1 +namespace: /l6_calibration +sdk_adapter: legacy_byte_sdk +command: + sdk_to_joint_direction: [-1, -1, -1, -1, -1, -1] + names: + - thumb_cmc_pitch + - thumb_cmc_roll + - index_mcp_pitch + - middle_mcp_pitch + - ring_mcp_pitch + - pinky_mcp_pitch + baseline_u8: + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + command_index_by_joint: + rh_thumb_cmc_pitch: 0 + rh_thumb_cmc_roll: 1 + rh_index_mcp_pitch: 2 + rh_middle_mcp_pitch: 3 + rh_ring_mcp_pitch: 4 + rh_pinky_mcp_pitch: 5 + disabled_indices: [] + urdf_joint_by_joint: + rh_thumb_cmc_pitch: rh_thumb_cmc_pitch + rh_thumb_cmc_roll: rh_thumb_cmc_roll + rh_index_mcp_pitch: rh_index_mcp_pitch + rh_middle_mcp_pitch: rh_middle_mcp_pitch + rh_ring_mcp_pitch: rh_ring_mcp_pitch + rh_pinky_mcp_pitch: rh_pinky_mcp_pitch + feedback_name_aliases: + thumb_cmc_yaw: thumb_cmc_roll + speed_slot_by_command_index: + '0': 0 + '1': 1 + '2': 2 + '3': 3 + '4': 4 + '5': 5 + unit: u8 + baseline: [] + lower_bounds: [] + upper_bounds: [] + feedback_lower_bounds: [] + feedback_upper_bounds: [] + feedback_by_index: false +vision: + views: + - name: front + tags: + - role: front_base + fixed_reference: true + id: 0 + - role: thumb_pitch + fixed_reference: false + id: 1 + - role: thumb_dip + fixed_reference: false + id: 2 + - name: side + tags: + - role: side_base + fixed_reference: true + id: 3 + - role: pinky_pitch + fixed_reference: false + id: 4 + - role: pinky_dip + fixed_reference: false + id: 5 + - name: top + tags: + - role: top_base + fixed_reference: true + id: 6 + - role: thumb_roll + fixed_reference: false + id: 7 + common_frame: calibration_common + extrinsic_reference_view: front + extrinsics_quality_limits: + reprojection_rms_px: 1.2 + maximum_rotation_repeatability_deg: 0.3 + maximum_translation_repeatability_m: 0.0015 + minimum_capture_counts: + front_side_captures: 15 + front_top_captures: 15 +motion: + tasks: + - key: thumb_roll_top + view: top + command_index: 1 + joints: + - rh_thumb_cmc_roll + auxiliary_commands: + - - 0 + - 255 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: 1 + formal_speed_u8: 1 + start: null + end: null + preflight_speed: null + formal_speed: null + - key: thumb_pitch_dip_front + view: front + command_index: 0 + joints: + - rh_thumb_cmc_pitch + - rh_thumb_dip + auxiliary_commands: + - - 1 + - 255 + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: 1 + formal_speed_u8: 1 + start: null + end: null + preflight_speed: null + formal_speed: null + - key: pinky_pitch_dip_side + view: side + command_index: 5 + joints: + - rh_pinky_mcp_pitch + - rh_pinky_dip + auxiliary_commands: [] + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: 1 + formal_speed_u8: 1 + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_waypoints_u8: [] + safe_return_waypoints_u8: [] + speed_parameters: + preflight_u8: 1 + formal_u8: 1 + speed_settle_seconds: 0.2 + command_trajectory_full_range_seconds: 6.0 + torque_u8: 80 + endpoint_hold_seconds: 1.0 + stall_timeout_seconds: 2.0 + precheck_sweeps: false + steady_command_checkpoints: false +measurement: + measurements: + rh_thumb_cmc_roll: + joint: rh_thumb_cmc_roll + kind: relative_rotation + view: top + parent_role: top_base + child_role: thumb_roll + validation_source: null + pose_axis_line_required: true + rh_thumb_cmc_pitch: + joint: rh_thumb_cmc_pitch + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_pitch + validation_source: null + pose_axis_line_required: true + rh_thumb_dip: + joint: rh_thumb_dip + kind: relative_rotation + view: front + parent_role: thumb_pitch + child_role: thumb_dip + validation_source: null + pose_axis_line_required: true + rh_pinky_mcp_pitch: + joint: rh_pinky_mcp_pitch + kind: relative_rotation + view: side + parent_role: side_base + child_role: pinky_pitch + validation_source: null + pose_axis_line_required: true + rh_pinky_dip: + joint: rh_pinky_dip + kind: relative_rotation + view: side + parent_role: pinky_pitch + child_role: pinky_dip + validation_source: null + pose_axis_line_required: true + cross_view_sources: {} + image_curve_joints: [] + directional_zero: true + cross_view_roll_curve: false + stable_cross_view_cone_bias: false +zero: + active_joints: + - rh_index_mcp_pitch + - rh_middle_mcp_pitch + - rh_pinky_mcp_pitch + - rh_ring_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_roll + passive_joints: + - rh_index_dip + - rh_middle_dip + - rh_pinky_dip + - rh_ring_dip + - rh_thumb_dip + direct_zero_joints: + - rh_pinky_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_roll + axis_joints: + - rh_pinky_dip + - rh_pinky_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_roll + - rh_thumb_dip + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: + rh_thumb_dip: rh_thumb_cmc_pitch + rh_index_dip: rh_index_mcp_pitch + rh_middle_dip: rh_middle_mcp_pitch + rh_ring_dip: rh_ring_mcp_pitch + rh_pinky_dip: rh_pinky_mcp_pitch + cad_frozen_joints: + - rh_index_dip + - rh_middle_dip + - rh_pinky_dip + - rh_ring_dip + - rh_thumb_dip + endpoint_anchor_by_joint: {} + fitted_mimic_joints: + - rh_pinky_dip + - rh_thumb_dip + coupling_model_by_joint: + rh_thumb_dip: linear_mimic + rh_pinky_dip: linear_mimic + rh_index_dip: linear_mimic + rh_middle_dip: linear_mimic + rh_ring_dip: linear_mimic + transferred_zero_sources: {rh_index_mcp_pitch: rh_pinky_mcp_pitch, rh_middle_mcp_pitch: rh_pinky_mcp_pitch, rh_ring_mcp_pitch: rh_pinky_mcp_pitch} + transferred_mimic_sources: {rh_index_dip: rh_pinky_dip, rh_middle_dip: rh_pinky_dip, rh_ring_dip: rh_pinky_dip} + spatial: + base_pose_strategy: thumb_serial + root_anchor_joints: [rh_thumb_cmc_roll] + orientation_anchor_joint: rh_pinky_mcp_pitch + directed_base_axis_joints: [rh_thumb_cmc_roll, rh_pinky_mcp_pitch] + depth_free_axis_projection: true + axis_order: [rh_thumb_cmc_roll, rh_thumb_cmc_pitch, rh_thumb_dip, rh_pinky_mcp_pitch, rh_pinky_dip] + axis_parent_joint: {rh_thumb_cmc_pitch: rh_thumb_cmc_roll} + phase_parent_joint: {rh_thumb_dip: rh_thumb_cmc_pitch, rh_pinky_dip: rh_pinky_mcp_pitch} + offset_observer_joint: {rh_thumb_cmc_roll: rh_thumb_cmc_pitch, rh_thumb_cmc_pitch: rh_thumb_dip, rh_pinky_mcp_pitch: rh_pinky_dip} +quality: + training_cycles: + - 0 + - 1 + - 2 + holdout_cycle: 3 + hard_threshold_keys: + - maximum_mimic_residual_rad + - maximum_state_image_skew_ms + - maximum_validation_error_rad + - minimum_detection_rate + retry_metric_scope: {} + isolated_holdout: true +scope: + calibrate_joints: + partial: + - rh_pinky_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_roll + frozen_joints: + partial: + - rh_index_mcp_pitch + - rh_middle_mcp_pitch + - rh_ring_mcp_pitch + default_scope: partial +artifacts: + output_schema_version: 2 + calibration_filename: l6_right_{serial_number}_partial_calibration.json + corrected_urdf_filename: linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf + protected_input_fields: + - calibration_config_sha256 + - camera_extrinsics_sha256 + - profile_config_sha256 + - source_urdf_sha256 + - tag_config_sha256 + publication_pointer: latest_partial_passed + session_compatibility_tokens: + - feedback_curves_v6 + - l6_partial_v1 + publish_corrected_urdf: true +acquisition: + policy_version: unified_engine_v4_dual_mapping + mapping_probe_maximum_rad: 0.0 + automatic_rescan_limit: 1 + minimum_valid_samples: 40 + minimum_bins: 32 + maximum_unobserved_fraction: 0.0625 + legacy_minimum_span_01: 0.9411764705882353 + physical_first_cycle_minimum_span_01: 0.85 + physical_repeat_minimum_fraction: 0.9 + stall_timeout_seconds: 2.0 + feedback_stale_seconds: 1.0 + fixed_reference_minimum_frames: 10 + fixed_reference_maximum_drift_px: 5.0 + fixed_reference_confirmation_frames: 10 +urdf: + authorized_fields: + rh_pinky_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_thumb_cmc_roll: + - limit.lower + - limit.upper + - origin.rpy + rh_index_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_thumb_cmc_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_middle_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_ring_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_ring_dip: + - mimic.multiplier + - mimic.offset + rh_thumb_dip: + - mimic.multiplier + - mimic.offset + rh_index_dip: + - mimic.multiplier + - mimic.offset + rh_pinky_dip: + - mimic.multiplier + - mimic.offset + rh_middle_dip: + - mimic.multiplier + - mimic.offset +joint_coverage: + rh_pinky_mcp_pitch: measured_static_dynamic + rh_thumb_cmc_roll: measured_static_dynamic + rh_thumb_cmc_pitch: measured_static_dynamic + rh_middle_mcp_pitch: transferred_static_dynamic + rh_index_mcp_pitch: transferred_static_dynamic + rh_ring_mcp_pitch: transferred_static_dynamic + rh_pinky_dip: measured_dynamic_cad_static + rh_middle_dip: transferred_dynamic_cad_static + rh_ring_dip: transferred_dynamic_cad_static + rh_thumb_dip: measured_dynamic_cad_static + rh_index_dip: transferred_dynamic_cad_static diff --git a/src/linkerhand_calibration/config/profiles/o12_right_16.yaml b/src/linkerhand_calibration/config/profiles/o12_right_16.yaml new file mode 100644 index 0000000..5cf99b8 --- /dev/null +++ b/src/linkerhand_calibration/config/profiles/o12_right_16.yaml @@ -0,0 +1,926 @@ +schema_version: 1 +profile_id: O12/right/o12_right_16/v1 +namespace: /o12_calibration +sdk_adapter: o12_hcan_sdk +command: + unit: rad + feedback_by_index: true + sdk_to_joint_direction: + - 1 + - -1 + - -1 + - -1 + - -1 + - 1 + - 1 + - -1 + - 1 + - 1 + - 1 + - 1 + names: + - thumb_roll + - thumb_abad + - thumb_mcp + - thumb_pip + - index_abad + - index_mcp + - index_pip + - middle_abad + - middle_mcp + - middle_pip + - ring_mcp + - pinky_mcp + baseline_u8: [] + baseline: + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + lower_bounds: + - 0 + - -1.387536755335492 + - -0.8272860654453121 + - -1.2915436464758039 + - -0.2617993877991494 + - 0 + - 0 + - -0.2617993877991494 + - 0 + - 0 + - 0 + - 0 + upper_bounds: + - 0.9424777960769379 + - 0 + - 0 + - 0 + - 0.2617993877991494 + - 1.3526301702956054 + - 1.530653753999027 + - 0.2617993877991494 + - 1.3578661580515883 + - 1.8151424220741028 + - 1.53588974175501 + - 1.53588974175501 + feedback_lower_bounds: + - -0.03490658503988659 + - -1.4224433403753787 + - -0.8621926504851987 + - -1.3264502315156905 + - -0.296705972839036 + - -0.03490658503988659 + - -0.03490658503988659 + - -0.296705972839036 + - -0.03490658503988659 + - -0.03490658503988659 + - -0.03490658503988659 + - -0.03490658503988659 + feedback_upper_bounds: + - 0.9773843811168246 + - 0.03490658503988659 + - 0.03490658503988659 + - 0.03490658503988659 + - 0.296705972839036 + - 1.387536755335492 + - 1.5655603390389137 + - 0.296705972839036 + - 1.392772743091475 + - 1.8500490071139892 + - 1.5707963267948966 + - 1.5707963267948966 + command_index_by_joint: + thumb_cmc_roll: 0 + thumb_cmc_yaw: 1 + thumb_cmc_pitch: 2 + thumb_mcp: 3 + index_mcp_roll: 4 + index_mcp_pitch: 5 + index_pip: 6 + middle_mcp_roll: 7 + middle_mcp_pitch: 8 + middle_pip: 9 + ring_mcp_pitch: 10 + pinky_mcp_pitch: 11 + urdf_joint_by_joint: + thumb_cmc_roll: thumb_cmc_roll + thumb_cmc_yaw: thumb_cmc_yaw + thumb_cmc_pitch: thumb_cmc_pitch + thumb_mcp: thumb_mcp + index_mcp_roll: index_mcp_roll + index_mcp_pitch: index_mcp_pitch + index_pip: index_pip + middle_mcp_roll: middle_mcp_roll + middle_mcp_pitch: middle_mcp_pitch + middle_pip: middle_pip + ring_mcp_pitch: ring_mcp_pitch + pinky_mcp_pitch: pinky_mcp_pitch + maximum_velocity: + - 0.16 + - 0.16 + - 0.1 + - 0.32 + - 0.12 + - 0.32 + - 0.32 + - 0.12 + - 0.32 + - 0.32 + - 0.32 + - 0.32 +vision: + common_frame: calibration_common + extrinsic_reference_view: front + extrinsics_quality_limits: + reprojection_rms_px: 2.0 + maximum_rotation_repeatability_deg: 0.3 + maximum_translation_repeatability_m: 0.0015 + minimum_capture_counts: + front_side_captures: 15 + front_top_captures: 15 + views: + - name: front + tags: + - role: front_base + id: 0 + fixed_reference: true + - role: thumb_cmc + id: 1 + - role: thumb_mcp + id: 2 + - role: thumb_dip + id: 3 + - role: middle_roll + id: 12 + - role: index_roll + id: 13 + - name: side + tags: + - role: side_base + id: 4 + fixed_reference: true + - role: pinky_mcp + id: 5 + - role: pinky_pip + id: 6 + - role: pinky_dip + id: 7 + - role: middle_pip + id: 8 + - role: middle_dip + id: 9 + - role: index_pip + id: 10 + - role: index_dip + id: 11 + - name: top + tags: + - role: top_base + id: 14 + fixed_reference: true + - role: thumb_yaw + id: 15 +motion: + speed_parameters: + command_rate_hz: 50 + clearance_flex_rad_s: 0.1 + clearance_splay_rad_s: 0.04 + probe_travel_rad: 0.05235987755982989 + probe_speed_rad_s: 0.02 + endpoint_hold_seconds: 1.0 + stall_timeout_seconds: 2.0 + baseline_rad_s: 0.2 + trajectory_ramp_seconds: 0.4 + tasks: + - key: thumb_pitch_front + view: front + command_index: 2 + joints: + - thumb_cmc_pitch + start: 0 + end: -0.8272860654453121 + preflight_speed: 0.06 + formal_speed: 0.1 + preparation_groups: + - - 0 + - 1 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 2 + - key: thumb_roll_front + view: front + command_index: 0 + joints: + - thumb_cmc_roll + start: 0 + end: 0.9424777960769379 + preflight_speed: 0.06 + formal_speed: 0.16 + preparation_groups: + - - 1 + - 2 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 0 + - key: thumb_mcp_dip_front + view: front + command_index: 3 + joints: + - thumb_mcp + - thumb_dip + start: 0 + end: -1.2915436464758039 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 0 + - 1 + - 2 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 3 + - key: thumb_yaw_top + view: top + command_index: 1 + joints: + - thumb_cmc_yaw + start: 0 + end: -1.387536755335492 + preflight_speed: 0.06 + formal_speed: 0.16 + preparation_groups: + - - 0 + - 2 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - 11 + - - 1 + - key: pinky_chain_side + view: side + command_index: 11 + joints: + - pinky_mcp_pitch + - pinky_pip + - pinky_dip + start: 0 + end: 1.53588974175501 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 0 + - 1 + - 2 + - 3 + - - 4 + - 7 + - - 5 + - 6 + - 8 + - 9 + - - 10 + - - 11 + - key: middle_roll_front + view: front + command_index: 7 + joints: + - middle_mcp_roll + auxiliary_commands: + - - 4 + - -0.17453292519943295 + - - 5 + - 0.11 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - 8 + - 0.17 + start: 0.2617993877991494 + end: -0.2617993877991494 + preflight_speed: 0.06 + formal_speed: 0.12 + preparation_groups: + - - 10 + - 11 + - - 5 + - 6 + - - 4 + - - 0 + - 1 + - 2 + - 3 + - - 8 + - 9 + - - 7 + - key: middle_mcp_side + view: side + command_index: 8 + joints: + - middle_mcp_pitch + auxiliary_commands: + - - 4 + - -0.17453292519943295 + - - 5 + - 0.11 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.3578661580515883 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 10 + - 11 + - - 5 + - 6 + - - 4 + - - 0 + - 1 + - 2 + - 3 + - - 7 + - 9 + - - 8 + - key: middle_pip_dip_side + view: side + command_index: 9 + joints: + - middle_pip + - middle_dip + auxiliary_commands: + - - 4 + - -0.17453292519943295 + - - 5 + - 0.11 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.8151424220741028 + preflight_speed: 0.06 + formal_speed: 0.32 + preparation_groups: + - - 10 + - 11 + - - 5 + - 6 + - - 4 + - - 0 + - 1 + - 2 + - 3 + - - 7 + - 8 + - - 9 + - key: index_roll_front + view: front + command_index: 4 + joints: + - index_mcp_roll + auxiliary_commands: + - - 7 + - 0 + - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - 5 + - 0.17 + start: 0.2617993877991494 + end: -0.2617993877991494 + preflight_speed: 0.06 + formal_speed: 0.12 + entry_waypoints: + - - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - - 4 + - 0 + - - 7 + - 0 + - - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + preparation_groups: + - - 10 + - 11 + - - 7 + - - 8 + - 9 + - - 0 + - 1 + - 2 + - 3 + - - 5 + - 6 + - - 4 + - key: index_mcp_side + view: side + command_index: 5 + joints: + - index_mcp_pitch + auxiliary_commands: + - - 7 + - 0 + - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.3526301702956054 + preflight_speed: 0.06 + formal_speed: 0.32 + entry_waypoints: + - - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - - 4 + - 0 + - - 7 + - 0 + - - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + preparation_groups: + - - 10 + - 11 + - - 7 + - - 8 + - 9 + - - 0 + - 1 + - 2 + - 3 + - - 4 + - 6 + - - 5 + - key: index_pip_dip_side + view: side + command_index: 6 + joints: + - index_pip + - index_dip + auxiliary_commands: + - - 7 + - 0 + - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + start: 0 + end: 1.530653753999027 + preflight_speed: 0.06 + formal_speed: 0.32 + entry_waypoints: + - - - 10 + - 1.53588974175501 + - - 11 + - 1.53588974175501 + - - - 4 + - 0 + - - 7 + - 0 + - - - 8 + - 1.3578661580515883 + - - 9 + - 1.8151424220741028 + preparation_groups: + - - 10 + - 11 + - - 7 + - - 8 + - 9 + - - 0 + - 1 + - 2 + - 3 + - - 4 + - 5 + - - 6 + return_groups: + - - 4 + - 7 + - - 8 + - 9 + - - 10 + - 11 + - - 0 + - 1 + - 2 + - 3 + - 5 + - 6 +measurement: + candidate_selection_tasks: + - thumb_mcp_dip_front + directional_zero: true + stable_cross_view_cone_bias: true + cross_view_sources: + middle_mcp_roll: middle_mcp_pitch + index_mcp_roll: index_mcp_pitch + measurements: + thumb_cmc_pitch: + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_cmc + thumb_cmc_roll: + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_cmc + thumb_mcp: + kind: relative_rotation + view: front + parent_role: thumb_cmc + child_role: thumb_mcp + thumb_dip: + kind: relative_rotation + view: front + parent_role: thumb_mcp + child_role: thumb_dip + pose_axis_line_required: false + thumb_cmc_yaw: + kind: relative_rotation + view: top + parent_role: top_base + child_role: thumb_yaw + pinky_mcp_pitch: + kind: relative_rotation + view: side + parent_role: side_base + child_role: pinky_mcp + pinky_pip: + kind: relative_rotation + view: side + parent_role: pinky_mcp + child_role: pinky_pip + pose_axis_line_required: false + pinky_dip: + kind: relative_rotation + view: side + parent_role: pinky_pip + child_role: pinky_dip + pose_axis_line_required: false + middle_mcp_roll: + kind: relative_rotation + view: front + parent_role: front_base + child_role: middle_roll + middle_mcp_pitch: + kind: relative_rotation + view: side + parent_role: side_base + child_role: middle_pip + middle_pip: + kind: relative_rotation + view: side + parent_role: side_base + child_role: middle_pip + middle_dip: + kind: relative_rotation + view: side + parent_role: middle_pip + child_role: middle_dip + pose_axis_line_required: false + index_mcp_roll: + kind: relative_rotation + view: front + parent_role: front_base + child_role: index_roll + index_mcp_pitch: + kind: relative_rotation + view: side + parent_role: side_base + child_role: index_pip + index_pip: + kind: relative_rotation + view: side + parent_role: side_base + child_role: index_pip + index_dip: + kind: relative_rotation + view: side + parent_role: index_pip + child_role: index_dip + pose_axis_line_required: false +zero: + active_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - ring_mcp_pitch + - pinky_mcp_pitch + passive_joints: + - thumb_dip + - index_dip + - middle_dip + - ring_pip + - ring_dip + - pinky_pip + - pinky_dip + direct_zero_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - pinky_mcp_pitch + axis_joints: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - pinky_mcp_pitch + - index_dip + - middle_dip + - pinky_pip + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: + thumb_dip: thumb_mcp + index_dip: index_pip + middle_dip: middle_pip + ring_pip: ring_mcp_pitch + ring_dip: ring_pip + pinky_pip: pinky_mcp_pitch + pinky_dip: pinky_pip + cad_frozen_joints: + - thumb_mcp + - thumb_dip + - index_dip + - middle_dip + - ring_pip + - ring_dip + - pinky_pip + - pinky_dip + fitted_mimic_joints: + - thumb_dip + - index_dip + - middle_dip + - pinky_pip + - pinky_dip + coupling_model_by_joint: + thumb_dip: linear_mimic + index_dip: linear_mimic + middle_dip: linear_mimic + pinky_pip: linear_mimic + pinky_dip: linear_mimic + transferred_zero_sources: + ring_mcp_pitch: pinky_mcp_pitch + spatial: + base_pose_strategy: thumb_serial + root_anchor_joints: + - thumb_cmc_roll + orientation_anchor_joint: pinky_mcp_pitch + directed_base_axis_joints: + - thumb_cmc_roll + - pinky_mcp_pitch + depth_free_axis_projection: true + accept_validated_zero_in_confidence_interval: true + project_axis_gauge_before_image: true + axis_order: + - 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 + axis_parent_joint: + 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_joint: + 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 + offset_observer_joint: + 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 +quality: + training_cycles: + - 0 + - 1 + - 2 + holdout_cycle: 3 + hard_threshold_keys: + - holdout + isolated_holdout: true +scope: + default_scope: full + calibrate_joints: + full: + - thumb_cmc_roll + - thumb_cmc_yaw + - thumb_cmc_pitch + - thumb_mcp + - index_mcp_roll + - index_mcp_pitch + - index_pip + - middle_mcp_roll + - middle_mcp_pitch + - middle_pip + - ring_mcp_pitch + - pinky_mcp_pitch + frozen_joints: + full: [] +artifacts: + output_schema_version: 2 + calibration_filename: o12_right_{serial_number}_calibration.json + corrected_urdf_filename: linkerhand_o12_right_{serial_number}_zero_calibrated.urdf + protected_input_fields: + - source_urdf_sha256 + - camera_extrinsics_sha256 + - calibration_config_sha256 + - tag_config_sha256 + - sdk_config_sha256 + - sdk_package_sha256 + - profile_config_sha256 + publication_pointer: latest_passed + session_compatibility_tokens: + - o12_right_16_v1 + - feedback_rad_v1 + - full_sdk_range_v2 + publish_corrected_urdf: true +acquisition: + policy_version: unified_engine_v4_dual_mapping + mapping_probe_maximum_rad: 0.05235987755982989 +urdf: + authorized_fields: + thumb_cmc_roll: + - origin.rpy + - limit.lower + - limit.upper + thumb_cmc_yaw: + - origin.rpy + - limit.lower + - limit.upper + thumb_cmc_pitch: + - origin.rpy + - limit.lower + - limit.upper + thumb_mcp: + - origin.rpy + - limit.lower + - limit.upper + index_mcp_roll: + - origin.rpy + - limit.lower + - limit.upper + index_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + index_pip: + - origin.rpy + - limit.lower + - limit.upper + middle_mcp_roll: + - origin.rpy + - limit.lower + - limit.upper + middle_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + middle_pip: + - origin.rpy + - limit.lower + - limit.upper + ring_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + pinky_mcp_pitch: + - origin.rpy + - limit.lower + - limit.upper + thumb_dip: + - mimic.multiplier + - mimic.offset + index_dip: + - mimic.multiplier + - mimic.offset + middle_dip: + - mimic.multiplier + - mimic.offset + pinky_pip: + - mimic.multiplier + - mimic.offset + pinky_dip: + - mimic.multiplier + - mimic.offset + ring_pip: + - mimic.offset + ring_dip: + - mimic.offset +joint_coverage: + thumb_cmc_roll: measured_static_dynamic + thumb_cmc_yaw: measured_static_dynamic + thumb_cmc_pitch: measured_static_dynamic + thumb_mcp: measured_dynamic_cad_static + thumb_dip: measured_dynamic_cad_static + index_mcp_roll: measured_static_dynamic + index_mcp_pitch: measured_static_dynamic + index_pip: measured_static_dynamic + index_dip: measured_dynamic_cad_static + middle_mcp_roll: measured_static_dynamic + middle_mcp_pitch: measured_static_dynamic + middle_pip: measured_static_dynamic + middle_dip: measured_dynamic_cad_static + ring_mcp_pitch: transferred_static_dynamic + ring_pip: mimic_nominal + ring_dip: mimic_nominal + pinky_mcp_pitch: measured_static_dynamic + pinky_pip: measured_dynamic_cad_static + pinky_dip: measured_dynamic_cad_static diff --git a/src/linkerhand_calibration/config/profiles/o6_right_8.yaml b/src/linkerhand_calibration/config/profiles/o6_right_8.yaml new file mode 100644 index 0000000..778ee50 --- /dev/null +++ b/src/linkerhand_calibration/config/profiles/o6_right_8.yaml @@ -0,0 +1,368 @@ +schema_version: 1 +profile_id: O6/right/o6_right_8/v1 +namespace: /o6_calibration +sdk_adapter: legacy_byte_sdk +command: + sdk_to_joint_direction: [-1, -1, -1, -1, -1, -1] + names: + - thumb_cmc_pitch + - thumb_cmc_yaw + - index_mcp_pitch + - middle_mcp_pitch + - ring_mcp_pitch + - pinky_mcp_pitch + baseline_u8: + - 255 + - 255 + - 255 + - 255 + - 255 + - 255 + command_index_by_joint: + rh_thumb_cmc_pitch: 0 + rh_thumb_cmc_yaw: 1 + rh_index_mcp_pitch: 2 + rh_middle_mcp_pitch: 3 + rh_ring_mcp_pitch: 4 + rh_pinky_mcp_pitch: 5 + disabled_indices: [] + urdf_joint_by_joint: + rh_thumb_cmc_pitch: rh_thumb_cmc_pitch + rh_thumb_cmc_yaw: rh_thumb_cmc_yaw + rh_index_mcp_pitch: rh_index_mcp_pitch + rh_middle_mcp_pitch: rh_middle_mcp_pitch + rh_ring_mcp_pitch: rh_ring_mcp_pitch + rh_pinky_mcp_pitch: rh_pinky_mcp_pitch + feedback_name_aliases: {} + speed_slot_by_command_index: + '0': 0 + '1': 1 + '2': 2 + '3': 3 + '4': 4 + '5': 5 + unit: u8 + baseline: [] + lower_bounds: [] + upper_bounds: [] + feedback_lower_bounds: [] + feedback_upper_bounds: [] + feedback_by_index: false +vision: + views: + - name: front + tags: + - role: front_base + fixed_reference: true + id: 0 + - role: thumb_pitch + fixed_reference: false + id: 1 + - role: thumb_ip + fixed_reference: false + id: 2 + - name: side + tags: + - role: side_base + fixed_reference: true + id: 3 + - role: pinky_pitch + fixed_reference: false + id: 4 + - role: pinky_dip + fixed_reference: false + id: 5 + - name: top + tags: + - role: top_base + fixed_reference: true + id: 6 + - role: thumb_yaw + fixed_reference: false + id: 7 + common_frame: calibration_common + extrinsic_reference_view: front + extrinsics_quality_limits: + reprojection_rms_px: 1.5 + maximum_rotation_repeatability_deg: 0.3 + maximum_translation_repeatability_m: 0.0015 + minimum_capture_counts: + front_side_captures: 15 + front_top_captures: 15 +motion: + tasks: + - key: thumb_yaw_top + view: top + command_index: 1 + joints: + - rh_thumb_cmc_yaw + auxiliary_commands: [] + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: 60 + formal_speed_u8: 40 + start: null + end: null + preflight_speed: null + formal_speed: null + - key: thumb_pitch_ip_front + view: front + command_index: 0 + joints: + - rh_thumb_cmc_pitch + - rh_thumb_ip + auxiliary_commands: [] + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: 60 + formal_speed_u8: 40 + start: null + end: null + preflight_speed: null + formal_speed: null + - key: pinky_pitch_dip_side + view: side + command_index: 5 + joints: + - rh_pinky_mcp_pitch + - rh_pinky_dip + auxiliary_commands: [] + validation_only: false + start_u8: 255 + end_u8: 0 + preflight_speed_u8: 60 + formal_speed_u8: 40 + start: null + end: null + preflight_speed: null + formal_speed: null + preparation_waypoints_u8: [] + safe_return_waypoints_u8: [] + speed_parameters: + baseline_u8: 80 + preflight_u8: 60 + formal_u8: 40 + speed_settle_seconds: 0.2 + command_trajectory_full_range_seconds: 6.0 + torque_u8: 80 + endpoint_hold_seconds: 1.0 + stall_timeout_seconds: 2.0 + precheck_sweeps: false + steady_command_checkpoints: false +measurement: + measurements: + rh_thumb_cmc_yaw: + joint: rh_thumb_cmc_yaw + kind: relative_rotation + view: top + parent_role: top_base + child_role: thumb_yaw + validation_source: null + pose_axis_line_required: true + rh_thumb_cmc_pitch: + joint: rh_thumb_cmc_pitch + kind: relative_rotation + view: front + parent_role: front_base + child_role: thumb_pitch + validation_source: null + pose_axis_line_required: true + rh_thumb_ip: + joint: rh_thumb_ip + kind: relative_rotation + view: front + parent_role: thumb_pitch + child_role: thumb_ip + validation_source: null + pose_axis_line_required: true + rh_pinky_mcp_pitch: + joint: rh_pinky_mcp_pitch + kind: relative_rotation + view: side + parent_role: side_base + child_role: pinky_pitch + validation_source: null + pose_axis_line_required: true + rh_pinky_dip: + joint: rh_pinky_dip + kind: relative_rotation + view: side + parent_role: pinky_pitch + child_role: pinky_dip + validation_source: null + pose_axis_line_required: true + cross_view_sources: {} + image_curve_joints: [] + directional_zero: true + cross_view_roll_curve: false + stable_cross_view_cone_bias: false +zero: + active_joints: + - rh_index_mcp_pitch + - rh_middle_mcp_pitch + - rh_pinky_mcp_pitch + - rh_ring_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_yaw + passive_joints: + - rh_index_dip + - rh_middle_dip + - rh_pinky_dip + - rh_ring_dip + - rh_thumb_ip + direct_zero_joints: + - rh_pinky_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_yaw + axis_joints: + - rh_pinky_dip + - rh_pinky_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_yaw + - rh_thumb_ip + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: + rh_thumb_ip: rh_thumb_cmc_pitch + rh_index_dip: rh_index_mcp_pitch + rh_middle_dip: rh_middle_mcp_pitch + rh_ring_dip: rh_ring_mcp_pitch + rh_pinky_dip: rh_pinky_mcp_pitch + cad_frozen_joints: + - rh_index_dip + - rh_middle_dip + - rh_pinky_dip + - rh_ring_dip + - rh_thumb_ip + endpoint_anchor_by_joint: {} + fitted_mimic_joints: + - rh_pinky_dip + - rh_thumb_ip + coupling_model_by_joint: + rh_thumb_ip: linear_mimic + rh_index_dip: linear_mimic + rh_middle_dip: linear_mimic + rh_ring_dip: linear_mimic + rh_pinky_dip: linear_mimic + transferred_zero_sources: {rh_index_mcp_pitch: rh_pinky_mcp_pitch, rh_middle_mcp_pitch: rh_pinky_mcp_pitch, rh_ring_mcp_pitch: rh_pinky_mcp_pitch} + transferred_mimic_sources: {rh_index_dip: rh_pinky_dip, rh_middle_dip: rh_pinky_dip, rh_ring_dip: rh_pinky_dip} + spatial: + base_pose_strategy: thumb_serial + root_anchor_joints: [rh_thumb_cmc_yaw] + orientation_anchor_joint: rh_pinky_mcp_pitch + directed_base_axis_joints: [rh_thumb_cmc_yaw, rh_pinky_mcp_pitch] + depth_free_axis_projection: true + axis_order: [rh_thumb_cmc_yaw, rh_thumb_cmc_pitch, rh_thumb_ip, rh_pinky_mcp_pitch, rh_pinky_dip] + axis_parent_joint: {rh_thumb_cmc_pitch: rh_thumb_cmc_yaw} + phase_parent_joint: {rh_thumb_ip: rh_thumb_cmc_pitch, rh_pinky_dip: rh_pinky_mcp_pitch} + offset_observer_joint: {rh_thumb_cmc_yaw: rh_thumb_cmc_pitch, rh_thumb_cmc_pitch: rh_thumb_ip, rh_pinky_mcp_pitch: rh_pinky_dip} +quality: + training_cycles: + - 0 + - 1 + - 2 + holdout_cycle: 3 + hard_threshold_keys: + - maximum_mimic_residual_rad + - maximum_state_image_skew_ms + - maximum_validation_error_rad + - minimum_detection_rate + retry_metric_scope: {} + isolated_holdout: true +scope: + calibrate_joints: + partial: + - rh_pinky_mcp_pitch + - rh_thumb_cmc_pitch + - rh_thumb_cmc_yaw + frozen_joints: + partial: + - rh_index_mcp_pitch + - rh_middle_mcp_pitch + - rh_ring_mcp_pitch + default_scope: partial +artifacts: + output_schema_version: 2 + calibration_filename: o6_right_{serial_number}_partial_calibration.json + corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf + protected_input_fields: + - calibration_config_sha256 + - camera_extrinsics_sha256 + - profile_config_sha256 + - source_urdf_sha256 + - tag_config_sha256 + publication_pointer: latest_partial_passed + session_compatibility_tokens: + - feedback_curves_v6 + - o6_partial_v1 + publish_corrected_urdf: true +acquisition: + policy_version: unified_engine_v4_dual_mapping + mapping_probe_maximum_rad: 0.0 + automatic_rescan_limit: 1 + minimum_valid_samples: 40 + minimum_bins: 32 + maximum_unobserved_fraction: 0.0625 + legacy_minimum_span_01: 0.9411764705882353 + physical_first_cycle_minimum_span_01: 0.85 + physical_repeat_minimum_fraction: 0.9 + stall_timeout_seconds: 2.0 + feedback_stale_seconds: 1.0 + fixed_reference_minimum_frames: 10 + fixed_reference_maximum_drift_px: 5.0 + fixed_reference_confirmation_frames: 10 +urdf: + authorized_fields: + rh_pinky_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_index_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_thumb_cmc_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_middle_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_ring_mcp_pitch: + - limit.lower + - limit.upper + - origin.rpy + rh_thumb_cmc_yaw: + - limit.lower + - limit.upper + - origin.rpy + rh_ring_dip: + - mimic.multiplier + - mimic.offset + rh_index_dip: + - mimic.multiplier + - mimic.offset + rh_pinky_dip: + - mimic.multiplier + - mimic.offset + rh_thumb_ip: + - mimic.multiplier + - mimic.offset + rh_middle_dip: + - mimic.multiplier + - mimic.offset +joint_coverage: + rh_pinky_mcp_pitch: measured_static_dynamic + rh_thumb_cmc_pitch: measured_static_dynamic + rh_middle_mcp_pitch: transferred_static_dynamic + rh_index_mcp_pitch: transferred_static_dynamic + rh_ring_mcp_pitch: transferred_static_dynamic + rh_thumb_cmc_yaw: measured_static_dynamic + rh_pinky_dip: measured_dynamic_cad_static + rh_thumb_ip: measured_dynamic_cad_static + rh_middle_dip: transferred_dynamic_cad_static + rh_ring_dip: transferred_dynamic_cad_static + rh_index_dip: transferred_dynamic_cad_static diff --git a/src/linkerhand_calibration/config/schemas/hand_profile.schema.yaml b/src/linkerhand_calibration/config/schemas/hand_profile.schema.yaml new file mode 100644 index 0000000..c194df5 --- /dev/null +++ b/src/linkerhand_calibration/config/schemas/hand_profile.schema.yaml @@ -0,0 +1,44 @@ +$schema: https://json-schema.org/draft/2020-12/schema +title: LinkerHand calibration profile +type: object +additionalProperties: false +required: + - schema_version + - profile_id + - namespace + - sdk_adapter + - command + - vision + - motion + - measurement + - zero + - quality + - scope + - artifacts + - urdf +properties: + schema_version: {const: 1} + profile_id: {type: string, pattern: "^[A-Za-z0-9_-]+/(left|right)/[a-z0-9_-]+/v[1-9][0-9]*$"} + namespace: {type: string, pattern: "^/[^/].*[^/]$"} + sdk_adapter: {type: string, minLength: 1} + command: {type: object} + vision: {type: object} + motion: {type: object} + measurement: {type: object} + zero: {type: object} + quality: {type: object} + scope: {type: object} + artifacts: {type: object} + acquisition: {type: object} + urdf: + type: object + required: [authorized_fields] + properties: + authorized_fields: + type: object + additionalProperties: + type: array + uniqueItems: true + items: + enum: [origin.rpy, limit.lower, limit.upper, mimic.multiplier, mimic.offset] + joint_coverage: {type: object} diff --git a/src/linkerhand_calibration/config/schemas/product.schema.yaml b/src/linkerhand_calibration/config/schemas/product.schema.yaml new file mode 100644 index 0000000..f58ae27 --- /dev/null +++ b/src/linkerhand_calibration/config/schemas/product.schema.yaml @@ -0,0 +1,14 @@ +$schema: https://json-schema.org/draft/2020-12/schema +title: LinkerHand calibration product +type: object +required: [schema_version, profile_id, serial_number, cameras, artifacts] +properties: + schema_version: {type: integer, minimum: 2} + profile_id: {type: string} + profile_config: {type: string} + serial_number: {type: string, pattern: "^[A-Za-z0-9_.-]+$"} + namespace: {type: string} + sdk: {type: object} + cameras: {type: object, minProperties: 1} + artifacts: {type: object} + release: {type: object} diff --git a/src/linkerhand_calibration/config/three_camera_calibration.yaml b/src/linkerhand_calibration/config/three_camera_calibration.yaml index 8881a26..6ffc19f 100644 --- a/src/linkerhand_calibration/config/three_camera_calibration.yaml +++ b/src/linkerhand_calibration/config/three_camera_calibration.yaml @@ -45,7 +45,6 @@ g20_calibration: minimum_edge_pixels: 30.0 pnp_maximum_reprojection_error_px: 1.5 - pnp_reprojection_tie_px: 1.5 pnp_maximum_pose_jump_deg: 35.0 pnp_maximum_translation_jump_m: 0.04 pnp_maximum_tag_tilt_deg: 75.0 @@ -110,7 +109,7 @@ g20_calibration: # roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。 baseline_hold_seconds: 0.5 minimum_baseline_hold_frames: 10 - # unified_engine_v1 不执行每任务全行程预检；保留参数仅兼容旧配置读取。 + # unified_engine_v2 不执行每任务全行程预检；保留参数仅兼容旧配置读取。 task_precheck_hold_seconds: 2.0 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 diff --git a/src/linkerhand_calibration/launch/front_cmc_pitch_zero.launch.py b/src/linkerhand_calibration/launch/front_cmc_pitch_zero.launch.py deleted file mode 100644 index 3e6f19c..0000000 --- a/src/linkerhand_calibration/launch/front_cmc_pitch_zero.launch.py +++ /dev/null @@ -1,272 +0,0 @@ -"""Launch front-camera trajectory-circle CMC pitch zero measurement.""" - -from __future__ import annotations - -from datetime import datetime -from pathlib import Path -import re - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - LogInfo, - OpaqueFunction, - SetEnvironmentVariable, -) -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode -from launch_ros.parameter_descriptions import ParameterValue - - -def _launch_stack(context): - serial_number = LaunchConfiguration("serial_number").perform(context) - if ( - not serial_number - or serial_number == "UNSET" - or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None - or serial_number in {".", ".."} - ): - raise RuntimeError( - "serial_number is required and may contain only letters, " - "digits, dot, underscore and dash" - ) - - requested_session = LaunchConfiguration("session_dir").perform(context) - output_root = Path( - LaunchConfiguration("output_root").perform(context) - ).expanduser().resolve() - if requested_session: - session_dir = Path(requested_session).expanduser().resolve() - else: - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session_dir = output_root / serial_number / timestamp - session_dir.mkdir(parents=True, exist_ok=True) - - tag_config = LaunchConfiguration("tag_config").perform(context) - zero_config = LaunchConfiguration("zero_config").perform(context) - camera = Node( - package="linkerhand_calibration", - executable="hikrobot_camera_node", - name="hikrobot_camera", - namespace="/camera/camera/color", - output="screen", - emulate_tty=True, - condition=IfCondition(LaunchConfiguration("start_camera")), - parameters=[ - { - "serial_number": LaunchConfiguration("camera_serial_number"), - "expected_model": LaunchConfiguration("camera_model"), - "camera_name": LaunchConfiguration("camera_name"), - "frame_id": LaunchConfiguration("camera_frame_id"), - "image_width": ParameterValue( - LaunchConfiguration("image_width"), value_type=int - ), - "image_height": ParameterValue( - LaunchConfiguration("image_height"), value_type=int - ), - "frame_rate": ParameterValue( - LaunchConfiguration("camera_frame_rate"), value_type=float - ), - "exposure_time_us": ParameterValue( - LaunchConfiguration("exposure_time_us"), value_type=float - ), - "gain_db": ParameterValue( - LaunchConfiguration("gain_db"), value_type=float - ), - "auto_exposure": ParameterValue( - LaunchConfiguration("auto_exposure"), value_type=bool - ), - "camera_info_url": LaunchConfiguration("camera_info_url"), - } - ], - ) - - raw_topic = "/camera/camera/color/image_raw" - camera_info_topic = "/camera/camera/color/camera_info" - rect_topic = "/camera/camera/color/image_rect" - vision_container = ComposableNodeContainer( - name="g20_thumb_zero_vision_container", - namespace="/", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="image_proc", - plugin="image_proc::RectifyNode", - name="rectify_color", - namespace="/camera/camera/color", - remappings=[ - ("image", raw_topic), - ("camera_info", camera_info_topic), - ("image_rect", rect_topic), - ], - parameters=[{"queue_size": 1}], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="apriltag_ros", - plugin="AprilTagNode", - name="apriltag", - namespace="/apriltag", - parameters=[ - tag_config, - { - "detector.decimate": ParameterValue( - LaunchConfiguration("apriltag_decimate"), - value_type=float, - ) - }, - ], - remappings=[ - ("image_rect", rect_topic), - ("camera_info", camera_info_topic), - ], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - emulate_tty=True, - ) - - sdk = Node( - package="linker_hand_ros2_sdk", - executable="linker_hand_sdk", - name="linker_hand_sdk", - output="screen", - condition=IfCondition(LaunchConfiguration("start_sdk")), - parameters=[ - { - "hand_type": "left", - "hand_joint": "G20", - "can": LaunchConfiguration("can_interface"), - "modbus": "None", - "topic_prefix": "/g20", - "move_on_startup": False, - "startup_speed": ParameterValue( - LaunchConfiguration("calibration_speed"), - value_type=int, - ), - "startup_torque": 80, - "state_poll_rate": 10.0, - "repeat_position_commands": False, - "is_touch": False, - } - ], - ) - - zero_node = Node( - package="linkerhand_calibration", - executable="cmc_pitch_zero_node", - name="g20_thumb_cmc_pitch_zero", - output="screen", - parameters=[ - zero_config, - { - "serial_number": serial_number, - "session_dir": str(session_dir), - "commands_enabled": ParameterValue( - LaunchConfiguration("commands_enabled"), - value_type=bool, - ), - "image_topic": rect_topic, - "publish_debug_image": ParameterValue( - LaunchConfiguration("publish_debug_image"), - value_type=bool, - ), - }, - ], - ) - - return [ - LogInfo(msg=f"G20 CMC pitch zero session: {session_dir}"), - LogInfo( - msg=( - "Only T0(ID 0) and T3(ID 1) are required; " - "T4/T5 detections are ignored" - ) - ), - LogInfo( - msg=( - "Motor 0 performs three 255->64->255 sweeps; " - "zero angles come from the fitted T3-centre trajectory radius" - ) - ), - camera, - vision_container, - sdk, - zero_node, - ] - - -def generate_launch_description() -> LaunchDescription: - package_share = Path( - get_package_share_directory("linkerhand_calibration") - ) - return LaunchDescription( - [ - SetEnvironmentVariable( - name="FASTRTPS_DEFAULT_PROFILES_FILE", - value=str( - package_share / "config" / "fastdds_large_images.xml" - ), - ), - DeclareLaunchArgument("serial_number", default_value="UNSET"), - DeclareLaunchArgument( - "camera_serial_number", default_value="DB2163742" - ), - DeclareLaunchArgument( - "camera_model", default_value="MV-CS020-10UM" - ), - DeclareLaunchArgument( - "camera_name", default_value="hikrobot_front_DB2163742" - ), - DeclareLaunchArgument( - "camera_frame_id", default_value="camera_color_optical_frame" - ), - DeclareLaunchArgument("image_width", default_value="1624"), - DeclareLaunchArgument("image_height", default_value="1240"), - DeclareLaunchArgument("camera_frame_rate", default_value="30.0"), - DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), - DeclareLaunchArgument("gain_db", default_value="0.0"), - DeclareLaunchArgument("auto_exposure", default_value="false"), - DeclareLaunchArgument( - "camera_info_url", - default_value=str( - Path.home() - / ".ros" - / "camera_info" - / "hikrobot_DB2163742.yaml" - ), - ), - DeclareLaunchArgument("can_interface", default_value="can0"), - DeclareLaunchArgument("calibration_speed", default_value="15"), - DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), - DeclareLaunchArgument("commands_enabled", default_value="true"), - DeclareLaunchArgument( - "publish_debug_image", default_value="true" - ), - DeclareLaunchArgument("start_camera", default_value="true"), - DeclareLaunchArgument("start_sdk", default_value="true"), - DeclareLaunchArgument( - "output_root", - default_value=str(Path.cwd() / "calibration_output"), - ), - DeclareLaunchArgument("session_dir", default_value=""), - DeclareLaunchArgument( - "zero_config", - default_value=str( - package_share / "config" / "cmc_pitch_zero.yaml" - ), - ), - DeclareLaunchArgument( - "tag_config", - default_value=str( - package_share / "config" / "front_tags.yaml" - ), - ), - OpaqueFunction(function=_launch_stack), - ] - ) diff --git a/src/linkerhand_calibration/launch/front_cmc_roll_calibration.launch.py b/src/linkerhand_calibration/launch/front_cmc_roll_calibration.launch.py deleted file mode 100644 index 4c8c10b..0000000 --- a/src/linkerhand_calibration/launch/front_cmc_roll_calibration.launch.py +++ /dev/null @@ -1,276 +0,0 @@ -"""Launch front-camera trajectory-circle CMC roll zero/travel calibration.""" - -from __future__ import annotations - -from datetime import datetime -from pathlib import Path -import re - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - LogInfo, - OpaqueFunction, - SetEnvironmentVariable, -) -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode -from launch_ros.parameter_descriptions import ParameterValue - - -def _launch_stack(context): - serial_number = LaunchConfiguration("serial_number").perform(context) - if ( - not serial_number - or serial_number == "UNSET" - or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None - or serial_number in {".", ".."} - ): - raise RuntimeError( - "serial_number is required and may contain only letters, " - "digits, dot, underscore and dash" - ) - - requested_session = LaunchConfiguration("session_dir").perform(context) - output_root = Path( - LaunchConfiguration("output_root").perform(context) - ).expanduser().resolve() - if requested_session: - session_dir = Path(requested_session).expanduser().resolve() - else: - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session_dir = output_root / serial_number / timestamp - session_dir.mkdir(parents=True, exist_ok=True) - - tag_config = LaunchConfiguration("tag_config").perform(context) - calibration_config = LaunchConfiguration( - "calibration_config" - ).perform(context) - camera = Node( - package="linkerhand_calibration", - executable="hikrobot_camera_node", - name="hikrobot_camera", - namespace="/camera/camera/color", - output="screen", - emulate_tty=True, - condition=IfCondition(LaunchConfiguration("start_camera")), - parameters=[ - { - "serial_number": LaunchConfiguration("camera_serial_number"), - "expected_model": LaunchConfiguration("camera_model"), - "camera_name": LaunchConfiguration("camera_name"), - "frame_id": LaunchConfiguration("camera_frame_id"), - "image_width": ParameterValue( - LaunchConfiguration("image_width"), value_type=int - ), - "image_height": ParameterValue( - LaunchConfiguration("image_height"), value_type=int - ), - "frame_rate": ParameterValue( - LaunchConfiguration("camera_frame_rate"), value_type=float - ), - "exposure_time_us": ParameterValue( - LaunchConfiguration("exposure_time_us"), value_type=float - ), - "gain_db": ParameterValue( - LaunchConfiguration("gain_db"), value_type=float - ), - "auto_exposure": ParameterValue( - LaunchConfiguration("auto_exposure"), value_type=bool - ), - "camera_info_url": LaunchConfiguration("camera_info_url"), - } - ], - ) - - raw_topic = "/camera/camera/color/image_raw" - camera_info_topic = "/camera/camera/color/camera_info" - rect_topic = "/camera/camera/color/image_rect" - vision_container = ComposableNodeContainer( - name="g20_thumb_roll_vision_container", - namespace="/", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="image_proc", - plugin="image_proc::RectifyNode", - name="rectify_color", - namespace="/camera/camera/color", - remappings=[ - ("image", raw_topic), - ("camera_info", camera_info_topic), - ("image_rect", rect_topic), - ], - parameters=[{"queue_size": 1}], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="apriltag_ros", - plugin="AprilTagNode", - name="apriltag", - namespace="/apriltag", - parameters=[ - tag_config, - { - "detector.decimate": ParameterValue( - LaunchConfiguration("apriltag_decimate"), - value_type=float, - ) - }, - ], - remappings=[ - ("image_rect", rect_topic), - ("camera_info", camera_info_topic), - ], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - emulate_tty=True, - ) - - sdk = Node( - package="linker_hand_ros2_sdk", - executable="linker_hand_sdk", - name="linker_hand_sdk", - output="screen", - condition=IfCondition(LaunchConfiguration("start_sdk")), - parameters=[ - { - "hand_type": "left", - "hand_joint": "G20", - "can": LaunchConfiguration("can_interface"), - "modbus": "None", - "topic_prefix": "/g20", - "move_on_startup": False, - "startup_speed": ParameterValue( - LaunchConfiguration("calibration_speed"), - value_type=int, - ), - "startup_torque": 80, - "state_poll_rate": 10.0, - "repeat_position_commands": False, - "is_touch": False, - } - ], - ) - - calibration_node = Node( - package="linkerhand_calibration", - executable="cmc_roll_calibration_node", - name="g20_thumb_cmc_roll_calibration", - output="screen", - parameters=[ - calibration_config, - { - "serial_number": serial_number, - "session_dir": str(session_dir), - "commands_enabled": ParameterValue( - LaunchConfiguration("commands_enabled"), - value_type=bool, - ), - "image_topic": rect_topic, - "publish_debug_image": ParameterValue( - LaunchConfiguration("publish_debug_image"), - value_type=bool, - ), - }, - ], - ) - - return [ - LogInfo(msg=f"G20 CMC roll calibration session: {session_dir}"), - LogInfo( - msg=( - "Only T0(ID 0) and T3(ID 1) are required; " - "T4/T5 detections are ignored" - ) - ), - LogInfo( - msg=( - "Motor 5 performs three 255->0->255 sweeps; " - "static captures measure both zero and angular travel" - ) - ), - camera, - vision_container, - sdk, - calibration_node, - ] - - -def generate_launch_description() -> LaunchDescription: - package_share = Path( - get_package_share_directory("linkerhand_calibration") - ) - return LaunchDescription( - [ - SetEnvironmentVariable( - name="FASTRTPS_DEFAULT_PROFILES_FILE", - value=str( - package_share / "config" / "fastdds_large_images.xml" - ), - ), - DeclareLaunchArgument("serial_number", default_value="UNSET"), - DeclareLaunchArgument( - "camera_serial_number", default_value="DB2163742" - ), - DeclareLaunchArgument( - "camera_model", default_value="MV-CS020-10UM" - ), - DeclareLaunchArgument( - "camera_name", default_value="hikrobot_front_DB2163742" - ), - DeclareLaunchArgument( - "camera_frame_id", default_value="camera_color_optical_frame" - ), - DeclareLaunchArgument("image_width", default_value="1624"), - DeclareLaunchArgument("image_height", default_value="1240"), - DeclareLaunchArgument("camera_frame_rate", default_value="30.0"), - DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), - DeclareLaunchArgument("gain_db", default_value="0.0"), - DeclareLaunchArgument("auto_exposure", default_value="false"), - DeclareLaunchArgument( - "camera_info_url", - default_value=str( - Path.home() - / ".ros" - / "camera_info" - / "hikrobot_DB2163742.yaml" - ), - ), - DeclareLaunchArgument("can_interface", default_value="can0"), - DeclareLaunchArgument("calibration_speed", default_value="15"), - DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), - DeclareLaunchArgument("commands_enabled", default_value="true"), - DeclareLaunchArgument( - "publish_debug_image", default_value="true" - ), - DeclareLaunchArgument("start_camera", default_value="true"), - DeclareLaunchArgument("start_sdk", default_value="true"), - DeclareLaunchArgument( - "output_root", - default_value=str(Path.cwd() / "calibration_output"), - ), - DeclareLaunchArgument("session_dir", default_value=""), - DeclareLaunchArgument( - "calibration_config", - default_value=str( - package_share - / "config" - / "cmc_roll_zero_travel.yaml" - ), - ), - DeclareLaunchArgument( - "tag_config", - default_value=str( - package_share / "config" / "front_tags.yaml" - ), - ), - OpaqueFunction(function=_launch_stack), - ] - ) diff --git a/src/linkerhand_calibration/launch/front_thumb_calibration.launch.py b/src/linkerhand_calibration/launch/front_thumb_calibration.launch.py deleted file mode 100644 index 3c3284d..0000000 --- a/src/linkerhand_calibration/launch/front_thumb_calibration.launch.py +++ /dev/null @@ -1,391 +0,0 @@ -"""Launch the complete front-camera G20 thumb calibration stack.""" - -from __future__ import annotations - -from datetime import datetime -from pathlib import Path -import re - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - LogInfo, - OpaqueFunction, - SetEnvironmentVariable, -) -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode -from launch_ros.parameter_descriptions import ParameterValue - - -def _launch_stack(context): - serial_number = LaunchConfiguration("serial_number").perform(context) - if not serial_number or serial_number == "UNSET": - raise RuntimeError( - "serial_number is required, for example serial_number:=G20_LEFT_001" - ) - if ( - re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None - or serial_number in {".", ".."} - ): - raise RuntimeError( - "serial_number may contain only letters, digits, dot, underscore and dash" - ) - requested_session = LaunchConfiguration("session_dir").perform(context) - output_root = Path(LaunchConfiguration("output_root").perform(context)).resolve() - if requested_session: - session_dir = Path(requested_session).expanduser().resolve() - else: - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session_dir = output_root / serial_number / timestamp - session_dir.mkdir(parents=True, exist_ok=True) - bag_path = session_dir / "rosbag" - - calibration_config = LaunchConfiguration("calibration_config").perform(context) - tag_config = LaunchConfiguration("tag_config").perform(context) - use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower() - if use_roi_text not in {"true", "false"}: - raise RuntimeError("use_roi must be true or false") - use_roi = use_roi_text == "true" - - roi_values = {} - for name in ("roi_x", "roi_y", "roi_width", "roi_height"): - text = LaunchConfiguration(name).perform(context) - try: - roi_values[name] = int(text) - except ValueError as error: - raise RuntimeError(f"{name} must be an integer") from error - if roi_values["roi_x"] < 0 or roi_values["roi_y"] < 0: - raise RuntimeError("roi_x and roi_y must be non-negative") - if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0: - raise RuntimeError("roi_width and roi_height must be positive") - - try: - image_width = int(LaunchConfiguration("image_width").perform(context)) - image_height = int(LaunchConfiguration("image_height").perform(context)) - except ValueError as error: - raise RuntimeError("image_width and image_height must be integers") from error - if image_width <= 0 or image_height <= 0: - raise RuntimeError("image_width and image_height must be positive") - if use_roi: - if ( - roi_values["roi_x"] + roi_values["roi_width"] > image_width - or roi_values["roi_y"] + roi_values["roi_height"] > image_height - ): - raise RuntimeError( - "ROI lies outside camera image " - f"{image_width}x{image_height}" - ) - - camera = Node( - package="linkerhand_calibration", - executable="hikrobot_camera_node", - name="hikrobot_camera", - namespace="/camera/camera/color", - output="screen", - emulate_tty=True, - condition=IfCondition(LaunchConfiguration("start_camera")), - parameters=[ - { - "serial_number": LaunchConfiguration("camera_serial_number"), - "expected_model": LaunchConfiguration("camera_model"), - "camera_name": LaunchConfiguration("camera_name"), - "frame_id": LaunchConfiguration("camera_frame_id"), - "image_width": ParameterValue( - LaunchConfiguration("image_width"), value_type=int - ), - "image_height": ParameterValue( - LaunchConfiguration("image_height"), value_type=int - ), - "frame_rate": ParameterValue( - LaunchConfiguration("camera_frame_rate"), value_type=float - ), - "exposure_time_us": ParameterValue( - LaunchConfiguration("exposure_time_us"), value_type=float - ), - "gain_db": ParameterValue( - LaunchConfiguration("gain_db"), value_type=float - ), - "auto_exposure": ParameterValue( - LaunchConfiguration("auto_exposure"), value_type=bool - ), - "camera_info_url": LaunchConfiguration("camera_info_url"), - } - ], - ) - - vision_components = [] - if use_roi: - processed_image_raw_topic = "/g20_thumb_roi/image_raw" - processed_camera_info_topic = "/g20_thumb_roi/camera_info" - processed_image_rect_topic = "/g20_thumb_roi/image_rect" - vision_components.append( - ComposableNode( - package="image_proc", - plugin="image_proc::CropDecimateNode", - name="crop_color_roi", - namespace="/g20_thumb_roi", - remappings=[ - ("in/image_raw", "/camera/camera/color/image_raw"), - ("in/camera_info", "/camera/camera/color/camera_info"), - ("out/image_raw", processed_image_raw_topic), - ("out/camera_info", processed_camera_info_topic), - ], - parameters=[ - { - "queue_size": 5, - "decimation_x": 1, - "decimation_y": 1, - "offset_x": roi_values["roi_x"], - "offset_y": roi_values["roi_y"], - "width": roi_values["roi_width"], - "height": roi_values["roi_height"], - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - ) - rectifier_namespace = "/g20_thumb_roi" - rectifier_name = "rectify_color_roi" - else: - processed_image_raw_topic = "/camera/camera/color/image_raw" - processed_camera_info_topic = "/camera/camera/color/camera_info" - processed_image_rect_topic = "/camera/camera/color/image_rect" - rectifier_namespace = "/camera/camera/color" - rectifier_name = "rectify_color" - - vision_components.append( - ComposableNode( - package="image_proc", - plugin="image_proc::RectifyNode", - name=rectifier_name, - namespace=rectifier_namespace, - remappings=[ - ("image", processed_image_raw_topic), - ("camera_info", processed_camera_info_topic), - ("image_rect", processed_image_rect_topic), - ], - parameters=[{"queue_size": 1}], - extra_arguments=[{"use_intra_process_comms": True}], - ) - ) - - vision_components.append( - ComposableNode( - package="apriltag_ros", - plugin="AprilTagNode", - name="apriltag", - namespace="/apriltag", - parameters=[ - tag_config, - { - "detector.decimate": ParameterValue( - LaunchConfiguration("apriltag_decimate"), - value_type=float, - ) - }, - ], - remappings=[ - ("image_rect", processed_image_rect_topic), - ("camera_info", processed_camera_info_topic), - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - ) - - vision_container = ComposableNodeContainer( - name="g20_thumb_vision_container", - namespace="/", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=vision_components, - output="screen", - emulate_tty=True, - ) - - sdk = Node( - package="linker_hand_ros2_sdk", - executable="linker_hand_sdk", - name="linker_hand_sdk", - output="screen", - condition=IfCondition(LaunchConfiguration("start_sdk")), - parameters=[ - { - "hand_type": "left", - "hand_joint": "G20", - "can": LaunchConfiguration("can_interface"), - "modbus": "None", - "topic_prefix": "/g20", - "move_on_startup": False, - "startup_speed": ParameterValue( - LaunchConfiguration("calibration_speed"), - value_type=int, - ), - "startup_torque": 80, - "state_poll_rate": 10.0, - "repeat_position_commands": False, - "is_touch": False, - } - ], - ) - - calibration = Node( - package="linkerhand_calibration", - executable="calibration_node", - name="g20_thumb_calibration", - output="screen", - parameters=[ - calibration_config, - tag_config, - { - "serial_number": serial_number, - "session_dir": str(session_dir), - "commands_enabled": LaunchConfiguration("commands_enabled"), - "calibration_speed": ParameterValue( - LaunchConfiguration("calibration_speed"), - value_type=int, - ), - "continuous_motion_mode": ParameterValue( - LaunchConfiguration("continuous_motion_mode"), - value_type=str, - ), - "angle_estimation_mode": ParameterValue( - LaunchConfiguration("angle_estimation_mode"), - value_type=str, - ), - "camera_serial_number": LaunchConfiguration("camera_serial_number"), - "rosbag_path": str(bag_path), - "camera_info_topic": processed_camera_info_topic, - "image_topic": processed_image_rect_topic, - "publish_debug_image": LaunchConfiguration( - "publish_debug_image" - ), - }, - ], - ) - - bag = ExecuteProcess( - condition=IfCondition(LaunchConfiguration("record_bag")), - cmd=[ - "ros2", - "bag", - "record", - "--storage", - "mcap", - "--storage-preset-profile", - "zstd_fast", - "--max-bag-size", - "10737418240", - "--output", - str(bag_path), - processed_image_raw_topic, - processed_camera_info_topic, - "/apriltag/detections", - "/tf", - "/g20/cb_left_hand_control_cmd", - "/g20/cb_left_hand_state", - "/g20/cb_left_hand_info", - "/g20_thumb_calibration/status", - ], - output="screen", - ) - - actions = [ - LogInfo(msg=f"G20 thumb calibration session: {session_dir}"), - LogInfo( - msg=( - "G20 thumb image ROI: " - f"x={roi_values['roi_x']}, y={roi_values['roi_y']}, " - f"width={roi_values['roi_width']}, " - f"height={roi_values['roi_height']}" - if use_roi - else "G20 thumb image ROI: disabled" - ) - ), - camera, - vision_container, - ] - actions.extend([sdk, calibration, bag]) - return actions - - -def generate_launch_description() -> LaunchDescription: - package_share = Path( - get_package_share_directory("linkerhand_calibration") - ) - default_output = str(Path.cwd() / "calibration_output") - return LaunchDescription( - [ - SetEnvironmentVariable( - name="FASTRTPS_DEFAULT_PROFILES_FILE", - value=str( - package_share / "config" / "fastdds_large_images.xml" - ), - ), - DeclareLaunchArgument("serial_number", default_value="UNSET"), - DeclareLaunchArgument( - "camera_serial_number", default_value="DB2163742" - ), - DeclareLaunchArgument( - "camera_model", default_value="MV-CS020-10UM" - ), - DeclareLaunchArgument( - "camera_name", default_value="hikrobot_front_DB2163742" - ), - DeclareLaunchArgument( - "camera_frame_id", default_value="camera_color_optical_frame" - ), - DeclareLaunchArgument("image_width", default_value="1624"), - DeclareLaunchArgument("image_height", default_value="1240"), - DeclareLaunchArgument("camera_frame_rate", default_value="30.0"), - DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), - DeclareLaunchArgument("gain_db", default_value="0.0"), - DeclareLaunchArgument("auto_exposure", default_value="false"), - DeclareLaunchArgument( - "camera_info_url", - default_value=str( - Path.home() - / ".ros" - / "camera_info" - / "hikrobot_DB2163742.yaml" - ), - ), - DeclareLaunchArgument( - "publish_debug_image", default_value="false" - ), - DeclareLaunchArgument("use_roi", default_value="false"), - DeclareLaunchArgument("roi_x", default_value="128"), - DeclareLaunchArgument("roi_y", default_value="192"), - DeclareLaunchArgument("roi_width", default_value="1024"), - DeclareLaunchArgument("roi_height", default_value="528"), - DeclareLaunchArgument("can_interface", default_value="can0"), - DeclareLaunchArgument("calibration_speed", default_value="15"), - DeclareLaunchArgument( - "continuous_motion_mode", default_value="endpoint" - ), - DeclareLaunchArgument( - "angle_estimation_mode", - default_value="trajectory_center_3d", - ), - DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), - DeclareLaunchArgument("commands_enabled", default_value="true"), - DeclareLaunchArgument("start_camera", default_value="true"), - DeclareLaunchArgument("start_sdk", default_value="true"), - DeclareLaunchArgument("record_bag", default_value="false"), - DeclareLaunchArgument("output_root", default_value=default_output), - DeclareLaunchArgument("session_dir", default_value=""), - DeclareLaunchArgument( - "calibration_config", - default_value=str(package_share / "config" / "calibration.yaml"), - ), - DeclareLaunchArgument( - "tag_config", - default_value=str(package_share / "config" / "front_tags.yaml"), - ), - OpaqueFunction(function=_launch_stack), - ] - ) diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 579032b..7dab455 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -1,4 +1,4 @@ -"""Launch three Hikrobot views and one registered hand calibration owner.""" +"""Launch Profile-declared Hikrobot views and one calibration owner.""" from __future__ import annotations @@ -23,32 +23,12 @@ from launch_ros.descriptions import ComposableNode from launch_ros.parameter_descriptions import ParameterValue -VIEWS = ("front", "side", "top") - - -def _default_source_urdf(model: str, hand_type: str) -> Path: - relative = ( - Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf" - if model.upper() == "L6" and hand_type == "right" - else Path("urdf") - / f"{model.lower()}_{hand_type}" - / f"linkerhand_{model.lower()}_{hand_type}.urdf" - ) - package_source_or_share = Path(__file__).resolve().parents[1] / relative - try: - installed = ( - Path(get_package_share_directory("linkerhand_calibration")) - / relative - ) - except Exception: - installed = package_source_or_share - return installed if installed.is_file() else package_source_or_share - - def _launch_stack(context): from linkerhand_calibration.product import ( - get_product_calibration_contract, + ProductCalibrationContract, ) + from linkerhand_calibration.profiles import load_hand_profile + from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics model = LaunchConfiguration("model").perform(context).strip().upper() hand_type = LaunchConfiguration("hand_type").perform(context).lower() @@ -56,53 +36,35 @@ def _launch_stack(context): raise RuntimeError("hand_type must be left or right") tag_layout = LaunchConfiguration("tag_layout").perform(context).lower() try: - contract = get_product_calibration_contract( - model, hand_type, tag_layout - ) + profile_path = LaunchConfiguration("profile_config").perform(context).strip() + if not profile_path: + raise ValueError("online calibration requires a protected YAML Profile; use calibrate_hand --config") + expected = LaunchConfiguration("profile_config_expected_sha256").perform(context).strip() + if not expected or hashlib.sha256(Path(profile_path).read_bytes()).hexdigest() != expected: + raise ValueError("Profile changed between product validation and launch") + contract = ProductCalibrationContract(declarative=load_hand_profile(profile_path)) + key = contract.typed_profile.key + if (key.model, key.side, key.layout) != (model, hand_type, tag_layout): + raise ValueError("launch identity differs from the protected Profile") except ValueError as error: raise RuntimeError(str(error)) from error + views = contract.typed_profile.vision.view_names requested_tag_config = LaunchConfiguration("tag_config").perform(context) - package_share = Path( - get_package_share_directory("linkerhand_calibration") - ) - tag_config = ( - Path(requested_tag_config).expanduser().resolve() - if requested_tag_config - else package_share - / "config" - / ( - "three_camera_tags_g20_right_19.yaml" - if tag_layout == "g20_right_19" - else "o12_right_16_tags.yaml" - if tag_layout == "o12_right_16" - else "o6_right_8_tags.yaml" - if tag_layout == "o6_right_8" - else "l6_right_8_tags.yaml" - if tag_layout == "l6_right_8" - else "three_camera_tags_g20_right_15.yaml" - if tag_layout == "g20_right_15" - else "three_camera_tags.yaml" - ) - ) + if not requested_tag_config: + raise RuntimeError("tag_config is required; use calibrate_hand --config") + tag_config = Path(requested_tag_config).expanduser().resolve() if not tag_config.is_file(): raise RuntimeError(f"tag config does not exist: {tag_config}") topic_prefix = f"/{model.lower()}" - is_o12 = model == "O12" - command_topic = ( - f"{topic_prefix}/{hand_type}/joint_cmd" - if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_control_cmd" - ) - state_topic = ( - f"{topic_prefix}/{hand_type}/joint_states" - if is_o12 else f"{topic_prefix}/cb_{hand_type}_hand_state" - ) - info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info" + uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk" + if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk"}: + raise RuntimeError("SDK adapter has no ROS launch binding") + topics = sdk_topics(contract.typed_profile) + command_topic, state_topic = topics.command, topics.feedback requested_source = LaunchConfiguration("source_urdf_path").perform(context) - source_urdf = ( - Path(requested_source).expanduser().resolve() - if requested_source - else _default_source_urdf(model, hand_type).resolve() - ) + if not requested_source: + raise RuntimeError("source_urdf_path is required; use calibrate_hand --config") + source_urdf = Path(requested_source).expanduser().resolve() if not source_urdf.is_file(): raise RuntimeError(f"source URDF does not exist: {source_urdf}") expected_source_hash = LaunchConfiguration( @@ -145,12 +107,12 @@ def _launch_stack(context): camera_serials = { view: LaunchConfiguration(f"{view}_camera_serial").perform(context) - for view in VIEWS + for view in views } if any(not serial for serial in camera_serials.values()): - raise RuntimeError("all three camera serial numbers are required") - if len(set(camera_serials.values())) != 3: - raise RuntimeError("front/side/top camera serial numbers must be unique") + raise RuntimeError("all Profile camera serial numbers are required") + if len(set(camera_serials.values())) != len(views): + raise RuntimeError("Profile camera serial numbers must be unique") cameras = [] components = [] @@ -158,7 +120,7 @@ def _launch_stack(context): info_topics = [] detection_topics = [] calibration_namespace = contract.typed_profile.namespace - for view in VIEWS: + for view in views: namespace = f"{calibration_namespace}/{view}/camera" raw_topic = f"{namespace}/image_raw" info_topic = f"{namespace}/camera_info" @@ -251,7 +213,7 @@ def _launch_stack(context): ) vision = ComposableNodeContainer( - name=f"{model.lower()}_three_camera_vision", + name=f"{model.lower()}_calibration_vision", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -261,15 +223,21 @@ def _launch_stack(context): ) sdk = ( Node( - package="omnihand_node", - executable="omnihand_pro_2025_node", - name="omnihand_pro_2025_node", - namespace="o12", + package="linkerhand_calibration", + executable="o12_sdk_bridge", + name="o12_sdk_bridge", output="screen", condition=IfCondition(LaunchConfiguration("start_sdk")), - parameters=[LaunchConfiguration("vendor_sdk_config")], + parameters=[{ + "vendor_config": LaunchConfiguration("vendor_sdk_config"), + "vendor_config_sha256": LaunchConfiguration("sdk_config_expected_sha256"), + "vendor_python_package": LaunchConfiguration("vendor_sdk_python_package"), + "vendor_package_sha256": LaunchConfiguration("sdk_package_expected_sha256"), + "hand_type": hand_type, + "topic_prefix": f"/{model.lower()}/{hand_type}", + }], ) - if is_o12 + if uses_hcan else Node( package="linker_hand_ros2_sdk", executable="linker_hand_sdk", @@ -312,26 +280,16 @@ def _launch_stack(context): arguments=[ "--profile-id", contract.typed_profile.key.profile_id, + "--profile-config", profile_path, "--profile-sha256", expected, ], parameters=[ LaunchConfiguration("calibration_config"), { "serial_number": hand_serial, - "model": model, - "hand_type": hand_type, - "tag_layout": tag_layout, "session_dir": str(session_dir), "resume_raw_samples_path": LaunchConfiguration( "resume_raw_samples_path" ), - "recalibration_scope": LaunchConfiguration( - "recalibration_scope" - ), - # The SDK performs roughly 25 synchronous CAN queries whenever - # cb__hand_info has a subscriber. Calibration only used - # that topic to display a speed diagnostic, while those reads - # created 17-33 command-unit holes in position trajectories. - "info_topic": f"{calibration_namespace}/disabled_hand_info", "command_topic": command_topic, "state_topic": state_topic, "camera_extrinsics_file": LaunchConfiguration( @@ -353,37 +311,13 @@ def _launch_stack(context): "sdk_config_expected_sha256": LaunchConfiguration( "sdk_config_expected_sha256" ), - "corrected_urdf_output_dir": LaunchConfiguration( - "corrected_urdf_output_dir" + "sdk_package_expected_sha256": LaunchConfiguration("sdk_package_expected_sha256"), + "profile_config_expected_sha256": LaunchConfiguration( + "profile_config_expected_sha256" ), - **{ - f"{view}_camera_serial": camera_serials[view] - for view in VIEWS - }, "commands_enabled": ParameterValue( LaunchConfiguration("commands_enabled"), value_type=bool ), - "normal_calibration_speed": ParameterValue( - LaunchConfiguration("calibration_speed"), value_type=int - ), - "index_roll_calibration_speed": ParameterValue( - LaunchConfiguration("index_roll_calibration_speed"), - value_type=int, - ), - "index_flex_calibration_speed": ParameterValue( - LaunchConfiguration("index_flex_calibration_speed"), - value_type=int, - ), - "adaptive_formal_speed_enabled": ParameterValue( - LaunchConfiguration("adaptive_formal_speed_enabled"), - value_type=bool, - ), - "cross_view_roll_diagnostic_finger": LaunchConfiguration( - "cross_view_roll_diagnostic_finger" - ), - "validation_enabled": ParameterValue( - LaunchConfiguration("validation_enabled"), value_type=bool - ), }, ], ) @@ -406,14 +340,11 @@ def _launch_stack(context): *detection_topics, command_topic, state_topic, - info_topic, *( [ - "/o12/right/joint_control_mode_states", - "/o12/right/joint_error_states", - "/o12/right/joint_temperature_states", + f"/{model.lower()}/{hand_type}/calibration_health", ] - if is_o12 else [] + if uses_hcan else [] ), f"{calibration_namespace}/status", ], @@ -422,15 +353,13 @@ def _launch_stack(context): return [ LogInfo( msg=( - f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; " + f"{model} {hand_type} {tag_layout} calibration session: {session_dir}; " f"source_urdf={source_urdf}" ) ), LogInfo( msg=( - "Camera mapping: front=" - f"{camera_serials['front']} side={camera_serials['side']} " - f"top={camera_serials['top']}" + "Camera mapping: " + " ".join(f"{view}={serial}" for view, serial in camera_serials.items()) ) ), *cameras, @@ -445,7 +374,6 @@ def generate_launch_description() -> LaunchDescription: package_share = Path( get_package_share_directory("linkerhand_calibration") ) - info_root = Path.home() / ".ros" / "camera_info" return LaunchDescription( [ # Camera processes publish ~2 MB frames across DDS. Force the @@ -463,41 +391,11 @@ def generate_launch_description() -> LaunchDescription: name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str(package_share / "config" / "fastdds_large_images.xml"), ), - DeclareLaunchArgument("model", default_value="G20"), - DeclareLaunchArgument("hand_type", default_value="left"), - DeclareLaunchArgument("tag_layout", default_value="legacy_11"), + DeclareLaunchArgument("model", default_value=""), + DeclareLaunchArgument("hand_type", default_value=""), + DeclareLaunchArgument("tag_layout", default_value=""), DeclareLaunchArgument("serial_number", default_value="UNSET"), - DeclareLaunchArgument( - "front_camera_serial", default_value="DB2163742" - ), - DeclareLaunchArgument( - "side_camera_serial", default_value="DB2163749" - ), - DeclareLaunchArgument( - "top_camera_serial", default_value="DB2163739" - ), DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"), - DeclareLaunchArgument( - "front_camera_name", default_value="hikrobot_front_DB2163742" - ), - DeclareLaunchArgument( - "side_camera_name", default_value="hikrobot_side_DB2163749" - ), - DeclareLaunchArgument( - "top_camera_name", default_value="hikrobot_top_DB2163739" - ), - DeclareLaunchArgument( - "front_camera_info_url", - default_value=str(info_root / "hikrobot_DB2163742.yaml"), - ), - DeclareLaunchArgument( - "side_camera_info_url", - default_value=str(info_root / "hikrobot_DB2163749.yaml"), - ), - DeclareLaunchArgument( - "top_camera_info_url", - default_value=str(info_root / "hikrobot_DB2163739.yaml"), - ), DeclareLaunchArgument("camera_frame_rate", default_value="30.0"), DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), DeclareLaunchArgument("gain_db", default_value="0.0"), @@ -505,25 +403,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), DeclareLaunchArgument("can_interface", default_value="can0"), DeclareLaunchArgument("calibration_speed", default_value="15"), - DeclareLaunchArgument( - "index_roll_calibration_speed", default_value="5" - ), - DeclareLaunchArgument( - "index_flex_calibration_speed", default_value="10" - ), - DeclareLaunchArgument( - "adaptive_formal_speed_enabled", default_value="true" - ), - DeclareLaunchArgument( - "cross_view_roll_diagnostic_finger", default_value="" - ), - DeclareLaunchArgument("validation_enabled", default_value="false"), - DeclareLaunchArgument( - "camera_extrinsics_file", - default_value=str( - Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml" - ), - ), + DeclareLaunchArgument("camera_extrinsics_file", default_value=""), DeclareLaunchArgument( "source_urdf_path", default_value="" ), @@ -540,12 +420,15 @@ def generate_launch_description() -> LaunchDescription: "tag_config_expected_sha256", default_value="" ), DeclareLaunchArgument("vendor_sdk_config", default_value=""), + DeclareLaunchArgument("vendor_sdk_python_package", default_value=""), + DeclareLaunchArgument("sdk_package_expected_sha256", default_value=""), DeclareLaunchArgument( "sdk_config_expected_sha256", default_value="" ), DeclareLaunchArgument( - "corrected_urdf_output_dir", default_value="" + "profile_config_expected_sha256", default_value="" ), + DeclareLaunchArgument("profile_config", default_value=""), DeclareLaunchArgument("commands_enabled", default_value="true"), DeclareLaunchArgument("start_cameras", default_value="true"), DeclareLaunchArgument("start_sdk", default_value="true"), @@ -556,12 +439,9 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument("session_dir", default_value=""), DeclareLaunchArgument("resume_raw_samples_path", default_value=""), - DeclareLaunchArgument("recalibration_scope", default_value="full"), DeclareLaunchArgument( "calibration_config", - default_value=str( - package_share / "config" / "three_camera_calibration.yaml" - ), + default_value="", ), DeclareLaunchArgument( "tag_config", diff --git a/src/linkerhand_calibration/launch/unified_calibration.launch.py b/src/linkerhand_calibration/launch/unified_calibration.launch.py new file mode 100644 index 0000000..8bb155d --- /dev/null +++ b/src/linkerhand_calibration/launch/unified_calibration.launch.py @@ -0,0 +1,18 @@ +"""Stable launch name for the profile-driven calibration stack.""" + +from __future__ import annotations + +import importlib.util +from pathlib import Path + + +def generate_launch_description(): + implementation = Path(__file__).with_name("three_camera_calibration.launch.py") + spec = importlib.util.spec_from_file_location( + "linkerhand_unified_calibration_launch", implementation + ) + if spec is None or spec.loader is None: + raise RuntimeError("unified calibration launch implementation is missing") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module.generate_launch_description() diff --git a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py index be87ad9..6ecb9a1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py +++ b/src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py @@ -1,4 +1,7 @@ -"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON. +"""Map explicit SDK commands or feedback to corrected URDF joint coordinates. + +Unified artifacts load their certified manifest and standard URDF. The old +readers below are used only when a historical payload is explicitly supplied. The static encoder-zero corrections in ``zero_angles`` are already baked into the corrected URDF joint origins. This bridge therefore publishes only the @@ -22,16 +25,6 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState -from .full_hand import ( - get_hand_calibration_profile, - infer_compact_payload_layout, - validate_compact_payload, -) -from .models import get_default_registry, validate_schema_v6_runtime_payload -from .models.o12.artifacts import validate_o12_runtime_payload -from .core import ProfileKey - - G20_COMMAND_NAMES: tuple[str, ...] = ( "thumb_cmc_pitch", "index_mcp_pitch", @@ -89,6 +82,10 @@ class CalibratedCommandMapper: def __init__( self, payload: Mapping[str, Any], *, expected_side: str | None = None ) -> None: + from .full_hand import get_hand_calibration_profile, infer_compact_payload_layout, validate_compact_payload + from .compat.legacy_diagnostic_tools.models import get_default_registry, validate_schema_v6_runtime_payload + from .compat.legacy_diagnostic_tools.models.o12.artifacts import validate_o12_runtime_payload + from .core import ProfileKey schema_version = int(payload["schema_version"]) if schema_version == 7: validate_o12_runtime_payload(payload) @@ -273,12 +270,15 @@ class CalibratedCommandMapper: def load_calibrated_command_mapper( - calibration_file: str | Path, *, expected_side: str | None = None + calibration_file: str | Path, *, expected_side: str | None = None, input_kind="command" ) -> CalibratedCommandMapper: path = Path(calibration_file).expanduser().resolve() if not path.is_file(): raise ValueError(f"calibration JSON does not exist: {path}") payload = json.loads(path.read_text(encoding="utf-8")) + if path.name == "release_manifest.json" or payload.get("format") in {"unified_calibration_v1", "unified_calibration_v2"}: + from .runtime.artifacts.reader import load_unified_mapper + return load_unified_mapper(path, expected_side=expected_side, input_kind=input_kind) return CalibratedCommandMapper(payload, expected_side=expected_side) @@ -294,6 +294,8 @@ def default_input_topic( return f"/{str(model).lower()}/cb_{side}_hand_control_cmd" if input_domain == "feedback_rad": return f"/{str(model).lower()}/{side}/joint_states" + if input_domain == "command_rad": + return f"/{str(model).lower()}/{side}/joint_cmd" raise ValueError("calibration curve_input_domain is invalid") @@ -304,6 +306,7 @@ class CalibratedJointStateBridge(Node): self.declare_parameter("calibration_file", "") self.declare_parameter("input_topic", "") self.declare_parameter("output_topic", "") + self.declare_parameter("input_kind", "command") hand_type = str(self.get_parameter("hand_type").value).lower() if hand_type not in {"left", "right"}: @@ -312,7 +315,7 @@ class CalibratedJointStateBridge(Node): if not calibration_file: raise ValueError("calibration_file is required") self.mapper = load_calibrated_command_mapper( - calibration_file, expected_side=hand_type + calibration_file, expected_side=hand_type, input_kind=str(self.get_parameter("input_kind").value) ) input_topic = str(self.get_parameter("input_topic").value).strip() output_topic = str(self.get_parameter("output_topic").value).strip() @@ -323,6 +326,9 @@ class CalibratedJointStateBridge(Node): output_topic or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state" ) + if self.output_topic in {self.input_topic, default_input_topic(hand_type, + "feedback_rad" if self.mapper.input_domain.endswith("rad") else "feedback_u8", self.mapper.model)}: + raise ValueError("mapped output must not overwrite the real SDK command/feedback topic") self.publisher = self.create_publisher(JointState, self.output_topic, 10) self.subscription = self.create_subscription( JointState, self.input_topic, self._command_callback, 10 diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/README.md b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/README.md new file mode 100644 index 0000000..71fc3de --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/README.md @@ -0,0 +1,14 @@ +# 历史兼容区 + +此目录保留旧格式读写、旧布局、历史数据诊断及必要的离线工具，不参与正式在线调度。 +四型号的 runner/node/pipeline 已由 `runtime/runner.py`、`runtime/session.py` 和 +`runtime/artifacts/finalization.py` 替代。只保留仍被历史工具调用的入口； +L6/O6 无调用的 pipeline 包装已删除。 + +旧独立断点实现、低速预检和在线节点已移除。旧发布函数已拒绝更新正式发布指针。 +这里生成的离线诊断文件不能作为标准 URDF 已验收的证据；正式回放使用 +`calibrate_hand --config --offline-raw `。 + +不要在此目录增加新型号。新型号提供 Profile、产品 YAML、原始 CAD/mesh；仅新 SDK 协议增加 Adapter。 +需要恢复已删除的历史实现时，使用工作区 +`calibration_output/refactor_backup.4WRWNn/` 中的归档，不要重新接入生产入口。 diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/__init__.py new file mode 100644 index 0000000..581de90 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/__init__.py @@ -0,0 +1 @@ +"""Read-only compatibility for archived sessions and diagnostic tools.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/__init__.py similarity index 59% rename from src/linkerhand_calibration/linkerhand_calibration/models/__init__.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/__init__.py index a86bee6..1962558 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/__init__.py @@ -1,12 +1,12 @@ """Model- and side-specific calibration policies.""" -from .registry import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import ( EngineBindings, ProfileRegistry, RegisteredProfile, get_default_registry, ) -from .runtime_schema import validate_schema_v6_runtime_payload +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.runtime_schema import validate_schema_v6_runtime_payload __all__ = [ "EngineBindings", diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/__init__.py new file mode 100644 index 0000000..ce3e88f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/__init__.py @@ -0,0 +1,15 @@ +"""Registered profiles for this hand family.""" + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.legacy_11 import build_left_profile, build_right_profile + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.right_19 import build_profile + + registry.register(build_profile()) + registry.register(build_left_profile()) + registry.register(build_right_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/_adapter.py similarity index 78% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/_adapter.py index e544e89..7bf7d45 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/_adapter.py @@ -2,7 +2,7 @@ from __future__ import annotations -from ...core import ( +from linkerhand_calibration.core import ( CalibrationProfile, CommandLayout, MeasurementPolicy, @@ -17,10 +17,10 @@ from ...core import ( VisionRigSpec, ZeroSolvePolicy, ) -from .profile import MIMIC_DERIVED_FINGER_DIPS -from ..registry import EngineBindings, RegisteredProfile -from .artifacts import build_artifact_policy -from .motion import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import MIMIC_DERIVED_FINGER_DIPS +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import EngineBindings, RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.artifacts import build_artifact_policy +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.motion import ( build_calibration_motion_command, build_calibration_preparation_waypoints, build_calibration_return_waypoints, @@ -39,18 +39,18 @@ _HARD_THRESHOLD_KEYS = frozenset( def _run_cli(args: list[str] | None = None) -> None: - from .runner import main + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.runner import main main(args) def _run_node(args: list[str] | None = None) -> None: - from .node import main + from linkerhand_calibration.runtime.ros.entrypoint import main main(args) -def adapt_profile( +def _legacy_typed_profile( *, key: ProfileKey, namespace: str, @@ -58,7 +58,7 @@ def adapt_profile( zero_profile, mechanical_endpoint_joints: frozenset[str] = frozenset(), post_solve_endpoint_joints: frozenset[str] = frozenset(), -) -> RegisteredProfile: +) -> CalibrationProfile: fixed_by_view = { view: frozenset(roles) for view, roles in hand_profile.preflight_view_roles.items() @@ -203,7 +203,33 @@ def adapt_profile( artifacts=build_artifact_policy( frozenset(hand_profile.capabilities) ), + urdf_authorized_fields={ + name: frozenset({"origin.rpy", "limit.upper"}) + for name in active + } | { + name: frozenset({"mimic.offset"}) + for name in passive + if name in zero_profile.static_output_zero_offsets_rad + }, ) + return typed + + +def adapt_profile( + *, key: ProfileKey, namespace: str, hand_profile, zero_profile, + mechanical_endpoint_joints: frozenset[str] = frozenset(), + post_solve_endpoint_joints: frozenset[str] = frozenset(), + typed_profile: CalibrationProfile | None = None, +) -> RegisteredProfile: + # Current products supply their authoritative YAML; only the archived + # 11-Tag compatibility contract is still compiled from its old definition. + typed = typed_profile if typed_profile is not None else _legacy_typed_profile( + key=key, namespace=namespace, hand_profile=hand_profile, + zero_profile=zero_profile, mechanical_endpoint_joints=mechanical_endpoint_joints, + post_solve_endpoint_joints=post_solve_endpoint_joints, + ) + if typed.key != key or typed.namespace != namespace: + raise ValueError("declarative profile identity differs from product binding") return RegisteredProfile( profile=typed, engine=EngineBindings( diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/artifacts.py similarity index 89% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/artifacts.py index d7d5cc3..c801bd4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/artifacts.py @@ -1,6 +1,6 @@ """Runtime JSON and corrected-URDF naming policy.""" -from ...core import ArtifactPolicy +from linkerhand_calibration.core import ArtifactPolicy def build_artifact_policy( @@ -18,6 +18,7 @@ def build_artifact_policy( "camera_extrinsics_sha256", "calibration_config_sha256", "tag_config_sha256", + "profile_config_sha256", } ), session_compatibility_tokens=frozenset(compatibility_tokens), diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/command_layout.py similarity index 100% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/command_layout.py diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/golden_regression.py similarity index 97% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/golden_regression.py index d2de771..c4f0e2c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/golden_regression.py @@ -99,7 +99,7 @@ def validate_golden_sessions( raise ValueError(f"failure session was published through {pointer_name}") if replay_full_session: - from .offline_replay import replay_session + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.offline_replay import replay_session session_id = "20260830_181154" replay = replay_session(root / session_id, write_outputs=False) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/legacy_11.py similarity index 66% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/legacy_11.py index 9a5223e..bcd0708 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/legacy_11.py @@ -1,10 +1,10 @@ """One-release typed wrappers for the legacy 11-Tag layouts.""" -from ...core import ProfileKey -from .profile import get_hand_calibration_profile -from ...urdf_zero import get_zero_calibration_profile -from ..registry import RegisteredProfile -from ._adapter import adapt_profile +from linkerhand_calibration.core import ProfileKey +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import get_hand_calibration_profile +from linkerhand_calibration.urdf_zero import get_zero_calibration_profile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20._adapter import adapt_profile LEFT_KEY = ProfileKey("G20", "left", "legacy_11", 1) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/motion.py similarity index 82% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/motion.py index 465bd0f..c7d0dc5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/motion.py @@ -1,6 +1,6 @@ """Reviewed motion and safe-waypoint strategy exports.""" -from .profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import ( build_calibration_motion_command, build_calibration_preparation_waypoints, build_calibration_return_waypoints, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/offline_replay.py similarity index 98% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/offline_replay.py index b81ea41..385ba35 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/offline_replay.py @@ -19,10 +19,10 @@ import numpy as np from scipy.spatial.transform import Rotation import yaml -from ...compat import default_three_camera_config_path -from ...compat.legacy import uses_coupled_full_hand_zero_solver -from ...extrinsics import load_three_camera_extrinsics -from .profile import ( +from linkerhand_calibration.compat import default_three_camera_config_path +from linkerhand_calibration.compat.legacy import uses_coupled_full_hand_zero_solver +from linkerhand_calibration.extrinsics import load_three_camera_extrinsics +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import ( G20_REFERENCE_THUMB_CMC_JOINTS, G20_RIGHT_19_LAYOUT, RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, @@ -38,15 +38,15 @@ from .profile import ( get_hand_calibration_profile, validate_compact_payload, ) -from ...product import get_product_calibration_contract -from ...sample_schema import fitting_sample_record, fitting_sample_records -from ...storage import atomic_write_json -from .publication import clamp_compact_payload_to_urdf_limits -from .urdf_input import ( +from linkerhand_calibration.product import get_product_calibration_contract +from linkerhand_calibration.sample_schema import fitting_sample_record, fitting_sample_records +from linkerhand_calibration.storage import atomic_write_json +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.publication import clamp_compact_payload_to_urdf_limits +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.urdf_input import ( build_g20_urdf_input_payload, load_g20_urdf_input, ) -from .zero_solver import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import ( JointAxisMeasurement, RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, UrdfKinematicModel, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/profile.py similarity index 97% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/profile.py index b38bede..1a40084 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/profile.py @@ -17,15 +17,15 @@ import xml.etree.ElementTree as ET import numpy as np -from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES -from ...trajectory import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES +from linkerhand_calibration.trajectory import ( _angle_for_circle, _fit_circle_with_axis, _fit_joint_curve, _fit_plane_axis, _orient_circle_positive, ) -from ...zero_calibration import ( +from linkerhand_calibration.zero_calibration import ( _fit_circle, _trajectory_arc_rad, circular_median_rad, @@ -41,72 +41,15 @@ THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = ( ) -@dataclass(frozen=True) -class JointSpec: - name: str - motor_index: int - active: bool - view: str | None - parent_role: str | None - child_role: str | None - source_joint: str | None = None - zero_kind: str | None = None - # Keep the physical axis-line gate only when that line contributes to a - # released URDF zero/axis decision. Curve-only passive measurements may - # retain the monocular line residual as a diagnostic while their image - # trajectory, relative rotation, synchronisation and holdout gates remain - # release-critical. - pose_axis_line_required: bool = True - - @property - def measured(self) -> bool: - return self.source_joint is None +from linkerhand_calibration.core.domain.measurement import ( + JointSpec, SweepSpec, PalmAxisObserver, JointCurveFit +) -@dataclass(frozen=True) -class SweepSpec: - view: str - motor_index: int - joints: tuple[str, ...] - task_name: str = "" - auxiliary_commands: tuple[tuple[int, int], ...] = () - validation_only: bool = False - - @property - def key(self) -> str: - if self.task_name: - return self.task_name - return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}" -@dataclass(frozen=True) -class PalmAxisObserver: - """Non-blocking direction observation attached to an existing task. - - This is deliberately not a ``JointSpec``: it has no curve, endpoint or - retry semantics. The camera callback records it only while both Tags are - visible during the named sweep. - """ - - source_name: str - task_name: str - view: str - parent_role: str - child_role: str - model_joint: str - motor_index: int -@dataclass(frozen=True) -class JointCurveFit: - angle_rad: tuple[float, ...] - decreasing_rad: tuple[float, ...] - increasing_rad: tuple[float, ...] - circle: Mapping[str, Any] - maximum_monotonic_correction_rad: float - maximum_hysteresis_rad: float - quality: Mapping[str, float] - zero_offset_rad: float = 0.0 @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/publication.py similarity index 73% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/publication.py index 0650610..4ecae2e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/publication.py @@ -2,7 +2,6 @@ from __future__ import annotations -from datetime import datetime, timezone import copy import json import math @@ -15,16 +14,16 @@ import xml.etree.ElementTree as ET import numpy as np from scipy.spatial.transform import Rotation -from .profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import ( G20_COMBINATION_REQUIRED_TARGET_KEYS, G20_RIGHT_19_LAYOUT, get_hand_calibration_profile, validate_compact_payload, ) -from ...product import ProductConfig, sha256_file -from ...storage import atomic_write_json -from ...core.urdf import UrdfCorrectionPlan, build_correction_plan -from .zero_solver import ( +from linkerhand_calibration.product import ProductConfig, sha256_file +from linkerhand_calibration.storage import atomic_write_json +from linkerhand_calibration.core.urdf import UrdfCorrectionPlan, build_correction_plan +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import ( RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, get_zero_calibration_profile, ) @@ -429,6 +428,8 @@ def build_mujoco_validation_commands( def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path: + if "passed" in name: + raise ValueError("release pointers require the common ArtifactPublisher") parent = Path(root).resolve() target = Path(session).resolve() if target.parent != parent: @@ -682,233 +683,5 @@ def _verify_combination_validation(combination: Mapping[str, Any]) -> None: ) -def finalize_session_artifacts( - config: ProductConfig, - session: str | Path, - *, - node_status: Mapping[str, Any], -) -> tuple[dict[str, Any], bool]: - """Create diagnostics and update latest_passed only after every guard.""" - directory = Path(session).resolve() - paths = session_artifact_paths(directory, config.serial_number) - for name in ("json", "urdf", "raw", "log"): - if not paths[name].is_file(): - raise ValueError(f"session is missing {name}: {paths[name]}") - payload = _load_json(paths["json"]) - standalone_thumb = bool( - payload.get("artifact_type") - == "g20_right_standalone_thumb_calibration" - ) - if standalone_thumb: - standalone_thumb_offsets(payload) - else: - validate_compact_payload(payload) - if payload.get("schema_version") != 4 or payload.get("side") != "right": - raise ValueError("runtime JSON is not the compact right-hand schema v4") - if not bool(payload.get("quality", {}).get("passed")): - raise ValueError("runtime JSON quality is not passed") - combination = node_status.get("combination_validation") - if not isinstance(combination, Mapping): - raise ValueError("node status is missing combination validation") - _verify_combination_validation(combination) - resume = node_status.get("resume", {}) - if not isinstance(resume, Mapping): - raise ValueError("node status has invalid resume provenance") - calibration_scope = str( - resume.get("recalibration_scope", "full") - ).strip().lower() - if calibration_scope not in {"full", "thumb", "fingers"}: - raise ValueError("node status has an unsupported recalibration scope") - recalibration_tasks = tuple( - str(value) for value in resume.get("recalibration_task_keys", ()) - ) - if calibration_scope == "thumb" and not standalone_thumb and ( - not bool(resume.get("used")) - or not str(resume.get("source_session", "")) - or len(recalibration_tasks) != 4 - or any("thumb_" not in name for name in recalibration_tasks) - ): - raise ValueError( - "thumb recalibration is missing its passed base-session provenance" - ) - if calibration_scope == "fingers" and ( - not bool(resume.get("used")) - or not str(resume.get("source_session", "")) - or len(recalibration_tasks) != 12 - or any("thumb_" in name for name in recalibration_tasks) - ): - raise ValueError( - "finger recalibration is missing its certified thumb-session " - "provenance" - ) - if standalone_thumb and calibration_scope != "thumb": - raise ValueError("standalone thumb artifact has the wrong node scope") - offsets = ( - standalone_thumb_offsets(payload) - if standalone_thumb - else active_offsets(payload) - ) - if calibration_scope != "full" and not standalone_thumb: - verify_partial_scope_preserves_certified_zeros( - scope=calibration_scope, - source_session=str(resume.get("source_session", "")), - serial_root=directory.parent, - serial_number=config.serial_number, - current_offsets=offsets, - ) - endpoint_offsets = { - name: offsets[name] - for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - } - typed_profile = config.calibration_contract.typed_profile - frozen_names = typed_profile.scope.frozen_joints[calibration_scope] - correction_plan = build_correction_plan( - typed_profile, - source_sha256=config.source_urdf_sha256, - scope=calibration_scope, - frozen_offsets_rad={name: offsets[name] for name in frozen_names}, - ) - changed_joints = verify_corrected_urdf( - config.source_urdf, - paths["urdf"], - expected_offsets_rad=offsets, - endpoint_anchored_offsets_rad=endpoint_offsets, - correction_plan=correction_plan, - ) - if standalone_thumb: - clipped_runtime_joints = {} - else: - payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( - payload, paths["urdf"] - ) - if clipped_runtime_joints: - atomic_write_json(paths["json"], payload) - validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) - mesh_resources = verify_urdf_mesh_resources(paths["urdf"]) - mesh_hashes = { - name: sha256_file(path) for name, path in mesh_resources.items() - } - commands = ( - { - "artifact_type": "standalone_thumb_no_full_hand_validation", - "poses": [], - } - if standalone_thumb - else build_mujoco_validation_commands(payload["baseline_command_u8"]) - ) - atomic_write_json(paths["commands"], commands) - - prior, differences = find_compatible_prior_session(config, directory, payload) - release_ready = config.required_independent_passes == 1 or prior is not None - quality = dict(payload["quality"]) - preserved_certified_zeros = sorted( - name - for name in ACTIVE_ZERO_JOINTS - if ( - calibration_scope == "thumb" - and not name.startswith("thumb_") - ) - or ( - calibration_scope == "fingers" - and name.startswith("thumb_") - ) - ) - if standalone_thumb: - preserved_certified_zeros = [] - calibrated_zero_joints = ( - sorted( - name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_") - ) - if standalone_thumb - else sorted(ACTIVE_ZERO_JOINTS) - ) - summary: dict[str, Any] = { - "schema_version": 1, - "serial_number": config.serial_number, - "session_id": f"{config.serial_number}_{directory.name}", - "calibration_scope": calibration_scope, - "inherited_base_session": ( - None - if calibration_scope == "full" or standalone_thumb - else str(resume.get("source_session")) - ), - "freshly_calibrated_task_keys": list(recalibration_tasks), - "preserved_certified_zero_joints": preserved_certified_zeros, - "result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION", - "quality": quality, - "runtime_limit_clipped_bins": clipped_runtime_joints, - "runtime_curve_domain": ( - "thumb_diagnostic_only" - if standalone_thumb - else "requested_command_u8" - ), - "static_zero_calibrated_joints": calibrated_zero_joints, - "non_thumb_zero_policy": ( - "source_cad_unchanged" if standalone_thumb else None - ), - "retained_active_urdf_zero_joints": sorted( - RETAINED_ACTIVE_ZERO_JOINTS - ), - "retained_passive_urdf_joints": sorted(PASSIVE_JOINTS), - "mimic_derived_dynamic_joints": [], - "visually_measured_passive_joints": sorted( - VISUALLY_MEASURED_FINGER_DIPS - ), - "changed_urdf_joint_origins": list(changed_joints), - "hashes": { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - "corrected_urdf_sha256": sha256_file(paths["urdf"]), - "mesh_resources_sha256": mesh_hashes, - "calibration_json_sha256": sha256_file(paths["json"]), - "raw_samples_sha256": sha256_file(paths["raw"]), - }, - "holdout": { - "training_cycles": [0, 1, 2], - "validation_cycle": 3, - "isolated": True, - }, - "combination_validation": dict(combination), - "formal_release": { - "passed": release_ready, - "required_independent_passes": config.required_independent_passes, - "comparison_session": None if prior is None else prior.name, - "maximum_static_difference_deg": ( - None if not differences else math.degrees(max(differences.values())) - ), - }, - "node_status": dict(node_status), - "created_at_utc": datetime.now(timezone.utc).isoformat(), - "artifacts": {name: path.name for name, path in paths.items()}, - } - atomic_write_json(paths["summary"], summary) - # Recompute hashes after all files exist and validate the pair once more - # immediately before the one atomic publication operation. - if sha256_file(config.source_urdf) != config.source_urdf_sha256: - raise ValueError("source URDF changed during calibration") - verify_corrected_urdf( - config.source_urdf, - paths["urdf"], - expected_offsets_rad=offsets, - endpoint_anchored_offsets_rad=endpoint_offsets, - ) - # Revalidate the same coordinate contract immediately before publication: - # verify_corrected_urdf proves that corrected endpoint limits map back to - # the original physical CAD endpoints, while the runtime curves must stay - # inside those corrected-coordinate limits. - if not standalone_thumb: - validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) - final_mesh_hashes = { - name: sha256_file(path) - for name, path in verify_urdf_mesh_resources(paths["urdf"]).items() - } - if final_mesh_hashes != mesh_hashes: - raise ValueError("URDF mesh resources changed during publication") - if release_ready: - atomic_session_pointer( - config.session_root, - "latest_thumb_passed" if standalone_thumb else "latest_passed", - directory, - ) - return summary, release_ready +def finalize_session_artifacts(*args, **kwargs): + raise ValueError("legacy publication is retired; use calibrate_hand --offline-raw with the common finalizer") diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/reporting_zh.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/reporting_zh.py similarity index 100% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/reporting_zh.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/reporting_zh.py diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/right_19.py similarity index 53% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/right_19.py index 4948065..5d6b2aa 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/right_19.py @@ -1,14 +1,15 @@ """Independent reviewed profile for the right 19-Tag product layout.""" -from ...core import ProfileKey -from .profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile -from .zero_policy import ( +from linkerhand_calibration.core import ProfileKey +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_policy import ( RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, get_zero_calibration_profile, ) -from ..registry import RegisteredProfile -from ._adapter import adapt_profile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20._adapter import adapt_profile +from linkerhand_calibration.profiles.loader import load_bundled_hand_profile KEY = ProfileKey("G20", "right", G20_RIGHT_19_LAYOUT, 1) @@ -24,4 +25,5 @@ def build_profile() -> RegisteredProfile: zero_profile=zero, mechanical_endpoint_joints=RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, post_solve_endpoint_joints=RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, + typed_profile=load_bundled_hand_profile(KEY.layout), ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/runner.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/runner.py new file mode 100644 index 0000000..686d0f3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/runner.py @@ -0,0 +1,10 @@ +"""Legacy import path; all online work uses the common product runner.""" + +from linkerhand_calibration.runtime.runner import main +from linkerhand_calibration.compat.legacy_diagnostic_tools.runner_helpers import ( + _automatic_resume_candidate, _calibration_node_exited_before_status, + _launch_command, _resolve_partial_base_session, _status_timeout_seconds, +) + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/urdf_input.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/urdf_input.py similarity index 100% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/urdf_input.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/urdf_input.py diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/zero_policy.py similarity index 68% rename from src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/zero_policy.py index c2a4bbf..5d225c1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/zero_policy.py @@ -1,7 +1,7 @@ """Reviewed static-zero, endpoint, and mimic topology exports.""" -from .profile import MIMIC_DERIVED_FINGER_DIPS -from .zero_solver import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import MIMIC_DERIVED_FINGER_DIPS +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import ( RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/zero_solver.py new file mode 100644 index 0000000..bb724f7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/g20/zero_solver.py @@ -0,0 +1,931 @@ +"""Three-dimensional joint-axis fitting and URDF zero correction.""" + +from __future__ import annotations + +from dataclasses import replace +from datetime import datetime +import math +from pathlib import Path +import re +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core import fit_rotation_axis, robust_rotation_summary +from linkerhand_calibration.core.urdf import ( + UrdfCorrectionPlan, + UrdfJointPatch, + UrdfPatchSet, + materialize_relative_mesh_assets as _materialize_relative_mesh_assets, + write_urdf_patches, +) +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import ( + G20_RIGHT_19_LAYOUT, + IMAGE_TRAJECTORY_JOINTS, + LEFT_HAND_PROFILE, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, + HandCalibrationProfile, + JointCurveFit, + get_hand_calibration_profile, +) +from linkerhand_calibration.sample_schema import explicit_domain_value +from linkerhand_calibration.trajectory import ( + _fit_circle_with_axis, + _fit_joint_curve, + _fit_plane_axis, + _plane_basis, +) + + +from linkerhand_calibration.core.fitting import spatial as _spatial +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel, _parse_triplet, _axis_rotation +from linkerhand_calibration.core.fitting.spatial import ( + ZeroCalibrationProfile, + circle_direction_is_constrained, + select_cross_view_roll_direction_source, + _zero_sensitive_axis_error_rad, + _axis_cone_mismatch_rad, + _vector, + _pose_matrix, + _relative_rotation, + _reference_group_key, + _canonical_reference_records, + _interpolate_reference_rotation, + _near_zero_records, + _baseline_reference, + baseline_hysteresis_by_cycle_rad, + fit_rotation_joint_curve, + measure_rotation_joint_observation, + measure_joint_curve_observation, + rotation_curve_holdout_errors, + joint_curve_holdout_errors, + JointAxisMeasurement, + PalmOrientationMeasurement, + _incremental_common_rotation_axis, + fit_partial_palm_orientation_measurement, + fit_partial_palm_orientation_measurements, + with_depth_free_axis_projection, + cross_view_side_line_source, + axis_line_uses_depth_free_interpretation_plane, + refit_axis_line_group_with_shared_radius, + maximum_axis_line_cycle_spread_m, + axis_line_cycle_rms_m, + _fit_axis_point_from_pose_trajectory, + ZeroSolveResult, + _angles_from_state +) + + +def solve_urdf_zero_offsets(**kwargs) -> ZeroSolveResult: + """Compatibility input binding; all numerical work lives in core.""" + options = dict(kwargs) + profile = options.get("zero_profile") + if profile is None: + profile = get_zero_calibration_profile(options.get("hand_type", "left"), options.get("tag_layout", "legacy_11")) + pattern = profile.hand.layout_id == G20_RIGHT_19_LAYOUT + profile = replace(profile, + parallel_root_pattern=pattern, + common_mode_zero_joints=tuple(name for name in profile.direct_zero_joints if name.endswith("_mcp_roll") and not name.startswith("thumb_")) if pattern else (), + small_offset_joints=frozenset(name for name in profile.direct_zero_joints if name.startswith(("index_", "middle_", "ring_", "pinky_"))), + orientation_anchor_joint=profile.orientation_anchor_joint or ("thumb_cmc_pitch" if profile.base_pose_strategy == "thumb_serial" else f"{profile.reference_finger}_mcp_pitch"), + base_pose_strategy="serial_chain" if profile.base_pose_strategy == "thumb_serial" else profile.base_pose_strategy, + ) + options["zero_profile"] = profile + return _spatial.solve_urdf_zero_offsets(**options) + + +def fit_joint_axis_measurement(*args, **kwargs) -> JointAxisMeasurement: + kwargs.setdefault("constrained_circle_joints", CONSTRAINED_CIRCLE_JOINTS) + return _spatial.fit_joint_axis_measurement(*args, **kwargs) + + +def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: + if hand.layout_id == G20_RIGHT_19_LAYOUT: + return _build_right_19_zero_profile(hand) + reference = hand.reference_finger + reference_roll = f"{reference}_mcp_roll" + reference_pitch = f"{reference}_mcp_pitch" + reference_pip = f"{reference}_pip" + reference_dip = f"{reference}_dip" + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + reference_roll, + reference_pitch, + reference_pip, + ) + inherited = { + f"{finger}_{suffix}": f"{reference}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + if finger != reference + for suffix in ("mcp_roll", "mcp_pitch", "pip") + } + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + reference_pitch: reference_roll, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + reference_roll: reference_pitch, + reference_pitch: reference_pip, + reference_pip: reference_dip, + } + fixed_direct_zero_offsets = { + "thumb_mcp": 0.0, + reference_roll: 0.0, + reference_pitch: 0.0, + reference_pip: 0.0, + } + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=direct, + axis_joints=( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + reference_roll, + reference_pitch, + reference_pip, + reference_dip, + ), + inherited_zero_joints=inherited, + inherited_static_zero_joints={}, + constrained_circle_joints=( + hand.image_trajectory_joints | {reference_dip, "thumb_cmc_yaw"} + ), + root_anchor_joints=frozenset({"thumb_cmc_roll", reference_roll}), + axis_parent_joint=axis_parent, + phase_parent_joint=phase_parent, + offset_observer_joint=observer, + same_view_axis_pair_by_offset={}, + fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets, + static_output_zero_offsets_rad={}, + ) + + +def _build_right_19_zero_profile( + hand: HandCalibrationProfile, +) -> ZeroCalibrationProfile: + """Return the 16-active-zero graph for the 19-Tag layout. + + Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but + repeated motion with one unchanged camera/Tag installation cannot by + itself distinguish a fixed parallel-axis phase bias from an encoder zero. + The three coupled thumb-CMC offsets must therefore remain a visual + multi-axis solve; their electrical endpoints are not assumed to coincide + with source-CAD limits. Independently repeatable contact endpoints still + anchor thumb MCP, finger MCP-pitch and PIP from each session. DIP stays + passive and has no independently identifiable static zero. + """ + fingers = ("index", "middle", "ring", "pinky") + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in fingers + for suffix in ("mcp_roll", "mcp_pitch", "pip") + ), + ) + axis_joints = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + *( + f"{finger}_{suffix}" + for finger in fingers + for suffix in ("mcp_roll", "mcp_pitch", "pip", "dip") + ), + ) + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + **{ + f"{finger}_mcp_pitch": f"{finger}_mcp_roll" + for finger in fingers + }, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + **{ + f"{finger}_{child}": f"{finger}_{parent}" + for finger in fingers + for child, parent in ( + ("pip", "mcp_pitch"), + ("dip", "pip"), + ) + }, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + **{ + f"{finger}_{target}": f"{finger}_{observed}" + for finger in fingers + for target, observed in ( + ("mcp_roll", "mcp_pitch"), + ("mcp_pitch", "pip"), + ("pip", "dip"), + ) + }, + } + root_anchors = frozenset( + {"thumb_cmc_roll", *(f"{finger}_mcp_roll" for finger in fingers)} + ) + constrained = frozenset( + set(hand.image_trajectory_joints) + | {"thumb_cmc_yaw"} + # The side camera observes all four flexion axes nearly end-on. A + # 16 mm monocular planar Tag gives a very repeatable orientation axis, + # while its command-correlated depth bias can tilt the otherwise clean + # centre-trajectory plane by several degrees. Use the orientation + # axis to constrain that circle; PIP retains its independently fitted + # axis line and all final SE(3)/holdout quality gates. + | set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS) + # _fit_axis_measurement_raw constrains every passive DIP direction to + # its source-URDF-parallel upstream PIP. Declare the same fact here + # so validation does not compare that trusted direction with the + # depth-biased free plane of a distal Tag trajectory. + | set(RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS) + ) + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=tuple(direct), + axis_joints=tuple(axis_joints), + inherited_zero_joints={}, + inherited_static_zero_joints={}, + constrained_circle_joints=constrained, + root_anchor_joints=root_anchors, + axis_parent_joint=axis_parent, + phase_parent_joint=phase_parent, + offset_observer_joint=observer, + same_view_axis_pair_by_offset={ + "thumb_cmc_yaw": ("thumb_cmc_roll", "thumb_cmc_pitch") + }, + # Endpoint-observable offsets are supplied by the caller. CMC remains + # visually solved and no serial-specific zero is hidden in the shared + # profile. + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + ) + + +def get_right_19_thumb_zero_profile() -> ZeroCalibrationProfile: + """Return the independent four-zero thumb observer graph. + + Roll and MCP are supplied as mechanical endpoint datums by the caller; + yaw is observed by the same-view roll/pitch axis pair and pitch by the + pitch/MCP serial phase. No finger axis is part of this profile. + """ + full = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb_direct = tuple( + name for name in full.direct_zero_joints if name.startswith("thumb_") + ) + thumb_axes = tuple( + name for name in full.axis_joints if name.startswith("thumb_") + ) + return replace( + full, + direct_zero_joints=thumb_direct, + axis_joints=thumb_axes, + constrained_circle_joints=frozenset( + name + for name in full.constrained_circle_joints + if name.startswith("thumb_") + ), + root_anchor_joints=frozenset({"thumb_cmc_roll"}), + axis_parent_joint={ + child: parent + for child, parent in full.axis_parent_joint.items() + if child.startswith("thumb_") and parent.startswith("thumb_") + }, + phase_parent_joint={ + child: parent + for child, parent in full.phase_parent_joint.items() + if child.startswith("thumb_") and parent.startswith("thumb_") + }, + offset_observer_joint={ + target: observer + for target, observer in full.offset_observer_joint.items() + if target.startswith("thumb_") and observer.startswith("thumb_") + }, + same_view_axis_pair_by_offset={ + target: pair + for target, pair in full.same_view_axis_pair_by_offset.items() + if target.startswith("thumb_") + }, + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + base_pose_strategy="thumb_serial", + ) + + +RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset( + { + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + ), + } +) + +# Thumb CMC roll has a directly observed actuator-to-actuator travel, but its +# electrical stop is not used as a runtime URDF limit. Keep it out of +# RIGHT_19_MECHANICAL_ENDPOINT_JOINTS (whose offsets also move upper limits), +# and use its endpoint estimate only as the final static roll origin. This +# makes roll independent of the fitted palm-frame phase and of all yaw logic. +RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS = frozenset({"thumb_cmc_roll"}) +RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS = ( + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + | RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS +) +RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS = RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS + + +def anchor_right_19_mechanical_endpoint_curves( + curves: Mapping[str, JointCurveFit], + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + *, + maximum_direction_difference_rad: float = math.radians(1.0), + maximum_curve_correction_rad: float = math.radians(3.0), + feedback_endpoint_joints: frozenset[str] = frozenset(), + endpoint_joints: frozenset[str] | None = None, +) -> dict[str, JointCurveFit]: + """Set mechanical-endpoint curve scale from direct SO(3) travel. + + A fitted-axis projection is useful for a dense, signed command curve, but + a small view-dependent orthogonal PnP component can change that + projection's full-scale value. The rotation magnitude between the two + settled endpoint poses is the revolute-joint travel itself: conjugating + both poses by a rigid hand/camera transform or either fixed Tag mounting + rotation cannot change it. + + Use every supplied cycle and both sweep directions as independent endpoint + measurements, reject disagreement, and apply their robust median as one + scale correction to the already validated curve. Dense CMC-roll records + are selected in feedback coordinates; settled mechanical-contact records + remain selected in requested-command coordinates. + """ + if ( + not math.isfinite(maximum_direction_difference_rad) + or maximum_direction_difference_rad <= 0.0 + ): + raise ValueError("endpoint direction difference limit must be positive") + if ( + not math.isfinite(maximum_curve_correction_rad) + or maximum_curve_correction_rad <= 0.0 + ): + raise ValueError("endpoint curve correction limit must be positive") + + result = dict(curves) + selected_endpoint_joints = ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if endpoint_joints is None + else frozenset(str(name) for name in endpoint_joints) + ) + unknown_endpoint_joints = sorted( + set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_endpoint_joints: + raise ValueError( + "endpoint joints are not endpoint measurements: " + + ",".join(unknown_endpoint_joints) + ) + unknown_feedback_joints = sorted( + set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_feedback_joints: + raise ValueError( + "feedback endpoint joints are not endpoint measurements: " + + ",".join(unknown_feedback_joints) + ) + + for name in sorted(selected_endpoint_joints): + fit = curves.get(name) + records = list(records_by_joint.get(name, ())) + if fit is None: + raise ValueError(f"missing measured endpoint curve for {name}") + + direction_travel: dict[str, float] = {} + all_travel: list[float] = [] + for direction in ("decreasing", "increasing"): + cycles = sorted( + { + int(record.get("cycle", 0)) + for record in records + if str(record.get("direction")) == direction + } + ) + branch_travel: list[float] = [] + for cycle in cycles: + endpoint_rotations: dict[int, Rotation] = {} + for command in (0, 255): + quaternions = [] + for record in records: + if ( + str(record.get("direction")) != direction + or int(record.get("cycle", 0)) != cycle + ): + continue + endpoint_value = ( + explicit_domain_value(record, "feedback") + if name in feedback_endpoint_joints + else record.get( + "requested_command_u8", + record.get("command_u8", -1), + ) + ) + if endpoint_value is None: + continue + if int(round(float(endpoint_value))) == command: + quaternions.append(_relative_rotation(record)) + if not quaternions: + raise ValueError( + f"{name} {direction} cycle {cycle} is missing " + f"settled endpoint {command}" + ) + endpoint_rotations[command] = Rotation.from_quat( + robust_rotation_summary(quaternions)[0] + ) + branch_travel.append( + float( + ( + endpoint_rotations[255].inv() + * endpoint_rotations[0] + ).magnitude() + ) + ) + if not branch_travel: + raise ValueError(f"{name} is missing {direction} endpoint travel") + direction_travel[direction] = float(np.median(branch_travel)) + all_travel.extend(branch_travel) + + travel_values = np.asarray(list(direction_travel.values()), dtype=float) + direction_difference = float(np.ptp(travel_values)) + if direction_difference > maximum_direction_difference_rad: + raise ValueError( + f"{name} settled endpoint directions disagree: " + f"{math.degrees(direction_difference):.3f}deg" + ) + cycle_range = float(np.ptp(np.asarray(all_travel, dtype=float))) + if cycle_range > maximum_direction_difference_rad: + raise ValueError( + f"{name} settled endpoint cycles disagree: " + f"{math.degrees(cycle_range):.3f}deg" + ) + direct_travel = float(np.median(np.asarray(all_travel, dtype=float))) + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name} endpoint curve must contain 256 finite bins") + projected_travel = float(curve[0] - curve[255]) + if direct_travel <= 0.0 or projected_travel <= 0.0: + raise ValueError(f"{name} endpoint travel must be positive") + correction = abs(projected_travel - direct_travel) + if correction > maximum_curve_correction_rad: + raise ValueError( + f"{name} fitted/direct endpoint travel differs by " + f"{math.degrees(correction):.3f}deg" + ) + scale = direct_travel / projected_travel + + def scaled(values: Sequence[float]) -> tuple[float, ...]: + return tuple(float(value) * scale for value in values) + + circle = dict(fit.circle) + circle.update( + { + "mechanical_endpoint_direct_travel_rad": direct_travel, + "mechanical_endpoint_direction_difference_rad": ( + direction_difference + ), + "mechanical_endpoint_cycle_range_rad": cycle_range, + "mechanical_endpoint_sample_count": len(all_travel), + "mechanical_endpoint_raw_curve_travel_rad": projected_travel, + "mechanical_endpoint_curve_scale": scale, + } + ) + quality = dict(fit.quality) + if "arc_rad" in quality: + quality["arc_rad"] = float(quality["arc_rad"]) * scale + result[name] = replace( + fit, + angle_rad=scaled(fit.angle_rad), + decreasing_rad=scaled(fit.decreasing_rad), + increasing_rad=scaled(fit.increasing_rad), + circle=circle, + maximum_monotonic_correction_rad=( + float(fit.maximum_monotonic_correction_rad) * scale + ), + maximum_hysteresis_rad=float(fit.maximum_hysteresis_rad) * scale, + quality=quality, + zero_offset_rad=float(fit.zero_offset_rad) * scale, + ) + return result + + +def derive_right_19_mechanical_endpoint_offsets( + source_urdf: str | Path, + curves: Mapping[str, JointCurveFit], + *, + maximum_offset_rad: float = math.radians(5.0), + endpoint_joints: frozenset[str] | None = None, +) -> dict[str, float]: + """Estimate encoder origins from repeatable measured mechanical endpoints. + + For the four fingers feedback 0 is the independently verified palm-contact + endpoint; thumb MCP uses its independently verified actuator endpoint. + The source URDF upper limit describes those same physical endpoints. + Therefore ``origin_offset + measured_travel == CAD_upper``. + + Thumb CMC pitch/yaw remain excluded because their endpoint-to-CAD contract + is not independently established. CMC roll is included as a post-solve + origin only: its measured travel determines roll without allowing the + palm common-direction fit to write that joint. + + The returned corrections are measured, generally non-zero encoder zeros; + they do not retain the CAD origin. Using full relative rotation travel + makes them invariant to rigid hand movement and arbitrary fixed Tag + mounting rotation, unlike a cross-camera absolute planar-PnP phase. + """ + if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0: + raise ValueError("mechanical endpoint maximum offset must be positive") + root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() + joints = {str(node.get("name")): node for node in root.findall("joint")} + result: dict[str, float] = {} + selected_endpoint_joints = ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if endpoint_joints is None + else frozenset(str(name) for name in endpoint_joints) + ) + unknown_endpoint_joints = sorted( + set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_endpoint_joints: + raise ValueError( + "endpoint joints are not endpoint measurements: " + + ",".join(unknown_endpoint_joints) + ) + for name in sorted(selected_endpoint_joints): + fit = curves.get(name) + joint = joints.get(name) + limit = None if joint is None else joint.find("limit") + if fit is None: + raise ValueError(f"missing measured endpoint curve for {name}") + if limit is None or limit.get("upper") is None: + raise ValueError(f"source URDF joint {name} has no upper limit") + upper = float(limit.get("upper")) + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name} endpoint curve must contain 256 finite bins") + measured_endpoint = float(curve[0]) + if measured_endpoint <= 0.0 or abs(float(curve[255])) > math.radians(0.05): + raise ValueError( + f"{name} endpoint curve does not use command 255 as its zero reference" + ) + if abs(measured_endpoint - float(np.max(curve))) > math.radians(0.05): + raise ValueError( + f"{name} command 0 is not the measured upper mechanical endpoint" + ) + offset = upper - measured_endpoint + if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: + raise ValueError( + f"{name} endpoint-derived zero offset is outside the safe range: " + f"{math.degrees(offset):.3f}deg" + ) + result[name] = offset + return result + + +def get_zero_calibration_profile( + side: str, layout_id: str = "legacy_11" +) -> ZeroCalibrationProfile: + return _build_zero_profile(get_hand_calibration_profile(side, layout_id)) + + +LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) + +# Backwards-compatible left-hand aliases. +CONSTRAINED_CIRCLE_JOINTS = LEFT_ZERO_PROFILE.constrained_circle_joints +DIRECT_ZERO_JOINTS = LEFT_ZERO_PROFILE.direct_zero_joints +FIXED_DIRECT_ZERO_OFFSETS_RAD = dict( + LEFT_ZERO_PROFILE.fixed_direct_zero_offsets_rad +) +OPTIMIZED_ZERO_JOINTS = tuple( + name for name in DIRECT_ZERO_JOINTS + if name not in FIXED_DIRECT_ZERO_OFFSETS_RAD +) +AXIS_JOINTS = LEFT_ZERO_PROFILE.axis_joints +INHERITED_ZERO_JOINTS = dict(LEFT_ZERO_PROFILE.inherited_zero_joints) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +def merge_right_19_thumb_zero_result( + *, + thumb_result: ZeroSolveResult, + preserved_offsets_rad: Mapping[str, float] | None = None, + companion_result: ZeroSolveResult | None = None, +) -> ZeroSolveResult: + """Merge an independent thumb solve without allowing reverse coupling. + + ``companion_result`` is the finger/full solver result whose thumb values + were fixed to ``thumb_result``. For a thumb-only run, the 12 certified + finger values are supplied through ``preserved_offsets_rad`` instead. + """ + thumb_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + if set(thumb_result.direct_offsets_rad) != thumb_names: + raise ValueError("independent thumb result must contain exactly four zeros") + full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + direct_names = set(full_profile.direct_zero_joints) + preserved = { + str(name): float(value) + for name, value in (preserved_offsets_rad or {}).items() + } + if set(preserved) - (direct_names - thumb_names): + raise ValueError("preserved offsets contain a thumb or unknown joint") + if any(not math.isfinite(value) for value in preserved.values()): + raise ValueError("preserved offsets must be finite") + + if companion_result is None: + if set(preserved) != direct_names - thumb_names: + raise ValueError("thumb-only merge requires all 12 certified finger zeros") + direct = dict(preserved) + all_active = {name: 0.0 for name in full_profile.hand.active_joints} + all_active.update(preserved) + base_result = thumb_result + else: + if set(companion_result.direct_offsets_rad) != direct_names: + raise ValueError("full companion result must contain all 16 zeros") + direct = dict(companion_result.direct_offsets_rad) + all_active = dict(companion_result.all_active_offsets_rad) + base_result = companion_result + direct.update(thumb_result.direct_offsets_rad) + all_active.update(thumb_result.direct_offsets_rad) + + def merged_map(name: str) -> dict[str, Any]: + values = ( + {} + if companion_result is None + else dict(getattr(companion_result, name)) + ) + values.update(dict(getattr(thumb_result, name))) + return values + + failure_reasons = merged_map("failure_reasons") + passed = bool( + thumb_result.passed + and (companion_result is None or companion_result.passed) + ) + return replace( + base_result, + direct_offsets_rad=direct, + all_active_offsets_rad=all_active, + validation_errors_rad=( + tuple(thumb_result.validation_errors_rad) + if companion_result is None + else ( + *companion_result.validation_errors_rad, + *thumb_result.validation_errors_rad, + ) + ), + validation_error_by_joint_rad=merged_map( + "validation_error_by_joint_rad" + ), + validation_line_error_by_joint_m=merged_map( + "validation_line_error_by_joint_m" + ), + axis_line_rms_m=( + thumb_result.axis_line_rms_m + if companion_result is None + else max( + companion_result.axis_line_rms_m, + thumb_result.axis_line_rms_m, + ) + ), + passed=passed, + cycle_offsets_rad=merged_map("cycle_offsets_rad"), + offset_uncertainty_rad=merged_map("offset_uncertainty_rad"), + offset_confidence_half_width_rad=merged_map( + "offset_confidence_half_width_rad" + ), + validation_original_error_by_joint_rad=merged_map( + "validation_original_error_by_joint_rad" + ), + validation_improvement_by_joint_rad=merged_map( + "validation_improvement_by_joint_rad" + ), + validation_improvement_confidence_lower_rad=merged_map( + "validation_improvement_confidence_lower_rad" + ), + offset_covariance_rad2=merged_map("offset_covariance_rad2"), + axis_cone_mismatch_by_joint_rad=merged_map( + "axis_cone_mismatch_by_joint_rad" + ), + axis_cone_bias_classification_by_joint=merged_map( + "axis_cone_bias_classification_by_joint" + ), + failure_reasons=failure_reasons, + ) + + +def expand_right_19_thumb_zero_result_with_cad_fingers( + thumb_result: ZeroSolveResult, +) -> ZeroSolveResult: + """Build a standalone thumb result with every finger left at source CAD. + + This is intentionally different from inheriting a prior finger + calibration: the twelve non-thumb values are explicit zeros and no finger + observation or artifact is required. + """ + full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + if set(thumb_result.direct_offsets_rad) != thumb_names: + raise ValueError("independent thumb result must contain exactly four zeros") + direct = {name: 0.0 for name in full_profile.direct_zero_joints} + direct.update(thumb_result.direct_offsets_rad) + all_active = {name: 0.0 for name in full_profile.hand.active_joints} + all_active.update(thumb_result.direct_offsets_rad) + return replace( + thumb_result, + direct_offsets_rad=direct, + all_active_offsets_rad=all_active, + ) + + + + + + +def write_zero_corrected_urdf( + *, + source_urdf: str | Path, + output_directory: str | Path, + serial_number: str, + offsets_rad: Mapping[str, float], + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, + timestamp: str | None = None, + correction_plan: UrdfCorrectionPlan | None = None, +) -> Path: + source = Path(source_urdf).expanduser().resolve() + output = Path(output_directory).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"source URDF does not exist: {source}") + if not offsets_rad: + raise ValueError("offsets_rad must contain at least one joint") + offsets = {str(name): float(value) for name, value in offsets_rad.items()} + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } + if not set(endpoint_offsets) <= set(offsets): + raise ValueError("endpoint-anchored offsets must be URDF zero targets") + if correction_plan is not None: + correction_plan.verify_source(source) + correction_plan.authorize_offsets(offsets) + if not set(endpoint_offsets).issubset( + correction_plan.endpoint_limit_joints + ): + raise ValueError( + "endpoint offsets are not authorized by the correction plan" + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in offsets.values() + ): + raise ValueError("URDF zero offsets must be finite and within +/-90deg") + 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("URDF zero timestamp must use YYYYMMDD_HHMMSS") + safe_serial = "".join( + character if character.isalnum() or character in "_.-" else "_" + for character in str(serial_number) + ) + if not safe_serial: + raise ValueError("serial_number must not be empty") + from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan + import hashlib + + if "calibrated" in source.stem.lower(): + raise ValueError("source must be the original CAD URDF") + # The deployed JSON keeps the compatibility shape; zero coordinates must + # nevertheless transform both limits and every dependent mimic together. + # Endpoint metadata cannot select a different coordinate rule. + if any(not math.isclose(value, offsets[name], rel_tol=0, abs_tol=1e-12) + for name, value in endpoint_offsets.items()): + raise ValueError("endpoint metadata disagrees with the actual zero correction") + if correction_plan is not None: + rights = correction_plan.authorized_fields + if not rights: + raise ValueError("production correction plan lacks independent Profile field authorization") + source_hash = correction_plan.source_sha256 + else: + # Compatibility for standalone offline correction tools. The online + # product path always supplies its protected, scope-authorized plan. + model = UrdfKinematicModel(source) + rights = {name: ("origin.rpy", "limit.lower", "limit.upper") for name in offsets} + for name, joint in model.joints.items(): + if joint.mimic_joint is not None: + rights[name] = (*rights.get(name, ()), "mimic.offset") + source_hash = hashlib.sha256(source.read_bytes()).hexdigest() + plan = build_standard_correction_plan(source_urdf=source, + source_sha256=source_hash, zero_offsets_rad=offsets, authorized_fields=rights) + destination = output / f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" + return plan.write(source, destination) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/__init__.py new file mode 100644 index 0000000..01c6175 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/__init__.py @@ -0,0 +1,14 @@ +"""Registered L6 calibration profiles.""" + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.left_transfer import build_profile as build_left_transfer_profile + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.profile import build_profile + + registry.register(build_profile()) + registry.register(build_left_transfer_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/artifacts.py similarity index 95% rename from src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/artifacts.py index 5313922..6ea2b70 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/artifacts.py @@ -13,9 +13,9 @@ import xml.etree.ElementTree as ET import numpy as np -from ..g20.profile import JointCurveFit -from .fitting import L6FitResult, MimicFit -from .profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import JointCurveFit +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.fitting import L6FitResult, MimicFit +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.profile import ( ACTIVE_JOINTS, CALIBRATED_ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, @@ -558,7 +558,7 @@ def load_l6_urdf_input( def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: - from ...core import ProfileKey + from linkerhand_calibration.core import ProfileKey key = ProfileKey.parse(str(payload.get("profile_id", ""))) if key == KEY: @@ -568,7 +568,7 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: active_transfers = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT passive_transfers = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT else: - from .left_transfer import ( + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.left_transfer import ( KEY as LEFT_TRANSFER_KEY, build_typed_profile as build_left_transfer_profile, left_joint_name, @@ -667,7 +667,7 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: expected_model = coupling_models.get( name, "linear_mimic" ) - if model != expected_model: + if model not in {"linear_mimic", "quadratic_runtime"}: raise ValueError(f"{name} coupling model is invalid") coefficients = np.asarray( joint.get("coupling_coefficients"), dtype=float @@ -683,7 +683,7 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: "endpoint_linear_fallback" if model == "quadratic_runtime" else "exact_linear" - if name in profile.zero.fitted_mimic_joints + if name in profile.zero.fitted_mimic_joints or name in passive_transfers else "cad_nominal" ) # Early schema-v6 linear artifacts predate the explicit policy @@ -692,7 +692,7 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: policy = str( joint.get("urdf_mimic_policy", expected_policy) ) - if policy != expected_policy: + if policy not in {expected_policy, "exact_linear" if model == "linear_mimic" else "endpoint_linear_fallback"}: raise ValueError(f"{name} URDF mimic fallback is invalid") multiplier = float(joint.get("mimic_multiplier", "nan")) if not math.isfinite(multiplier) or multiplier <= 0.0: @@ -715,8 +715,14 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None: raise ValueError(f"{name} transfer provenance is invalid") donor = joints[transferred_from] for field in ("angle_rad", "decreasing_rad", "increasing_rad"): - if joint[field] != donor[field]: + if name not in passive_set and joint[field] != donor[field]: raise ValueError(f"{name} transfer curve differs from donor") + if name in passive_set and joint["coupling_model"] == "linear_mimic": + parent = joints[mimic_sources[name]] + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + expected = float(joint["mimic_offset_rad"]) + float(joint["mimic_multiplier"]) * np.asarray(parent[field]) + if not np.allclose(joint[field], expected, atol=1e-7, rtol=0): + raise ValueError(f"{name} curve differs from its declared standard mimic") quality = payload["quality"] if not isinstance(quality, Mapping) or quality.get("scope") != "partial": raise ValueError("schema v6 quality scope must be partial") @@ -732,7 +738,7 @@ def build_l6_left_transferred_runtime_payload( serial_number: str, ) -> dict[str, Any]: """Create a validated L6 left runtime JSON paired with a mirrored URDF.""" - from .left_transfer import ( + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.left_transfer import ( KEY as LEFT_TRANSFER_KEY, build_typed_profile as build_left_transfer_profile, left_joint_name, @@ -845,18 +851,8 @@ def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> Path: return destination -def publish_partial_session(serial_root: str | Path, session: str | Path) -> Path: - parent = Path(serial_root).resolve() - target = Path(session).resolve() - if target.parent != parent or not target.is_dir(): - raise ValueError("partial session must be a direct existing child") - destination = parent / "latest_partial_passed" - temporary = parent / f".latest_partial_passed.{os.getpid()}.tmp" - if temporary.exists() or temporary.is_symlink(): - temporary.unlink() - os.symlink(target.name, temporary, target_is_directory=True) - os.replace(temporary, destination) - return destination +def publish_partial_session(*args, **kwargs): + raise ValueError("legacy publication is retired; use the common ArtifactPublisher") def artifact_hashes(json_path: str | Path, urdf_path: str | Path) -> dict[str, str]: diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/fitting.py new file mode 100644 index 0000000..93ba434 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/fitting.py @@ -0,0 +1,33 @@ +"""Legacy result types plus a direct call to the ONE profile fitter. + +Old endpoint/range-centre fallbacks and model-specific refitting are retired. +The legacy result type remains only for reading archived JSON/tool inputs. +""" +from __future__ import annotations +from dataclasses import dataclass +from pathlib import Path +from typing import Mapping +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.fitting import CouplingFit as MimicFit, fit_coupling_model, curve_travel_rad +from linkerhand_calibration.core.fitting.coupling import LinearMimicFit, RelativeMimicEvidence +from linkerhand_calibration.core.fitting.spatial import ZeroSolveResult +from linkerhand_calibration.core.fitting.session import fit_profile_calibration, compile_spatial_profile +from linkerhand_calibration.profiles import load_bundled_hand_profile + +@dataclass(frozen=True) +class L6FitResult: + curves: Mapping[str, JointCurveFit] + zero_offsets_rad: Mapping[str, float] + travels_rad: Mapping[str, float] + mimic_fits: Mapping[str, MimicFit] + holdout_errors_rad: Mapping[str, tuple[float, ...]] + zero_method_by_joint: Mapping[str, str] + zero_fallback_reason_by_joint: Mapping[str, str] + thumb_zero_result: ZeroSolveResult | None = None + + +def fit_l6_session(source_urdf, records_by_joint, *, require_thumb_axis_zero=False): + # Historical flags can no longer disable spatial or holdout verification. + return fit_profile_calibration(load_bundled_hand_profile("l6_right_8"), + Path(source_urdf), records_by_joint) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py similarity index 87% rename from src/linkerhand_calibration/linkerhand_calibration/models/l6/left_transfer.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py index 7e9474f..a6ab2ab 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/left_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/left_transfer.py @@ -4,9 +4,9 @@ from __future__ import annotations from dataclasses import replace -from ...core import ProfileKey -from ..registry import EngineBindings, RegisteredProfile -from .profile import build_typed_profile as build_right_typed_profile +from linkerhand_calibration.core import ProfileKey +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import EngineBindings, RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.profile import build_typed_profile as build_right_typed_profile KEY = ProfileKey("L6", "left", "l6_left_transferred_8", 1) @@ -59,6 +59,12 @@ def build_typed_profile(): ), zero=replace( right.zero, + transferred_zero_sources={left_joint_name(k): left_joint_name(v) + for k, v in right.zero.transferred_zero_sources.items()}, + transferred_mimic_sources={left_joint_name(k): left_joint_name(v) + for k, v in right.zero.transferred_mimic_sources.items()}, + # Runtime-only mirrored products do not acquire/solve new geometry. + spatial={}, active_joints=active, passive_joints=passive, direct_zero_joints=tuple( @@ -131,7 +137,7 @@ def _unsupported(_args=None) -> None: def build_profile() -> RegisteredProfile: - from .motion import ( + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.motion import ( build_calibration_motion_command, build_calibration_preparation_waypoints, build_calibration_return_waypoints, diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/motion.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/motion.py new file mode 100644 index 0000000..e81360d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/motion.py @@ -0,0 +1,18 @@ +"""Safe six-channel motion helpers for the partial L6 profile.""" + +from __future__ import annotations + +from linkerhand_calibration.runtime.trajectory import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, + cosine_position_trajectory_u8, +) + + +__all__ = [ + "build_calibration_motion_command", + "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", + "cosine_position_trajectory_u8", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/profile.py new file mode 100644 index 0000000..8951f82 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/profile.py @@ -0,0 +1,78 @@ +"""Reviewed partial-calibration profile for the right L6 eight-Tag rig.""" + +from __future__ import annotations + +from linkerhand_calibration.core import CalibrationProfile, ProfileKey +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import EngineBindings, RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) + + +# Historical import constants are projections of the authoritative YAML, +# never a second set of model/zero/coupling definitions. +from linkerhand_calibration.profiles.loader import load_bundled_hand_profile +_DECLARED = load_bundled_hand_profile("l6_right_8") +KEY = _DECLARED.key +COMMAND_NAMES = _DECLARED.command.names +COMMAND_INDEX_BY_JOINT = dict(_DECLARED.command.command_index_by_joint) +ACTIVE_JOINTS = tuple(sorted(COMMAND_INDEX_BY_JOINT, key=COMMAND_INDEX_BY_JOINT.get)) +MIMIC_SOURCE_BY_JOINT = dict(_DECLARED.zero.mimic_source_by_joint) +PASSIVE_JOINTS = tuple(name for source in ACTIVE_JOINTS + for name, parent in MIMIC_SOURCE_BY_JOINT.items() if parent == source) +CALIBRATED_ACTIVE_JOINTS = _DECLARED.scope.selected_joints(_DECLARED.scope.default_scope) +MEASURED_PASSIVE_JOINTS = _DECLARED.zero.fitted_mimic_joints +TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = dict(_DECLARED.zero.transferred_zero_sources) +TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = dict(_DECLARED.zero.transferred_mimic_sources) +CORRECTED_ACTIVE_JOINTS = _DECLARED.zero.active_joints +CORRECTED_PASSIVE_JOINTS = frozenset(MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys()) +ENDPOINT_ANCHOR_BY_JOINT = dict(_DECLARED.zero.endpoint_anchor_by_joint) +COUPLING_MODEL_BY_JOINT = dict(_DECLARED.zero.coupling_model_by_joint) + + +def build_typed_profile() -> CalibrationProfile: + """Compatibility entry; the complete typed contract lives only in YAML.""" + from linkerhand_calibration.profiles.loader import load_bundled_hand_profile + return load_bundled_hand_profile("l6_right_8") + + +def _run_cli(args: list[str] | None = None) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.runner import main + + main(args) + + +def _run_node(args: list[str] | None = None) -> None: + from linkerhand_calibration.runtime.ros.calibration_node import run_profile_node + run_profile_node(build_typed_profile(), args) + + +def build_profile() -> RegisteredProfile: + typed = build_typed_profile() + return RegisteredProfile( + profile=typed, + engine=EngineBindings( + hand_profile=typed, + zero_profile=typed.zero, + motion_command=build_calibration_motion_command, + preparation_waypoints=build_calibration_preparation_waypoints, + return_waypoints=build_calibration_return_waypoints, + cli_main=_run_cli, + node_main=_run_node, + ), + ) + + +__all__ = [ + "ACTIVE_JOINTS", + "CALIBRATED_ACTIVE_JOINTS", + "COMMAND_NAMES", + "KEY", + "MEASURED_PASSIVE_JOINTS", + "MIMIC_SOURCE_BY_JOINT", + "PASSIVE_JOINTS", + "build_profile", + "build_typed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/runner.py new file mode 100644 index 0000000..330a386 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/runner.py @@ -0,0 +1,7 @@ +"""Legacy import path; all online work uses the common product runner.""" + +from linkerhand_calibration.runtime.runner import main +from linkerhand_calibration.runtime.runner_support import launch_command as _launch_command +from linkerhand_calibration.compat.legacy_diagnostic_tools.progress import ( + render_l6_progress_zh, render_six_channel_progress_zh, +) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/urdf.py similarity index 91% rename from src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/urdf.py index e7fdff7..6ca1e12 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/l6/urdf.py @@ -13,14 +13,16 @@ import xml.etree.ElementTree as ET import numpy as np from scipy.spatial.transform import Rotation -from ...core.urdf import ( +from linkerhand_calibration.core.urdf import ( MujocoEqualityPatch, UrdfJointPatch, UrdfPatchSet, + corrected_origin_rpy as _corrected_origin_rpy, + parse_vector3 as _triplet, write_urdf_patches, ) -from .fitting import L6FitResult -from .profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.fitting import L6FitResult +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.profile import ( CALIBRATED_ACTIVE_JOINTS, CORRECTED_ACTIVE_JOINTS, CORRECTED_PASSIVE_JOINTS, @@ -28,6 +30,7 @@ from .profile import ( MIMIC_SOURCE_BY_JOINT, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, ) @@ -41,36 +44,6 @@ class L6UrdfCorrection: explicit_runtime_joints: frozenset[str] -def _triplet(value: str) -> np.ndarray: - result = np.asarray([float(item) for item in str(value).split()], dtype=float) - if result.shape != (3,) or not np.all(np.isfinite(result)): - raise ValueError(f"invalid URDF vector: {value}") - return result - - -def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str: - origin = joint.find("origin") - if origin is None or origin.get("rpy") is None: - raise ValueError(f"joint {joint.get('name')} has no origin.rpy") - # Preserve the reviewed CAD spelling exactly when the calibrated open - # endpoint is the source joint zero. Apart from avoiding Euler round-off, - # this makes it explicit that a zero correction must not rotate the frame. - if abs(float(offset)) <= 1.0e-12: - return str(origin.get("rpy")) - axis_node = joint.find("axis") - axis = _triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - norm = float(np.linalg.norm(axis)) - if norm <= 1.0e-12: - raise ValueError(f"joint {joint.get('name')} has a degenerate axis") - source = Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0"))) - corrected = source * Rotation.from_rotvec(axis / norm * float(offset)) - return " ".join( - f"{float(value):.15g}" for value in corrected.as_euler("xyz") - ) - - def _revolute_joints(root: ET.Element) -> dict[str, ET.Element]: return { str(joint.get("name")): joint @@ -387,6 +360,7 @@ def write_l6_corrected_urdf( # Keep the complete vendor mesh bundle, including auxiliary meshes not # referenced by this XML revision. copy_complete_mesh_directory=True, + authorized_fields=build_typed_profile().urdf_authorized_fields, ) return L6UrdfCorrection( path=destination, diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/__init__.py new file mode 100644 index 0000000..163131d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/__init__.py @@ -0,0 +1,11 @@ +"""Registered O12 calibration profiles.""" + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import build_profile + registry.register(build_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/artifacts.py similarity index 70% rename from src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/artifacts.py index 301570f..2c893cb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/artifacts.py @@ -11,13 +11,8 @@ 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 ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.fitting import O12FitResult +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import ( ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, COMMAND_NAMES, @@ -35,8 +30,8 @@ from .profile import ( TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, build_typed_profile, ) -from .urdf import o12_endpoint_mimic_contract -from .zero import SPATIAL_ZERO_POLICY +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.urdf import o12_correction_plan +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.zero import SPATIAL_ZERO_POLICY ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS) @@ -49,21 +44,6 @@ def _motor_index(joint: str) -> int: return COMMAND_INDEX_BY_JOINT[source] -def _task_for_joint(joint: str): - target = str(joint) - while True: - if target in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT: - target = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT[target] - elif target not in COMMAND_INDEX_BY_JOINT: - target = MIMIC_SOURCE_BY_JOINT[target] - else: - break - return next( - task for task in build_typed_profile().motion.tasks - if target in task.joints - ) - - def _source_joint_metadata(source_urdf: str | Path) -> dict[str, dict[str, Any]]: root = ET.parse(Path(source_urdf)).getroot() result: dict[str, dict[str, Any]] = {} @@ -98,12 +78,6 @@ def _round(values: Sequence[float], digits: int = 10) -> list[float]: return [round(float(value), digits) for value in array] -def _zeroed(values: Sequence[float], inputs: Sequence[float]) -> np.ndarray: - curve = np.asarray(values, dtype=float) - baseline = float(np.interp(0.0, np.asarray(inputs, dtype=float), curve)) - return curve - baseline - - def build_o12_runtime_payload( *, serial_number: str, @@ -129,48 +103,25 @@ def build_o12_runtime_payload( 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) + plan = o12_correction_plan(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] 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] + mapping = result.output_mappings[name] + inputs = list(mapping.knots) + motor = mapping.motor_index sign = float(SDK_TO_URDF_SIGN[motor]) - decreasing = _zeroed( - curve_values_at_rad( - donor, fit, inputs, "decreasing_rad", feedback_domain - ), inputs - ) - increasing = _zeroed( - 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 + decreasing = np.asarray(mapping.decreasing_rad) + increasing = np.asarray(mapping.increasing_rad) + angle = np.asarray(mapping.angle_rad) 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) + lower, upper = plan.limits_output_rad[name] + values = np.concatenate((angle, decreasing, increasing)) + if np.min(values) < lower - 1e-9 or np.max(values) > upper + 1e-9: + raise ValueError(f"transferred curve exceeds recipient physical range: {name}; no clipping is permitted") curves[name] = (angle, decreasing, increasing, inputs) joint: dict[str, Any] = { "urdf_joint": name, @@ -201,66 +152,28 @@ def build_o12_runtime_payload( "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 - else "increasing" - ), + "raw_increasing_curve_branch": "increasing", } if donor != name: joint["transferred_from_joint"] = donor joint["transfer_policy"] = ( - "pinky_curve_clamped_to_ring_cad_range" + "donor_scalar_mapping_on_own_cad_frame" ) joint["static_zero_transfer_policy"] = "donor_offset_on_own_cad_frame" joints[name] = joint for name in PASSIVE_JOINTS: - metadata = source[name] - source_name = MIMIC_SOURCE_BY_JOINT[name] + source_name, multiplier, offset = plan.mimic_output[name] motor = _motor_index(source_name) - if name in MEASURED_PASSIVE_JOINTS: - fit = result.curves[name] - 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 = "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] - multiplier = float(metadata["multiplier"]) - offset = float(metadata["offset"]) - angle = offset + multiplier * source_curve - decreasing = offset + multiplier * source_dec - increasing = offset + multiplier * source_inc - coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0] - coupling_model = "linear_mimic" - policy = "cad_nominal_preserved" - status = profile.joint_coverage[name] + source_curve, source_dec, source_inc, inputs = curves[source_name] + angle = offset + multiplier * source_curve + decreasing = offset + multiplier * source_dec + increasing = offset + multiplier * source_inc + coefficients = [offset, multiplier, 0.0, 0.0, 0.0, 0.0] + coupling_model = "linear_mimic" + policy = ("visual_standard_linear_mimic" if name in MEASURED_PASSIVE_JOINTS + else "cad_nominal_coordinate_transformed") + status = profile.joint_coverage[name] curves[name] = (angle, decreasing, increasing, list(inputs)) joint = { "urdf_joint": name, @@ -269,7 +182,7 @@ def build_o12_runtime_payload( "passive": True, "source_joint": source_name, "calibration_status": status, - "static_zero_policy": "cad_mechanical_endpoint", + "static_zero_policy": "unobservable_static_offset_retains_cad", "curve_input_knots_rad": _round(inputs), "angle_rad": _round(angle), "decreasing_rad": _round(decreasing), @@ -279,34 +192,21 @@ def build_o12_runtime_payload( "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 - else "increasing" + "exported_standard_urdf_mimic" ), + "raw_increasing_curve_branch": "increasing", } if name in MEASURED_PASSIVE_JOINTS: + evidence = result.standard_mimic_evidence[name] 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 - ), + "measured_feedback_domain_rad": list(result.feedback_domains_rad[name]), + "visual_observation_role": "independent_linear_mimic_training_and_holdout", + "observed_coupling_model": "linear_mimic", + "observed_mimic_multiplier": round(evidence.fit.multiplier, 10), + "observed_residual_rms_rad": round(float(np.sqrt(np.mean(np.square(evidence.fit.residuals_rad)))), 10), + "observed_visual_arc_rad": round(float(result.visual_arc_diagnostics_rad[name]), 10), + "linear_mimic_holdout_mae_rad": float(np.mean(np.abs(evidence.holdout_errors_rad))), + "linear_mimic_holdout_max_rad": float(np.max(np.abs(evidence.holdout_errors_rad))), }) joints[name] = joint @@ -404,8 +304,8 @@ def build_o12_runtime_payload( }, "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", + "passive_runtime_source": "exported_standard_urdf_mimic", + "visual_measurement_role": "active_curve_and_passive_linear_fit_with_independent_holdout", }, } validate_o12_runtime_payload(payload) @@ -500,21 +400,17 @@ def validate_o12_runtime_payload(payload: Mapping[str, Any]) -> None: 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 joint.get("coupling_model") != "linear_mimic" or not np.allclose( + coefficients[1:], [multiplier, 0, 0, 0, 0], rtol=0, atol=2e-9 + ): + raise ValueError(f"{name} cannot replace standard URDF mimic with a nonlinear JSON model") + if not np.allclose(joint["curve_input_knots_rad"], joints[source_joint]["curve_input_knots_rad"], rtol=0, atol=1e-10): + raise ValueError(f"{name} mimic and source must have the same input knots") + 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, atol=2e-9): + raise ValueError(f"{name}.{field} differs from standard URDF mimic chain") if not bool(payload["quality"].get("passed")): raise ValueError("failed O12 calibration cannot be published") @@ -526,7 +422,7 @@ def validate_o12_runtime_payload_against_urdf( """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. + Passive runtime values must equal the actual exported linear URDF mimic. """ validate_o12_runtime_payload(payload) metadata = _source_joint_metadata(urdf) @@ -535,7 +431,6 @@ def validate_o12_runtime_payload_against_urdf( 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"]): @@ -551,33 +446,18 @@ def validate_o12_runtime_payload_against_urdf( 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)), - ) + source_values = np.asarray(joints[source_name][field], dtype=float) + urdf_values = float(expected["offset"]) + float(expected["multiplier"]) * source_values + if not np.allclose(values, urdf_values, rtol=0, atol=2e-9): + raise ValueError(f"{name}.{field} differs from exported standard URDF mimic") if ( float(np.min(values)) - < float(expected["lower"]) - allowance - 2.0e-9 + < float(expected["lower"]) - 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 + > float(expected["upper"]) + 2.0e-9 ): raise ValueError(f"{name}.{field} is above the URDF physical limit") if source_urdf is not None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/fitting.py new file mode 100644 index 0000000..b5daa82 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/fitting.py @@ -0,0 +1,38 @@ +"""Legacy result types plus a direct call to the ONE profile fitter. + +Old endpoint/range-centre fallbacks and model-specific refitting are retired. +The legacy result type remains only for reading archived JSON/tool inputs. +""" +from __future__ import annotations +from dataclasses import dataclass, field +from pathlib import Path +from typing import Mapping +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.fitting import CouplingFit as MimicFit, fit_coupling_model, curve_travel_rad +from linkerhand_calibration.core.fitting.coupling import LinearMimicFit, RelativeMimicEvidence +from linkerhand_calibration.core.fitting.spatial import ZeroSolveResult +from linkerhand_calibration.core.fitting.session import fit_profile_calibration, compile_spatial_profile +from linkerhand_calibration.profiles import load_bundled_hand_profile + +@dataclass(frozen=True) +class O12FitResult: + curves: Mapping[str, JointCurveFit] + zero_offsets_rad: Mapping[str, float] + travels_rad: Mapping[str, float] + mimic_fits: Mapping[str, LinearMimicFit] + 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 + standard_mimic_evidence: Mapping[str, RelativeMimicEvidence] = field(default_factory=dict) + output_mappings: Mapping[str, JointMapping] = field(default_factory=dict) + + +def fit_o12_session(source_urdf, records_by_joint, *, cross_view_records_by_joint=None, require_cross_view=False, require_thumb_root_spatial_zero=False, require_full_hand_spatial_zero=False): + # Historical flags can no longer disable spatial or holdout verification. + return fit_profile_calibration(load_bundled_hand_profile("o12_right_16"), + Path(source_urdf), records_by_joint, cross_view_records=cross_view_records_by_joint) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/health.py similarity index 100% rename from src/linkerhand_calibration/linkerhand_calibration/models/o12/health.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/health.py diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/kinematics.py similarity index 100% rename from src/linkerhand_calibration/linkerhand_calibration/models/o12/kinematics.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/kinematics.py diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/motion.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/motion.py new file mode 100644 index 0000000..270e07a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/motion.py @@ -0,0 +1,64 @@ +"""Physical-radian motion helpers for O12 right calibration.""" + +from __future__ import annotations + +from typing import Sequence + +from linkerhand_calibration.core import CalibrationProfile, TaskSpec + + +from linkerhand_calibration.runtime.trajectory import cosine_position_trajectory, cosine_ramp_velocity_trajectory + + +def cosine_position_trajectory_rad(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s): + return cosine_position_trajectory(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s) + + +def cosine_ramp_velocity_trajectory_rad(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s, ramp_seconds): + return cosine_ramp_velocity_trajectory(start_rad, target_rad, elapsed_seconds, maximum_speed_rad_s, ramp_seconds) + + +def build_calibration_motion_command( + task: TaskSpec, + command_rad: float, + *, + profile: CalibrationProfile, +) -> list[float]: + values = list(profile.command.baseline_values) + for index, value in task.auxiliary_commands: + values[int(index)] = float(value) + values[int(task.command_index)] = float(command_rad) + return values + + +def build_calibration_preparation_waypoints( + task: TaskSpec, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, +) -> tuple[tuple[float, ...], ...]: + del current_command + return (tuple(build_calibration_motion_command(task, task.start_value, profile=profile)),) + + +def build_calibration_return_waypoints( + target_command: Sequence[float] | None = None, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, + **_: object, +) -> tuple[tuple[float, ...], ...]: + del current_command + target = tuple( + profile.command.baseline_values if target_command is None else target_command + ) + if len(target) != profile.command.command_count: + raise ValueError("O12 return command has the wrong channel count") + return (tuple(float(value) for value in target),) + + +__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/compat/legacy_diagnostic_tools/models/o12/observations.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/observations.py new file mode 100644 index 0000000..dbb7d4e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/observations.py @@ -0,0 +1,17 @@ +"""Legacy offline name for the generic articulated candidate primitive.""" + +from linkerhand_calibration.core.geometry.candidate_selection import ( + MATRIX_SOURCE, POLICY, _relative, _rotation, resolve_chain_observations, +) +from linkerhand_calibration.core.geometry.pnp import solve_square_tag_ippe +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.pnp import THUMB_ROLES +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import build_typed_profile + + +def resolve_thumb_observations(records, *, projection_override=None): + profile = build_typed_profile() + task = next(task for task in profile.motion.tasks if task.key == 'thumb_mcp_dip_front') + return resolve_chain_observations(records, task_key=task.key, + role_pairs={name: (profile.measurement.measurements[name].parent_role, + profile.measurement.measurements[name].child_role) for name in task.joints}, + projection_override=projection_override, solve_pose=solve_square_tag_ippe) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/pipeline.py new file mode 100644 index 0000000..f326a3f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/pipeline.py @@ -0,0 +1,31 @@ +"""Old O12 journal names at the common finalization boundary. + +All fitting, quality, standard-URDF generation and publication live in the +common finalizer. This adapter only translates old capture field names and +task-relative direction labels, without changing numerical observations. +""" + +from linkerhand_calibration.runtime.acquisition import accepted_joint_records, accepted_secondary_records, load_capture +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import build_typed_profile + + +def _journal_rows(records): + return [dict(row, observation_joint=row["model_joint"]) + if row.get("kind") == "o12_roll_cross_view_sample" else dict(row) for row in records] + + +def accepted_records_by_joint(records): + return accepted_joint_records(build_typed_profile(), records) + + +def accepted_roll_cross_view_records(records): + return accepted_secondary_records(build_typed_profile(), _journal_rows(records)) + + +load_o12_raw_samples = load_capture + + +def finalize_o12_session(*, records, **kwargs): + return finalize_profile_session(profile=build_typed_profile(), + records=_journal_rows(records), directions_are_task_relative=True, **kwargs) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/pnp.py similarity index 98% rename from src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/pnp.py index 5825b6e..dadcdc8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pnp.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/pnp.py @@ -3,7 +3,7 @@ import math import numpy as np from scipy.spatial.transform import Rotation -from ...pnp import SquareTagGroupPoseTracker +from linkerhand_calibration.pnp import SquareTagGroupPoseTracker THUMB_ROLES = ('thumb_cmc', 'thumb_mcp', 'thumb_dip') diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/profile.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/profile.py new file mode 100644 index 0000000..a634686 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/profile.py @@ -0,0 +1,285 @@ +"""Reviewed O12 right-hand 16-Tag calibration profile.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +from linkerhand_calibration.core import CalibrationProfile, ProfileKey +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import EngineBindings, RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) + + +KEY = ProfileKey("O12", "right", "o12_right_16", 1) + +# sensor_msgs/JointState.position active-angle order from API_CPP_O12.md. +COMMAND_NAMES: tuple[str, ...] = ( + "thumb_roll", + "thumb_abad", + "thumb_mcp", + "thumb_pip", + "index_abad", + "index_mcp", + "index_pip", + "middle_abad", + "middle_mcp", + "middle_pip", + "ring_mcp", + "pinky_mcp", +) + +SDK_LOWER_RAD = ( + 0.0, -1.387536755335492, -0.8272860654453121, -1.2915436464758039, + -0.2617993877991494, 0.0, 0.0, -0.2617993877991494, + 0.0, 0.0, 0.0, 0.0, +) +SDK_UPPER_RAD = ( + 0.9424777960769379, 0.0, 0.0, 0.0, + 0.2617993877991494, 1.3526301702956054, 1.530653753999027, + 0.2617993877991494, 1.3578661580515883, 1.8151424220741028, + 1.53588974175501, 1.53588974175501, +) + +# These legacy constants describe SDK coordinates, not measured CAD angles. +# The YAML owns the task domain. Measured output angles must independently +# satisfy the source CAD physical range; a larger SDK value never authorizes +# expanding URDF limits. Remove these aliases with the remaining old runtime. +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 +) +FEEDBACK_UPPER_RAD = tuple( + value + FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_UPPER_RAD +) + +SDK_TO_URDF_JOINT: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + "middle_mcp_roll", + "middle_mcp_pitch", + "middle_pip", + "ring_mcp_pitch", + "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", + "index_dip", + "middle_dip", + "ring_pip", + "ring_dip", + "pinky_pip", + "pinky_dip", +) +CALIBRATED_ACTIVE_JOINTS = frozenset(ACTIVE_JOINTS) - {"ring_mcp_pitch"} +MEASURED_PASSIVE_JOINTS = frozenset( + {"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"} +) +TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { + "ring_mcp_pitch": "pinky_mcp_pitch", +} +MIMIC_SOURCE_BY_JOINT = { + "thumb_dip": "thumb_mcp", + "index_dip": "index_pip", + "middle_dip": "middle_pip", + "ring_pip": "ring_mcp_pitch", + "ring_dip": "ring_pip", + "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) +} + +# 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. The +# unified waypoint rule accepts 80% measured travel after the command +# trajectory ends and feedback settles; +# exact command equality is intentionally not required for tendon mechanisms. +CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION = 0.80 +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 build_typed_profile() -> CalibrationProfile: + """Compatibility entry; the complete typed contract lives only in YAML.""" + from linkerhand_calibration.profiles.loader import load_bundled_hand_profile + return load_bundled_hand_profile("o12_right_16") + + +def _run_cli(args: list[str] | None = None) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.runner import main + main(args) + + +def _run_node(args: list[str] | None = None) -> None: + from linkerhand_calibration.runtime.ros.calibration_node import run_profile_node + run_profile_node(build_typed_profile(), args) + + +def build_profile() -> RegisteredProfile: + typed = build_typed_profile() + return RegisteredProfile( + typed, + EngineBindings( + hand_profile=typed, + zero_profile=typed.zero, + motion_command=build_calibration_motion_command, + preparation_waypoints=build_calibration_preparation_waypoints, + return_waypoints=build_calibration_return_waypoints, + cli_main=_run_cli, + node_main=_run_node, + ), + ) + + +__all__ = [ + "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT", + "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/compat/legacy_diagnostic_tools/models/o12/quality.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/quality.py new file mode 100644 index 0000000..07ed869 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/quality.py @@ -0,0 +1,75 @@ +"""O12 schema-v7 compatibility facade for the common data gate. + +New runtime code calls :class:`runtime.engine.CalibrationEngine` directly. +The legacy function remains because deployed schema-v7 diagnostics and tests +import it; it delegates to the same model-independent evaluator. +""" + +from __future__ import annotations + +from typing import Any, Mapping, Sequence + +from linkerhand_calibration.runtime.engine import evaluate_observability + +QUALITY_POLICY_VERSION = 5 +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 schema-v7 fields derived from the common acceptance policy.""" + del minimum_joint_frame_rate, minimum_feedback_hz + quality = evaluate_observability( + normalized_feedback, + minimum_span=required_feedback_span, + minimum_valid_samples=minimum_sweep_frames, + minimum_bins=minimum_sweep_bins, + maximum_unobserved_fraction=MAXIMUM_UNOBSERVED_BIN_FRACTION, + total_frames=int(observation.get("total_frames", 0)), + joint_frame_rate=float(observation.get("joint_frame_rate", 0.0)), + feedback_hz=feedback_hz, + detection_rate=float(observation.get("tag_detection_rate", 0.0)), + bin_count=normalized_bin_count, + include_endpoint_gaps=False, + ) + warnings = list(quality.warnings) + maximum_gap = int(quality.metrics["maximum_bin_gap"]) + detection_rate = float(observation.get("tag_detection_rate", 0.0)) + 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") + target_warning = f"tag_rate_target[{worst_role}]={detection_rate:.3f}" + if target_warning not in warnings: + warnings.append(target_warning) + 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", + **dict(quality.metrics), + "required_feedback_span": round(float(required_feedback_span), 9), + "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": list(quality.failures), + "passed": quality.passed, + } + +__all__ = [ + "MAXIMUM_UNOBSERVED_BIN_FRACTION", + "QUALITY_POLICY_VERSION", + "evaluate_o12_observation_quality", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/runner.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/runner.py new file mode 100644 index 0000000..ab09480 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/runner.py @@ -0,0 +1,5 @@ +"""Legacy import path; all online work uses the common product runner.""" + +from linkerhand_calibration.runtime.runner import main +from linkerhand_calibration.runtime.runner_support import log_exception_summary as _log_exception_summary +from linkerhand_calibration.compat.legacy_diagnostic_tools.progress import render_o12_progress_zh diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/urdf.py new file mode 100644 index 0000000..8ecf4b3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/urdf.py @@ -0,0 +1,94 @@ +"""O12 result adapter for the common standard-URDF correction engine.""" + +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime +import hashlib +import math +from pathlib import Path +import re +from typing import Mapping + +from linkerhand_calibration.core.urdf.plan import StandardUrdfPlan, build_standard_correction_plan +from linkerhand_calibration.runtime.artifacts.publisher import StagedRelease +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.fitting import O12FitResult +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import ( + ACTIVE_JOINTS, CALIBRATED_ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, + MEASURED_PASSIVE_JOINTS, SDK_TO_URDF_SIGN, STATIC_ZERO_EXCLUDED_JOINTS, + TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, build_typed_profile, +) + + +def o12_active_ranges(result: O12FitResult) -> dict[str, tuple[float, float]]: + if set(result.output_mappings) != set(ACTIVE_JOINTS): + raise ValueError("O12 fit is missing frozen native output mappings") + return { + name: result.output_mappings[name].bounds_rad + for name in sorted(CALIBRATED_ACTIVE_JOINTS) + } + + +def o12_correction_plan(source_urdf: str | Path, result: O12FitResult) -> StandardUrdfPlan: + """Translate measured parameters; never invent endpoint mimics or limits.""" + if set(result.travels_rad) != CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT): + raise ValueError("O12 travel result has the wrong active joint set") + if set(result.standard_mimic_evidence) != MEASURED_PASSIVE_JOINTS: + raise ValueError("O12 standard mimic requires independent visual training/holdout evidence") + for name in STATIC_ZERO_EXCLUDED_JOINTS: + if not math.isclose(float(result.zero_offsets_rad.get(name, math.nan)), 0., abs_tol=1e-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 name, evidence in result.standard_mimic_evidence.items(): + fit = evidence.fit + if fit.target_joint != name or not evidence.training_sample_ids or not evidence.holdout_sample_ids or set(evidence.training_sample_ids) & set(evidence.holdout_sample_ids): + raise ValueError(f"O12 mimic evidence is not independent: {name}") + from linkerhand_calibration.core.urdf.acceptance import angular_metrics + if not angular_metrics(evidence.holdout_errors_rad).passed: + raise ValueError(f"standard_urdf_mimic_not_expressive:{name}") + source = Path(source_urdf) + return build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + zero_offsets_rad=result.zero_offsets_rad, + measured_ranges_output_rad=o12_active_ranges(result), + fitted_mimics_cad={name: evidence.fit for name, evidence in result.standard_mimic_evidence.items()}, + transferred_zero_sources=TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, + authorized_fields=build_typed_profile().urdf_authorized_fields) + + +@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, ...] + staged_release: StagedRelease | None = None + + +def write_o12_corrected_urdf( + *, source_urdf: str | Path, output_directory: str | Path, + serial_number: str, result: O12FitResult, timestamp: str | None = None, +) -> O12UrdfCorrection: + """Write authorized standard coordinates using the shared patch engine.""" + source = Path(source_urdf).expanduser().resolve() + if "calibrated" in source.stem.lower(): + raise ValueError("O12 source URDF must be immutable original CAD") + plan = o12_correction_plan(source, result) + 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") + serial = "".join(c if c.isalnum() or c in "_.-" else "_" for c in str(serial_number)) + if not serial: + raise ValueError("serial number must not be empty") + destination = Path(output_directory).expanduser().resolve() / f"{source.stem}_calibrated_{serial}_{stamp}.urdf" + plan.write(source, destination) + return O12UrdfCorrection(destination, + {name: float(result.zero_offsets_rad[name]) for name in ACTIVE_JOINTS}, + plan.limits_output_rad, + {name: value[1] for name, value in plan.mimic_output.items()}, + ("ring_mcp_pitch.origin.xyz", "ring_mcp_pitch.physical_range", + "ring_pip.origin", "ring_pip.limit", "ring_pip.mimic.multiplier", + "ring_dip.origin", "ring_dip.limit", "ring_dip.mimic.multiplier")) + + +__all__ = ["O12UrdfCorrection", "o12_active_ranges", "o12_correction_plan", "write_o12_corrected_urdf"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/zero.py similarity index 72% rename from src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/zero.py index 7b6306d..bdfb6a3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/zero.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o12/zero.py @@ -10,51 +10,31 @@ import math from pathlib import Path from typing import Any, Mapping, Sequence -from ..g20.profile import JointCurveFit, JointSpec -from ..g20.zero_solver import ( +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.fitting.session import compile_spatial_profile +from linkerhand_calibration.core.fitting.spatial import ( ZeroCalibrationProfile, ZeroSolveResult, fit_joint_axis_measurement, solve_urdf_zero_offsets, with_depth_free_axis_projection, ) -from .profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import ( CALIBRATED_ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, + SDK_TO_URDF_SIGN, + build_typed_profile, ) -from .kinematics import PASSIVE_SDK_SOURCE_BY_JOINT +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.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", -) +# Historical diagnostic imports read the same protected YAML as production. +_SPATIAL = build_typed_profile().zero.spatial +AXIS_PARENT = dict(_SPATIAL["axis_parent_joint"]) +PHASE_PARENT = dict(_SPATIAL["phase_parent_joint"]) +ZERO_OBSERVER = dict(_SPATIAL["offset_observer_joint"]) +AXIS_JOINTS = tuple(_SPATIAL["axis_order"]) def motor_index(joint: str) -> int: @@ -62,24 +42,7 @@ def motor_index(joint: str) -> int: 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, - ) + return compile_spatial_profile(build_typed_profile()) class O12SpatialZeroError(ValueError): @@ -99,6 +62,7 @@ def solve_full_hand_zero( solver_curves: Mapping[str, JointCurveFit], ) -> ZeroSolveResult: profile = full_hand_zero_profile() + tasks = {joint: task for task in build_typed_profile().motion.tasks for joint in task.joints} measurements = [] for cycle in range(4): by_joint = {} @@ -117,7 +81,9 @@ def solve_full_hand_zero( 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, + name, rows, cycle=cycle, + zero_command_u8=tasks[name].start_value if "input_value" in rows[0] else 255, + input_to_joint_direction=int(SDK_TO_URDF_SIGN[motor_index(name)]), constrained_circle_joints=profile.constrained_circle_joints, view_normal_common_xyz=rows[0]["view_normal_common_xyz"], canonical_zero_direction="decreasing", diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/__init__.py new file mode 100644 index 0000000..b07d9f8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/__init__.py @@ -0,0 +1,14 @@ +"""Registered O6 calibration profiles.""" + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.left_transfer import build_profile as build_left_transfer_profile + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.profile import build_profile + + registry.register(build_profile()) + registry.register(build_left_transfer_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/artifacts.py similarity index 93% rename from src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/artifacts.py index 7d84051..a58b49d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/artifacts.py @@ -12,10 +12,10 @@ import xml.etree.ElementTree as ET import numpy as np -from ..g20.profile import JointCurveFit -from ..l6.artifacts import artifact_hashes, atomic_write_json, publish_partial_session -from .fitting import MimicFit, O6FitResult -from .profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import JointCurveFit +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.artifacts import artifact_hashes, atomic_write_json, publish_partial_session +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.fitting import MimicFit, O6FitResult +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.profile import ( ACTIVE_JOINTS, CALIBRATED_ACTIVE_JOINTS, COMMAND_INDEX_BY_JOINT, @@ -241,7 +241,7 @@ def build_o6_runtime_payload( def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: - from ...core import ProfileKey + from linkerhand_calibration.core import ProfileKey key = ProfileKey.parse(str(payload.get("profile_id", ""))) if key == KEY: @@ -251,7 +251,7 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: active_transfers = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT passive_transfers = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT else: - from .left_transfer import ( + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.left_transfer import ( KEY as LEFT_TRANSFER_KEY, build_typed_profile as build_left_transfer_profile, left_joint_name, @@ -299,10 +299,11 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: ): raise ValueError("O6 schema v6 identity or command contract is invalid") protected = payload["protected_inputs"] - if not isinstance(protected, Mapping) or set(protected) != { + legacy_protected = { "source_urdf_sha256", "camera_extrinsics_sha256", "calibration_config_sha256", "tag_config_sha256", - } or any(len(str(value)) != 64 for value in protected.values()): + } + if not isinstance(protected, Mapping) or set(protected) not in (legacy_protected, set(profile.artifacts.protected_input_fields)) or any(len(str(value)) != 64 for value in protected.values()): raise ValueError("O6 protected inputs are invalid") joints = payload["joints"] if not isinstance(joints, Mapping) or set(joints) != all_joints: @@ -327,9 +328,9 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: if name in passive_set: if ( item.get("source_joint") != mimic_sources[name] - or item.get("coupling_model") != coupling_models[name] + or item.get("coupling_model") not in {"linear_mimic", "quadratic_runtime"} or item.get("urdf_mimic_enabled") is not True - or item.get("urdf_mimic_policy") != "endpoint_linear_fallback" + or item.get("urdf_mimic_policy") != ("exact_linear" if item.get("coupling_model") == "linear_mimic" else "endpoint_linear_fallback") ): raise ValueError(f"O6 passive coupling is invalid: {name}") coefficients = np.asarray(item.get("coupling_coefficients"), dtype=float) @@ -346,12 +347,20 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None: multiplier = float(item.get("mimic_multiplier", "nan")) if not math.isfinite(multiplier) or multiplier <= 0.0: raise ValueError(f"O6 passive mimic multiplier is invalid: {name}") + if item["coupling_model"] == "linear_mimic": + if abs(multiplier - coefficients[1]) > 1e-7 or np.any(np.abs(coefficients[2:]) > 1e-10): + raise ValueError(f"{name} linear mimic is not linear") + parent = joints[mimic_sources[name]] + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + expected = coefficients[0] + multiplier * np.asarray(parent[field]) + if not np.allclose(item[field], expected, atol=1e-7, rtol=0): + raise ValueError(f"{name} curve differs from its declared standard mimic") donor = active_transfers.get(name) or passive_transfers.get(name) if donor is not None: if item.get("transferred_from_joint") != donor: raise ValueError(f"O6 transfer provenance is invalid: {name}") for field in ("angle_rad", "decreasing_rad", "increasing_rad"): - if item[field] != joints[donor][field]: + if name not in passive_set and item[field] != joints[donor][field]: raise ValueError(f"O6 transferred curve differs: {name}") if payload["quality"].get("passed") is not True: raise ValueError("O6 quality.passed must be true") @@ -365,7 +374,7 @@ def build_o6_left_transferred_runtime_payload( serial_number: str, ) -> dict[str, Any]: """Create a validated left runtime JSON paired with a mirrored URDF.""" - from .left_transfer import ( + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.left_transfer import ( KEY as LEFT_TRANSFER_KEY, build_typed_profile as build_left_transfer_profile, left_joint_name, diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/fitting.py new file mode 100644 index 0000000..e552376 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/fitting.py @@ -0,0 +1,25 @@ +"""Legacy result types plus a direct call to the ONE profile fitter. + +Old endpoint/range-centre fallbacks and model-specific refitting are retired. +The legacy result type remains only for reading archived JSON/tool inputs. +""" +from __future__ import annotations +from pathlib import Path +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.domain.result import JointMapping +from linkerhand_calibration.core.fitting import CouplingFit as MimicFit, fit_coupling_model, curve_travel_rad +from linkerhand_calibration.core.fitting.coupling import LinearMimicFit, RelativeMimicEvidence +from linkerhand_calibration.core.fitting.spatial import ZeroSolveResult +from linkerhand_calibration.core.fitting.session import fit_profile_calibration, compile_spatial_profile +from linkerhand_calibration.profiles import load_bundled_hand_profile + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.fitting import L6FitResult as O6FitResult + + +def _zero_profile(): + return compile_spatial_profile(load_bundled_hand_profile("o6_right_8")) + +def fit_o6_session(source_urdf, records_by_joint, *, require_thumb_axis_zero=False): + # Historical flags can no longer disable spatial or holdout verification. + return fit_profile_calibration(load_bundled_hand_profile("o6_right_8"), + Path(source_urdf), records_by_joint) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/left_transfer.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py similarity index 87% rename from src/linkerhand_calibration/linkerhand_calibration/models/o6/left_transfer.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py index 0f240fd..d41a4a8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/left_transfer.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/left_transfer.py @@ -4,9 +4,9 @@ from __future__ import annotations from dataclasses import replace -from ...core import ProfileKey -from ..registry import EngineBindings, RegisteredProfile -from .profile import build_typed_profile as build_right_typed_profile +from linkerhand_calibration.core import ProfileKey +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import EngineBindings, RegisteredProfile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.profile import build_typed_profile as build_right_typed_profile KEY = ProfileKey("O6", "left", "o6_left_transferred_8", 1) @@ -59,6 +59,12 @@ def build_typed_profile(): ), zero=replace( right.zero, + transferred_zero_sources={left_joint_name(k): left_joint_name(v) + for k, v in right.zero.transferred_zero_sources.items()}, + transferred_mimic_sources={left_joint_name(k): left_joint_name(v) + for k, v in right.zero.transferred_mimic_sources.items()}, + # Runtime-only mirrored products do not acquire/solve new geometry. + spatial={}, active_joints=active, passive_joints=passive, direct_zero_joints=tuple( @@ -135,7 +141,7 @@ def _unsupported(_args=None) -> None: def build_profile() -> RegisteredProfile: - from ..l6.motion import ( + from linkerhand_calibration.runtime.trajectory import ( build_calibration_motion_command, build_calibration_preparation_waypoints, build_calibration_return_waypoints, diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/profile.py new file mode 100644 index 0000000..fce6606 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/profile.py @@ -0,0 +1,86 @@ +"""Reviewed partial-calibration profile for the right O6 eight-Tag rig.""" + +from __future__ import annotations + +from linkerhand_calibration.core import CalibrationProfile, ProfileKey +from linkerhand_calibration.runtime.trajectory import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry import EngineBindings, RegisteredProfile + + +# Historical import constants are projections of the authoritative YAML, +# never a second set of model/zero/coupling definitions. +from linkerhand_calibration.profiles.loader import load_bundled_hand_profile +_DECLARED = load_bundled_hand_profile("o6_right_8") +KEY = _DECLARED.key +COMMAND_NAMES = _DECLARED.command.names +COMMAND_INDEX_BY_JOINT = dict(_DECLARED.command.command_index_by_joint) +ACTIVE_JOINTS = tuple(sorted(COMMAND_INDEX_BY_JOINT, key=COMMAND_INDEX_BY_JOINT.get)) +MIMIC_SOURCE_BY_JOINT = dict(_DECLARED.zero.mimic_source_by_joint) +PASSIVE_JOINTS = tuple(name for source in ACTIVE_JOINTS + for name, parent in MIMIC_SOURCE_BY_JOINT.items() if parent == source) +CALIBRATED_ACTIVE_JOINTS = _DECLARED.scope.selected_joints(_DECLARED.scope.default_scope) +MEASURED_PASSIVE_JOINTS = _DECLARED.zero.fitted_mimic_joints +TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = dict(_DECLARED.zero.transferred_zero_sources) +TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = dict(_DECLARED.zero.transferred_mimic_sources) +CORRECTED_ACTIVE_JOINTS = _DECLARED.zero.active_joints +CORRECTED_PASSIVE_JOINTS = frozenset(MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys()) +ENDPOINT_ANCHOR_BY_JOINT = dict(_DECLARED.zero.endpoint_anchor_by_joint) +COUPLING_MODEL_BY_JOINT = dict(_DECLARED.zero.coupling_model_by_joint) +GEOMETRIC_ZERO_JOINTS = frozenset(_DECLARED.zero.direct_zero_joints) +CAD_ENDPOINT_ZERO_JOINTS = frozenset() +BASELINE_SPEED_U8 = _DECLARED.motion.speed_parameters["baseline_u8"] +PREFLIGHT_SPEED_U8 = _DECLARED.motion.speed_parameters["preflight_u8"] +FORMAL_SPEED_U8 = _DECLARED.motion.speed_parameters["formal_u8"] +MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX = _DECLARED.vision.extrinsics_quality_limits["reprojection_rms_px"] +# Kept only for archived result readers, never online stop or publication gates. +MAXIMUM_HYSTERESIS_DEG = 3.5 +COUPLING_RESIDUAL_P95_DEG = 2.0 +COUPLING_RESIDUAL_MAX_DEG = 3.0 +MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M = 0.020 + + +def build_typed_profile() -> CalibrationProfile: + """Compatibility entry; the complete typed contract lives only in YAML.""" + from linkerhand_calibration.profiles.loader import load_bundled_hand_profile + return load_bundled_hand_profile("o6_right_8") + + +def _run_cli(args: list[str] | None = None) -> None: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.runner import main + + main(args) + + +def _run_node(args: list[str] | None = None) -> None: + from linkerhand_calibration.runtime.ros.calibration_node import run_profile_node + run_profile_node(build_typed_profile(), args) + + +def build_profile() -> RegisteredProfile: + typed = build_typed_profile() + return RegisteredProfile( + profile=typed, + engine=EngineBindings( + hand_profile=typed, + zero_profile=typed.zero, + motion_command=build_calibration_motion_command, + preparation_waypoints=build_calibration_preparation_waypoints, + return_waypoints=build_calibration_return_waypoints, + cli_main=_run_cli, + node_main=_run_node, + ), + ) + + +__all__ = [ + "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_NAMES", + "CORRECTED_ACTIVE_JOINTS", "CORRECTED_PASSIVE_JOINTS", + "COUPLING_MODEL_BY_JOINT", "ENDPOINT_ANCHOR_BY_JOINT", "KEY", + "MEASURED_PASSIVE_JOINTS", "MIMIC_SOURCE_BY_JOINT", "PASSIVE_JOINTS", + "TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", + "TRANSFERRED_PASSIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/runner.py new file mode 100644 index 0000000..6ba7101 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/runner.py @@ -0,0 +1,4 @@ +"""Legacy import path; all online work uses the common product runner.""" + +from linkerhand_calibration.runtime.runner import main +from linkerhand_calibration.compat.legacy_diagnostic_tools.progress import render_o6_progress_zh diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/urdf.py similarity index 95% rename from src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/urdf.py index 029c9fa..2d7ca1c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/urdf.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/o6/urdf.py @@ -11,10 +11,14 @@ import xml.etree.ElementTree as ET import numpy as np from scipy.spatial.transform import Rotation -from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches -from ..l6.urdf import L6UrdfCorrection, _corrected_origin_rpy, _triplet -from .fitting import O6FitResult -from .profile import ( +from linkerhand_calibration.core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches +from linkerhand_calibration.core.urdf import ( + corrected_origin_rpy as _corrected_origin_rpy, + parse_vector3 as _triplet, +) +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.urdf import L6UrdfCorrection +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.fitting import O6FitResult +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.profile import ( CALIBRATED_ACTIVE_JOINTS, CORRECTED_ACTIVE_JOINTS, CORRECTED_PASSIVE_JOINTS, @@ -22,6 +26,7 @@ from .profile import ( MIMIC_SOURCE_BY_JOINT, TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, + build_typed_profile, ) @@ -254,6 +259,7 @@ def write_o6_corrected_urdf( patches=UrdfPatchSet(joints=patches), forbidden_source_stem_patterns=(r"calibrated",), copy_complete_mesh_directory=True, + authorized_fields=build_typed_profile().urdf_authorized_fields, ) return O6UrdfCorrection( path=destination, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/registry.py similarity index 80% rename from src/linkerhand_calibration/linkerhand_calibration/models/registry.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/registry.py index b2fcd54..b18d0b7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/registry.py @@ -5,9 +5,9 @@ from __future__ import annotations 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 +from linkerhand_calibration.core import CalibrationProfile, ProfileKey, validate_profile +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.adapters import ProfileSdkAdapter @dataclass(frozen=True) @@ -85,10 +85,10 @@ _DEFAULT_REGISTRY: ProfileRegistry | None = None def get_default_registry() -> ProfileRegistry: global _DEFAULT_REGISTRY if _DEFAULT_REGISTRY is None: - from .g20 import register_profiles - from .l6 import register_profiles as register_l6_profiles - from .o6 import register_profiles as register_o6_profiles - from .o12 import register_profiles as register_o12_profiles + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20 import register_profiles + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6 import register_profiles as register_l6_profiles + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6 import register_profiles as register_o6_profiles + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12 import register_profiles as register_o12_profiles registry = ProfileRegistry() register_profiles(registry) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/runtime_schema.py similarity index 66% rename from src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/runtime_schema.py index 1b42c27..8f7e438 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/runtime_schema.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/models/runtime_schema.py @@ -4,18 +4,18 @@ from __future__ import annotations from typing import Any, Mapping -from ..core import ProfileKey +from linkerhand_calibration.core import ProfileKey def validate_schema_v6_runtime_payload(payload: Mapping[str, Any]) -> None: key = ProfileKey.parse(str(payload.get("profile_id", ""))) if key.model == "L6": - from .l6.artifacts import validate_l6_runtime_payload + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.artifacts import validate_l6_runtime_payload validate_l6_runtime_payload(payload) return if key.model == "O6": - from .o6.artifacts import validate_o6_runtime_payload + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.artifacts import validate_o6_runtime_payload validate_o6_runtime_payload(payload) return diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/progress.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/progress.py new file mode 100644 index 0000000..b2ff8d7 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/progress.py @@ -0,0 +1,251 @@ +"""Read-only renderers for old recorded status payloads, not live runners.""" + +from __future__ import annotations + +from typing import Any, Mapping + +from ...operator_report import ProgressEstimator +from ...runtime.reporting import reason_zh, render_progress_zh + +common_reason_zh = reason_zh +render_six_channel_progress_zh = render_progress_zh + +_L6_TASK_LABELS = { + "thumb_roll_top": "拇指 CMC roll（上方机位，ID6→ID7）", + "thumb_pitch_dip_front": "拇指 CMC pitch / DIP（正面机位，ID0→ID1→ID2）", + "pinky_pitch_dip_side": "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）", +} + + +def _l6_reason_zh( + status: Mapping[str, Any], *, model_name: str = "L6" +) -> tuple[str, str, str]: + return common_reason_zh(status, model_name=model_name) + + +def render_l6_progress_zh( + status: Mapping[str, Any], + estimator: ProgressEstimator | None = None, +) -> str: + """Render L6 progress in the same operator-oriented layout as G20.""" + return render_six_channel_progress_zh( + status, + task_labels=_L6_TASK_LABELS, + reason_renderer=_l6_reason_zh, + estimator=estimator, + ) + +_O6_TASK_LABELS = { + "thumb_yaw_top": "拇指 CMC yaw（上面机位，ID6→ID7）", + "thumb_pitch_ip_front": "拇指 CMC pitch / IP（正面机位，ID0→ID1→ID2）", + "pinky_pitch_dip_side": "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）", +} + + +def render_o6_progress_zh( + status: dict[str, Any], + estimator: ProgressEstimator | None = None, +) -> str: + return render_progress_zh( + status, + task_labels=_O6_TASK_LABELS, + reason_renderer=lambda value: reason_zh( + value, model_name="O6" + ), + estimator=estimator, + ) + +_O12_TASK_LABELS = { + "thumb_pitch_front": "拇指 CMC pitch（正面 ID0→ID1）", + "thumb_roll_front": "拇指 CMC roll（正面 ID0→ID1）", + "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 + 侧面 ID4→ID8）", + "middle_mcp_side": "中指 MCP pitch（侧面 ID4→ID8）", + "middle_pip_dip_side": "中指 PIP / 被动 DIP（侧面 ID8→ID9）", + "index_roll_front": "食指 MCP roll（正面 ID0→ID13 + 侧面 ID4→ID10）", + "index_mcp_side": "食指 MCP pitch（侧面 ID4→ID10）", + "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 ( + "O12-MAPPING-301", + f"固定 SDK 映射点动时没有观察到目标关节运动：{task}。", + "检查对应 Tag、通道映射和机械连接；不要交换通道后强行继续。", + ) + 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('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 reason_zh(status, model_name="O12") + + +def render_o12_progress_zh(status, estimator=None) -> str: + """Render O12 in the same operator-oriented layout as G20 and L6.""" + text = render_progress_zh( + status, + task_labels=_O12_TASK_LABELS, + reason_renderer=_o12_reason_zh, + estimator=estimator, + ) + temperature = ( + "直接温度回读" + if status.get("temperature_report_verified") + else "错误码 bit1 过热保护" + if status.get("temperature_fallback_active") + else "等待温度能力确认" + ) + 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"（{error_health_text}）；" + f"温度策略={temperature}；速度倍率=" + f"{float(status.get('motion_speed_scale', 1.0)):.1f}x；" + f"控制发布={float(status.get('command_publish_hz', 0.0)):.1f} Hz" + ) + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/runner_helpers.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/runner_helpers.py new file mode 100644 index 0000000..850e2c0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy_diagnostic_tools/runner_helpers.py @@ -0,0 +1,283 @@ +"""Read-only helpers retained for archived runner diagnostics; no live loop.""" + +from __future__ import annotations + +import json +import os +from pathlib import Path +from typing import Any, Mapping + +from ...product import ProductConfig, sha256_file +from ...runtime.engine import ACQUISITION_POLICY_VERSION + +STATUS_TIMEOUT_SECONDS = 90.0 +FITTING_STATUS_TIMEOUT_SECONDS = 600.0 + +def _status_timeout_seconds(status: Mapping[str, Any]) -> float: + """Return the watchdog deadline for the node's current phase. + + Motion and acquisition are expected to publish twice a second and retain + the strict transport watchdog. The final 3-D fit is intentionally a + synchronous, CPU-bound operation, so its executor cannot service the + status timer until the fit returns. The node publishes an explicit + FITTING status immediately before entering that operation; allow that + known phase enough time without weakening motion safety checks. + """ + if str(status.get("state", "")) == "FITTING": + return FITTING_STATUS_TIMEOUT_SECONDS + return STATUS_TIMEOUT_SECONDS + + +def _calibration_node_exited_before_status(log_path: Path) -> bool: + """Detect a launch child crash while the parent launch is still alive.""" + try: + with log_path.open("rb") as stream: + stream.seek(0, os.SEEK_END) + size = stream.tell() + stream.seek(max(0, size - 65536), os.SEEK_SET) + tail = stream.read().decode("utf-8", errors="replace") + except OSError: + return False + return ( + "[three_camera_calibration_node-" in tail + and "]: process has died" in tail + ) + +def _launch_command( + config: ProductConfig, + session: Path, + *, + resume_from: Path | None = None, + recalibration_scope: str = "full", +) -> list[str]: + values = { + "model": config.model, + "hand_type": config.side, + "tag_layout": config.tag_layout, + "serial_number": config.serial_number, + "can_interface": config.can_interface, + "session_dir": str(session), + "output_root": str(config.output_root), + "camera_extrinsics_file": str(config.camera_extrinsics), + "source_urdf_path": str(config.source_urdf), + "source_urdf_expected_sha256": config.source_urdf_sha256, + "camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256, + "calibration_config_expected_sha256": config.calibration_config_sha256, + "tag_config_expected_sha256": config.tag_config_sha256, + "profile_config_expected_sha256": getattr( + config, "profile_config_sha256", "" + ), + "corrected_urdf_output_dir": str(session), + "calibration_config": str(config.calibration_config), + "tag_config": str(config.tag_config), + "commands_enabled": "true", + "start_cameras": "true", + "start_sdk": "true", + "record_bag": "false", + "validation_enabled": "false", + "recalibration_scope": recalibration_scope, + } + if resume_from is not None: + values["resume_raw_samples_path"] = str( + resume_from / "raw_samples.jsonl" + ) + for view, camera in config.cameras.items(): + values[f"{view}_camera_serial"] = camera["serial_number"] + values[f"{view}_camera_name"] = camera["camera_name"] + values[f"{view}_camera_info_url"] = camera["camera_info"] + return [ + "ros2", + "launch", + "linkerhand_calibration", + "unified_calibration.launch.py", + *(f"{name}:={value}" for name, value in values.items()), + ] + +def _automatic_resume_candidate(config: ProductConfig) -> Path | None: + """Return the newest compatible failed attempt, never a passed session. + + Do not trust only ``latest_attempt``. A process interrupted during the + device-only startup gate may have already moved that pointer while still + containing no ``session_start`` checkpoint. In that case walk backwards + to the preceding usable failed session instead of throwing away hours of + completed tasks. + """ + root = config.session_root + try: + resolved_root = root.resolve(strict=True) + except OSError: + return None + + candidates: list[Path] = [] + pointer = config.session_root / "latest_attempt" + if pointer.exists(): + try: + candidates.append(pointer.resolve(strict=True)) + except OSError: + pass + try: + candidates.extend( + sorted( + ( + path + for path in root.iterdir() + if path.is_dir() and not path.name.startswith("latest_") + ), + key=lambda path: path.name, + reverse=True, + ) + ) + except OSError: + return None + + passed_pointer = config.session_root / "latest_passed" + passed: Path | None = None + if passed_pointer.exists(): + try: + passed = passed_pointer.resolve(strict=True) + except OSError: + pass + + seen: set[Path] = set() + for unresolved in candidates: + try: + candidate = unresolved.resolve(strict=True) + except OSError: + continue + if candidate in seen: + continue + seen.add(candidate) + if candidate.parent != resolved_root or not candidate.is_dir(): + continue + # A failed attempt older than the current formal release is stale and + # must not seed a new independent calibration. + if passed is not None and candidate.name <= passed.name: + continue + raw_path = candidate / "raw_samples.jsonl" + if not raw_path.is_file(): + continue + summary_path = candidate / "calibration_summary_zh.json" + summary: dict[str, Any] | None = None + if summary_path.is_file(): + try: + loaded = json.loads(summary_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError): + continue + if not isinstance(loaded, dict) or loaded.get("result") != "FAIL": + continue + summary = loaded + hashes = summary.get("hashes", {}) + if not isinstance(hashes, Mapping): + continue + if ( + str(hashes.get("source_urdf_sha256", "")) + != config.source_urdf_sha256 + or str(hashes.get("camera_extrinsics_sha256", "")) + != config.camera_extrinsics_sha256 + ): + continue + start: dict[str, Any] | None = None + try: + with raw_path.open("r", encoding="utf-8") as stream: + for line in stream: + if not line.strip(): + continue + value = json.loads(line) + if ( + isinstance(value, dict) + and value.get("kind") == "session_start" + ): + start = value + break + except (OSError, json.JSONDecodeError): + 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") + != config.source_urdf_sha256 + or ( + bool(start.get("protected_hashes")) + and dict(start.get("protected_hashes", {})) + != { + "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, + "profile_config_sha256": config.profile_config_sha256, + } + ) + ): + continue + if summary is None: + # Ctrl+C can terminate the ROS launch tree before the wrapper gets + # a chance to create calibration_summary_zh.json. The immutable + # checkpoint itself is enough to resume only after independently + # proving that its external geometry still matches the product. + try: + checkpoint_extrinsics = Path( + str(start["camera_extrinsics_file"]) + ).expanduser().resolve(strict=True) + if sha256_file(checkpoint_extrinsics) != ( + config.camera_extrinsics_sha256 + ): + continue + except (KeyError, OSError, ValueError): + continue + return candidate + return None + + +def _resolve_partial_base_session( + config: ProductConfig, base_session: str | Path | None +) -> Path: + """Validate the complete passed session that donates non-target tasks.""" + if base_session is None or not str(base_session).strip(): + raise ValueError( + "partial scope requires --base-session pointing to a passed " + "complete G20 right session" + ) + candidate = Path(base_session).expanduser().resolve(strict=True) + root = config.session_root.resolve() + if candidate.parent != root or not candidate.is_dir(): + raise ValueError( + "base session must resolve to a direct session directory under " + f"{root}" + ) + raw_path = candidate / "raw_samples.jsonl" + summary_path = candidate / "calibration_summary_zh.json" + payload_path = ( + candidate + / f"g20_right_{config.serial_number}_calibration.json" + ) + for required in (raw_path, summary_path, payload_path): + if not required.is_file(): + raise ValueError(f"base session is missing required artifact: {required}") + try: + summary = json.loads(summary_path.read_text(encoding="utf-8")) + except json.JSONDecodeError as error: + raise ValueError("base session summary is invalid JSON") from error + if ( + not isinstance(summary, dict) + or summary.get("result") != "PASS" + or not bool(summary.get("quality", {}).get("passed")) + ): + raise ValueError("base session is not a formally passed session") + hashes = summary.get("hashes", {}) + expected_hashes = { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + } + if not isinstance(hashes, Mapping) or any( + str(hashes.get(name, "")) != expected + for name, expected in expected_hashes.items() + ): + raise ValueError( + "base session source URDF, camera extrinsics or calibration " + "configuration differs from the current product" + ) + return candidate diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py index b6b43d6..6cd02ad 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py @@ -3,17 +3,23 @@ from .domain import ( AcquisitionPolicy, ArtifactPolicy, + AcquisitionStatus, CalibrationProfile, + CalibrationResult, + JointMapping, + CalibrationStatus, CommandLayout, MeasurementPolicy, MeasurementSpec, MotionPolicy, + MotionStatus, + PauseStatus, ProfileKey, ProfileValidationError, QualityPolicy, - SampleRecord, ScopePolicy, TagSpec, + TaskStatus, TaskSpec, ViewSpec, VisionRigSpec, @@ -43,7 +49,11 @@ from .geometry import ( __all__ = [ "AcquisitionPolicy", "ArtifactPolicy", + "AcquisitionStatus", "CalibrationProfile", + "CalibrationResult", + "JointMapping", + "CalibrationStatus", "CommandLayout", "DIRECTION_DECREASING", "DIRECTION_INCREASING", @@ -52,14 +62,16 @@ __all__ = [ "MeasurementPolicy", "MeasurementSpec", "MotionPolicy", + "MotionStatus", + "PauseStatus", "PHASE_ROOT", "PHASE_TIP", "ProfileKey", "ProfileValidationError", "QualityPolicy", - "SampleRecord", "ScopePolicy", "TagSpec", + "TaskStatus", "TaskSpec", "ViewSpec", "VisionRigSpec", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py index 25cb69c..26ef89e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py @@ -1,5 +1 @@ -"""Artifact schema and release validation contracts.""" - -from .release import ReleaseValidation, ReleaseValidator - -__all__ = ["ReleaseValidation", "ReleaseValidator"] +"""Storage helpers shared by calibration artifact writers.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py deleted file mode 100644 index ce0bf4b..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py +++ /dev/null @@ -1,27 +0,0 @@ -"""Release validation protocol used before atomic publication.""" - -from __future__ import annotations - -from dataclasses import dataclass -from pathlib import Path -from typing import Mapping, Protocol - -from ..domain import CalibrationProfile -from ..urdf import UrdfCorrectionPlan - - -@dataclass(frozen=True) -class ReleaseValidation: - passed: bool - errors: tuple[str, ...] = () - verified_hashes: Mapping[str, str] | None = None - - -class ReleaseValidator(Protocol): - def validate_release( - self, - profile: CalibrationProfile, - plan: UrdfCorrectionPlan, - calibration_json: Path, - corrected_urdf: Path, - ) -> ReleaseValidation: ... diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py index dd75d79..cfd381e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py @@ -19,22 +19,35 @@ from .profile import ( ZeroSolvePolicy, validate_profile, ) -from .sample import SampleRecord +from .result import CalibrationResult, JointMapping +from .status import ( + AcquisitionStatus, + CalibrationStatus, + MotionStatus, + PauseStatus, + TaskStatus, +) __all__ = [ "AcquisitionPolicy", "ArtifactPolicy", + "AcquisitionStatus", "CalibrationProfile", + "CalibrationResult", + "JointMapping", + "CalibrationStatus", "CommandLayout", "MeasurementPolicy", "MeasurementSpec", "MotionPolicy", + "MotionStatus", "ProfileKey", "ProfileValidationError", + "PauseStatus", "QualityPolicy", - "SampleRecord", "ScopePolicy", "TagSpec", + "TaskStatus", "TaskSpec", "ViewSpec", "VisionRigSpec", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/measurement.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/measurement.py new file mode 100644 index 0000000..cbb27a9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/measurement.py @@ -0,0 +1,72 @@ +"""Shared measurement and curve contracts; no hardware or model defaults.""" +from __future__ import annotations +from dataclasses import dataclass +from typing import Any, Mapping + +@dataclass(frozen=True) +class JointSpec: + name: str + motor_index: int + active: bool + view: str | None + parent_role: str | None + child_role: str | None + source_joint: str | None = None + zero_kind: str | None = None + # Keep the physical axis-line gate only when that line contributes to a + # released URDF zero/axis decision. Curve-only passive measurements may + # retain the monocular line residual as a diagnostic while their image + # trajectory, relative rotation, synchronisation and holdout gates remain + # release-critical. + pose_axis_line_required: bool = True + + @property + def measured(self) -> bool: + return self.source_joint is None + + +@dataclass(frozen=True) +class SweepSpec: + view: str + motor_index: int + joints: tuple[str, ...] + task_name: str = "" + auxiliary_commands: tuple[tuple[int, int], ...] = () + validation_only: bool = False + + @property + def key(self) -> str: + if self.task_name: + return self.task_name + return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}" + + +@dataclass(frozen=True) +class PalmAxisObserver: + """Non-blocking direction observation attached to an existing task. + + This is deliberately not a ``JointSpec``: it has no curve, endpoint or + retry semantics. The camera callback records it only while both Tags are + visible during the named sweep. + """ + + source_name: str + task_name: str + view: str + parent_role: str + child_role: str + model_joint: str + motor_index: int + + +@dataclass(frozen=True) +class JointCurveFit: + angle_rad: tuple[float, ...] + decreasing_rad: tuple[float, ...] + increasing_rad: tuple[float, ...] + circle: Mapping[str, Any] + maximum_monotonic_correction_rad: float + maximum_hysteresis_rad: float + quality: Mapping[str, float] + zero_offset_rad: float = 0.0 + diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index e0d26aa..6fa6244 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -8,6 +8,9 @@ from pathlib import PurePath from typing import Mapping +ACQUISITION_POLICY_VERSION = "unified_engine_v4_dual_mapping" + + @dataclass(frozen=True, order=True) class ProfileKey: """Stable identity for one independently reviewed hand profile.""" @@ -74,6 +77,15 @@ class CommandLayout: feedback_lower_bounds: tuple[float, ...] = () feedback_upper_bounds: tuple[float, ...] = () feedback_by_index: bool = False + # Signed derivative of the URDF coordinate with respect to native SDK + # feedback, in fixed channel order. It is a product contract, not inferred + # by swapping channels after observing motion. + sdk_to_joint_direction: tuple[int, ...] = () + maximum_velocity: tuple[float, ...] = () + + @property + def joint_directions(self) -> tuple[int, ...]: + return self.sdk_to_joint_direction or (-1,) * self.command_count @property def command_count(self) -> int: @@ -137,6 +149,8 @@ class TagSpec: role: str tag_id: int fixed_reference: bool = False + link: str | None = None + size_m: float = 0.016 @dataclass(frozen=True) @@ -182,6 +196,8 @@ class TaskSpec: end: float | None = None preflight_speed: float | None = None formal_speed: float | None = None + preparation_groups: tuple[tuple[int, ...], ...] = () + entry_waypoints: tuple[tuple[tuple[int, float], ...], ...] = () @property def start_value(self) -> float: @@ -192,6 +208,13 @@ class TaskSpec: return float(self.end_u8 if self.end is None else self.end) +@dataclass(frozen=True) +class ReferenceWaypoint: + key: str + command: tuple[float, ...] + tag_ids_by_view: Mapping[str, tuple[int, ...]] + + @dataclass(frozen=True) class MotionPolicy: """Reviewed motion tasks and optional safe waypoint sequences.""" @@ -202,6 +225,8 @@ class MotionPolicy: speed_parameters: Mapping[str, float] = field(default_factory=dict) precheck_sweeps: bool = False steady_command_checkpoints: bool = False + return_groups: tuple[tuple[int, ...], ...] = () + resume_verification_waypoints: tuple[ReferenceWaypoint, ...] = () @dataclass(frozen=True) @@ -228,6 +253,8 @@ class MeasurementPolicy: directional_zero: bool = False cross_view_roll_curve: bool = False stable_cross_view_cone_bias: bool = False + candidate_selection_tasks: frozenset[str] = frozenset() + input_domain: str = "" @dataclass(frozen=True) @@ -249,6 +276,10 @@ class ZeroSolvePolicy: # ``mimic`` element. Profiles must opt in explicitly before a nonlinear # runtime/MuJoCo relation may be published. coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict) + transferred_zero_sources: Mapping[str, str] = field(default_factory=dict) + transferred_mimic_sources: Mapping[str, str] = field(default_factory=dict) + # Geometry policy only; no Python callback/import strings are permitted. + spatial: Mapping[str, object] = field(default_factory=dict) @dataclass(frozen=True) @@ -264,7 +295,7 @@ class QualityPolicy: class AcquisitionPolicy: """Cross-model runtime safety and retained-data acceptance contract.""" - policy_version: str = "unified_engine_v1" + policy_version: str = ACQUISITION_POLICY_VERSION mapping_probe_maximum_rad: float = 0.0 automatic_rescan_limit: int = 1 minimum_valid_samples: int = 40 @@ -274,8 +305,16 @@ class AcquisitionPolicy: physical_first_cycle_minimum_span_01: float = 0.85 physical_repeat_minimum_fraction: float = 0.90 stall_timeout_seconds: float = 2.0 + feedback_stale_seconds: float = 1.0 + fixed_reference_minimum_frames: int = 10 fixed_reference_maximum_drift_px: float = 5.0 fixed_reference_confirmation_frames: int = 10 + require_endpoint_observations: bool = False + endpoint_tolerance_01: float = 2.0 / 255.0 + steady_training_nodes: int = 9 + steady_minimum_samples: int = 3 + steady_window_seconds: float = 0.2 + steady_timeout_seconds: float = 2.0 @dataclass(frozen=True) @@ -315,12 +354,20 @@ class CalibrationProfile: scope: ScopePolicy artifacts: ArtifactPolicy acquisition: AcquisitionPolicy = field(default_factory=AcquisitionPolicy) + sdk_adapter: str = "legacy_byte_sdk" + urdf_authorized_fields: Mapping[str, frozenset[str]] = field( + default_factory=dict + ) # 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, # and mimic_nominal. joint_coverage: Mapping[str, str] = field(default_factory=dict) + @property + def curve_input_domain(self) -> str: + return self.measurement.input_domain or f"feedback_{self.command.unit}" + class ProfileValidationError(ValueError): """Raised before hardware startup when a profile is internally unsafe.""" @@ -337,6 +384,12 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append("command names must be unique") if command.unit not in {"u8", "rad"}: errors.append("command unit must be u8 or rad") + if profile.curve_input_domain not in {f"feedback_{command.unit}", f"command_{command.unit}"}: + errors.append("curve input must explicitly identify native command or feedback coordinates") + if (len(command.joint_directions) != command.command_count + or any(value not in {-1, 1} for value in command.joint_directions) + or (command.unit == "rad" and not command.sdk_to_joint_direction)): + errors.append("native SDK directions must explicitly bind every physical-angle channel") if command.unit == "u8" and any(value < 0 or value > 255 for value in baseline): errors.append("baseline command values must be in [0, 255]") if ( @@ -363,8 +416,6 @@ def validate_profile(profile: CalibrationProfile) -> None: 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, @@ -373,9 +424,12 @@ def validate_profile(profile: CalibrationProfile) -> None: ) ): errors.append( - "feedback bounds must be finite and contain the command domain" + "feedback bounds must be finite and ordered in feedback coordinates" ) indices = set(range(command.command_count)) + if command.maximum_velocity and (len(command.maximum_velocity) != command.command_count + or any(not math.isfinite(v) or v <= 0 for v in command.maximum_velocity)): + errors.append("per-channel velocity limits must be finite, positive and complete") if not set(command.disabled_indices).issubset(indices): errors.append("disabled command index is out of range") if any(index not in indices for index in command.command_index_by_joint.values()): @@ -406,6 +460,8 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append("extrinsic reference view is not declared") tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags] tag_roles = [tag.role for view in profile.vision.views for tag in view.tags] + if any(not math.isfinite(tag.size_m) or tag.size_m <= 0 for view in profile.vision.views for tag in view.tags): + errors.append("Tag side lengths must be finite positive meters") if len(set(tag_ids)) != len(tag_ids): errors.append("Tag IDs must be unique across views") if len(set(tag_roles)) != len(tag_roles): @@ -416,6 +472,24 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append("at least one fixed reference Tag is required") task_keys = [task.key for task in profile.motion.tasks] + reference_keys = set() + for waypoint in profile.motion.resume_verification_waypoints: + if not waypoint.key or waypoint.key in reference_keys or "/" in waypoint.key: + errors.append("reference waypoint keys must be nonempty, unique and contain no slash") + reference_keys.add(waypoint.key) + if len(waypoint.command) != command.command_count or any( + not math.isfinite(v) or not lo <= v <= hi for v, lo, hi in + zip(waypoint.command, command.minimum_values, command.maximum_values)): + errors.append("reference waypoint command outside declared domain") + if any(i < len(waypoint.command) and waypoint.command[i] != command.baseline_values[i] + for i in command.disabled_indices): + errors.append("reference waypoint must keep disabled channels at baseline") + if not waypoint.tag_ids_by_view or any(view not in view_names or not ids or not set(ids) <= { + tag.tag_id for rig_view in profile.vision.views if rig_view.name == view for tag in rig_view.tags + if not tag.fixed_reference} for view, ids in waypoint.tag_ids_by_view.items()): + errors.append("reference waypoint must name observed moving Tags in their declared views") + if not profile.measurement.candidate_selection_tasks <= set(task_keys): + errors.append("candidate selection references an unknown motion task") if not task_keys or len(set(task_keys)) != len(task_keys): errors.append("motion task keys must be non-empty and unique") measurement_names = set(profile.measurement.measurements) @@ -424,6 +498,7 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"task {task.key} uses an unknown view") if task.command_index not in indices: errors.append(f"task {task.key} command index is out of range") + continue if not task.joints or not set(task.joints).issubset(measurement_names): errors.append(f"task {task.key} references unknown measurements") if any(index not in indices for index, _ in task.auxiliary_commands): @@ -439,11 +514,21 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"task {task.key} sweep endpoint is out of range") if task.start_value == task.end_value: errors.append(f"task {task.key} sweep endpoints must differ") + grouped = [index for group in task.preparation_groups for index in group] + if any(not group for group in task.preparation_groups) or len(grouped) != len(set(grouped)) or not set(grouped) <= indices: + errors.append(f"task {task.key} preparation groups must contain unique valid channels") + if any(task.command_index in group for group in task.preparation_groups[:-1]): + errors.append(f"task {task.key} measured channel must move after avoidance channels") + for waypoint in task.entry_waypoints: + if not waypoint or len({i for i, _ in waypoint}) != len(waypoint) or any( + i not in indices or i in command.disabled_indices or not math.isfinite(v) + or not command.minimum_values[i] <= v <= command.maximum_values[i] for i, v in waypoint): + errors.append(f"task {task.key} has an invalid entry waypoint") for speed in (task.preflight_speed_u8, task.formal_speed_u8): if speed is not None and not 0 <= speed <= 255: errors.append(f"task {task.key} speed is out of range") if command.unit == "rad" and ( - task.formal_speed is None or float(task.formal_speed) <= 0.0 + task.formal_speed is None or not math.isfinite(float(task.formal_speed)) or float(task.formal_speed) <= 0.0 ): errors.append(f"task {task.key} physical speed must be positive") for name, spec in profile.measurement.measurements.items(): @@ -451,9 +536,16 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append(f"measurement mapping key differs for {name}") if spec.view is not None and spec.view not in view_names: errors.append(f"measurement {name} uses an unknown view") + if spec.view is not None: + roles = {tag.role for view in profile.vision.views if view.name == spec.view for tag in view.tags} + if spec.parent_role not in roles or spec.child_role not in roles or spec.parent_role == spec.child_role: + errors.append(f"measurement {name} must reference distinct parent/child Tags in its view") for primary, validation in profile.measurement.cross_view_sources.items(): if primary not in measurement_names or validation not in measurement_names: errors.append("cross-view measurement source is unknown") + return_indices = [index for group in profile.motion.return_groups for index in group] + if any(not group for group in profile.motion.return_groups) or len(return_indices) != len(set(return_indices)) or not set(return_indices) <= indices: + errors.append("return groups must contain unique valid channels") zero = profile.zero if zero.active_joints & zero.passive_joints: @@ -486,6 +578,12 @@ def validate_profile(profile: CalibrationProfile) -> None: errors.append("endpoint anchor policy is unsupported") if not zero.fitted_mimic_joints.issubset(zero.passive_joints): errors.append("fitted mimic targets must be passive joints") + for transfers, targets, donors, label in ( + (zero.transferred_zero_sources, zero.active_joints, set(zero.direct_zero_joints), "zero"), + (zero.transferred_mimic_sources, zero.passive_joints, zero.fitted_mimic_joints, "mimic"), + ): + if not set(transfers) <= targets or not set(transfers.values()) <= donors or any(k == v for k, v in transfers.items()): + errors.append(f"{label} transfer requires distinct known recipients and measured donors") if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint): errors.append("fitted mimic target has no source mapping") if not set(zero.coupling_model_by_joint).issubset( @@ -525,8 +623,22 @@ 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") + if not str(profile.sdk_adapter).strip(): + errors.append("SDK adapter name must be non-empty") + valid_urdf_fields = { + "origin.rpy", "limit.lower", "limit.upper", + "mimic.multiplier", "mimic.offset", + } + if not set().union(*profile.urdf_authorized_fields.values(), set()).issubset( + valid_urdf_fields + ): + errors.append("URDF authorization contains an unsupported field") acquisition = profile.acquisition - if acquisition.policy_version != "unified_engine_v1": + if acquisition.steady_training_nodes < 3 or acquisition.steady_minimum_samples < 3: + errors.append("steady mapping requires at least three nodes and three images per node") + if not 0 < acquisition.steady_window_seconds < acquisition.steady_timeout_seconds <= 2.0: + errors.append("steady acquisition must have a finite stability window and <=2 second wait") + if acquisition.policy_version != ACQUISITION_POLICY_VERSION: errors.append("unsupported acquisition policy version") if acquisition.automatic_rescan_limit != 1: errors.append("unified engine requires exactly one automatic rescan") @@ -538,6 +650,19 @@ def validate_profile(profile: CalibrationProfile) -> None: 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 acquisition.feedback_stale_seconds <= 0.0: + errors.append("feedback stale timeout must be positive") + if not all(math.isfinite(float(value)) and float(value) > 0 for value in ( + acquisition.feedback_stale_seconds, acquisition.stall_timeout_seconds, + acquisition.fixed_reference_maximum_drift_px, + )): + errors.append("safety timeouts and reference drift must be finite and positive") + if not 0 <= acquisition.endpoint_tolerance_01 < 0.5: + errors.append("endpoint observation tolerance must be in [0, 0.5)") + if acquisition.fixed_reference_minimum_frames < 10: + errors.append("fixed reference locking requires at least ten frames") + if acquisition.fixed_reference_confirmation_frames < 10: + errors.append("fixed reference movement requires ten confirming frames") if profile.joint_coverage: valid_coverage = { "measured_static_dynamic", diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py new file mode 100644 index 0000000..d49772f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/result.py @@ -0,0 +1,97 @@ +"""Frozen numerical results, independent of a serializer or SDK transport.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import math +from typing import TYPE_CHECKING, Mapping, Sequence + +import numpy as np + +if TYPE_CHECKING: + from .measurement import JointCurveFit + from ..fitting.coupling import RelativeMimicEvidence + from ..fitting.spatial import ZeroSolveResult + + +@dataclass(frozen=True) +class CalibrationResult: + """Training-frozen measurements and parameters consumed by every writer. + + This is not a publication certificate: the final serialized URDF must + still pass independent spatial replay and structural authorization. + """ + + curves: Mapping[str, JointCurveFit] + output_mappings: Mapping[str, JointMapping] + zero_offsets_rad: Mapping[str, float] + zero_method_by_joint: Mapping[str, str] + standard_mimic_evidence: Mapping[str, RelativeMimicEvidence] + holdout_errors_rad: Mapping[str, tuple[float, ...]] + spatial_zero: ZeroSolveResult + cross_view_metrics: Mapping = field(default_factory=dict) + reference_inputs: Mapping[str, float] = field(default_factory=dict) + command_mappings: Mapping[str, JointMapping] = field(default_factory=dict) + command_holdout_metrics: Mapping = field(default_factory=dict) + command_applicability: Mapping = field(default_factory=dict) + + +@dataclass(frozen=True) +class JointMapping: + """SDK input -> output URDF coordinate; branches use raw SDK direction. + + Coordinate/sign choices belong to fitting. Writers may round numbers but + must not rebase, reverse, clip or refit this result. + """ + + joint: str + motor_index: int + input_domain: str + knots: tuple[float, ...] + angle_rad: tuple[float, ...] + increasing_rad: tuple[float, ...] + decreasing_rad: tuple[float, ...] + transferred_from: str | None = None + + def __post_init__(self) -> None: + if not self.joint or self.motor_index < 0: + raise ValueError("mapping requires a joint and a valid SDK channel") + if self.input_domain not in {"feedback_rad", "command_rad", "feedback_u8", "command_u8"}: + raise ValueError("mapping requires an explicit SDK input domain") + x = np.asarray(self.knots, dtype=float) + if x.ndim != 1 or x.size < 2 or not np.all(np.isfinite(x)) or np.any(np.diff(x) <= 0): + raise ValueError("mapping knots must be finite and strictly increasing") + for branch in (self.angle_rad, self.increasing_rad, self.decreasing_rad): + y = np.asarray(branch, dtype=float) + if y.shape != x.shape or not np.all(np.isfinite(y)): + raise ValueError("mapping curve must align with its native knots") + d = np.diff(y) + if not (np.all(d >= -1e-9) or np.all(d <= 1e-9)): + raise ValueError("mapping curve must be monotonic") + + @property + def bounds_rad(self) -> tuple[float, float]: + values = self.angle_rad + self.increasing_rad + self.decreasing_rad + return min(values), max(values) + + def evaluate(self, value: float, direction: str = "") -> float: + if direction not in {"", "increasing", "decreasing"}: + raise ValueError("invalid native SDK direction") + if not math.isfinite(value) or not self.knots[0] <= value <= self.knots[-1]: + raise ValueError(f"SDK input outside fitted support:{self.joint}:{value}") + branch = getattr(self, direction + "_rad") if direction else self.angle_rad + if self.input_domain.endswith("_u8"): + value = math.floor(value + 0.5) + return float(np.interp(value, self.knots, branch)) + + +def evaluate_mappings( + mappings: Mapping[str, JointMapping], sdk_values: Sequence[float], + directions: Mapping[str, str], +) -> dict[str, float]: + result = {} + for joint, mapping in mappings.items(): + if joint != mapping.joint or mapping.motor_index >= len(sdk_values): + raise ValueError("mapping/SDK vector contract differs") + result[joint] = mapping.evaluate(float(sdk_values[mapping.motor_index]), directions.get(joint, "")) + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py deleted file mode 100644 index fa00099..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py +++ /dev/null @@ -1,39 +0,0 @@ -"""Normalized records shared by online evaluation and offline replay.""" - -from __future__ import annotations - -from dataclasses import dataclass, field -import math -from typing import Any, Mapping - - -@dataclass(frozen=True) -class SampleRecord: - task_key: str - measurement: str - view: str - cycle: int - direction: str - command_u8: int - timestamp_ns: int - values: Mapping[str, Any] - quality: Mapping[str, float] = field(default_factory=dict) - command: float | None = None - command_unit: str = "u8" - progress_01: float | None = None - - def __post_init__(self) -> None: - if not self.task_key or not self.measurement or not self.view: - raise ValueError("sample task, measurement, and view are required") - if self.cycle < 0: - raise ValueError("sample cycle or command is out of range") - if self.command_unit == "u8" and not 0 <= self.command_u8 <= 255: - raise ValueError("sample cycle or command is out of range") - if self.command_unit not in {"u8", "rad"}: - raise ValueError("sample command unit must be u8 or rad") - if self.command is not None and not math.isfinite(float(self.command)): - raise ValueError("sample command must be finite") - if self.progress_01 is not None and not 0.0 <= float(self.progress_01) <= 1.0: - raise ValueError("sample progress must be in [0, 1]") - if self.timestamp_ns < 0: - raise ValueError("sample timestamp must be non-negative") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py new file mode 100644 index 0000000..2b5cb66 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/status.py @@ -0,0 +1,82 @@ +"""Model-independent progress and failure status contracts.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +from typing import Any, Mapping + + +@dataclass(frozen=True) +class TaskStatus: + key: str = "" + label: str = "" + cycle: int | None = None + cycle_count: int = 4 + direction: str = "" + required_tag_ids_by_view: Mapping[str, tuple[int, ...]] = field( + default_factory=dict + ) + + +@dataclass(frozen=True) +class MotionStatus: + channel: str = "" + command: float | None = None + feedback: float | None = None + unit: str = "" + speed: float | None = None + + +@dataclass(frozen=True) +class AcquisitionStatus: + valid_samples: int = 0 + minimum_samples: int = 40 + coverage_01: float = 0.0 + bins: int = 0 + maximum_gap: int = 0 + retry_count: int = 0 + warnings: tuple[str, ...] = () + + +@dataclass(frozen=True) +class PauseStatus: + code: str = "" + reason: str = "" + suggestion: str = "" + details: Mapping[str, Any] = field(default_factory=dict) + + +@dataclass(frozen=True) +class CalibrationStatus: + """Stable payload rendered by the one common product runner.""" + + schema_version: int = 1 + state: str = "WAIT_DEVICE" + phase: str = "" + overall_progress_01: float = 0.0 + estimated_remaining_seconds: float | None = None + reference_locked: bool = False + reference_message: str = "" + task: TaskStatus = field(default_factory=TaskStatus) + recognized_tag_ids: tuple[int, ...] = () + missing_tag_ids: tuple[int, ...] = () + rejected_tag_ids: tuple[int, ...] = () + motion: MotionStatus = field(default_factory=MotionStatus) + acquisition: AcquisitionStatus = field(default_factory=AcquisitionStatus) + resume: Mapping[str, Any] = field(default_factory=dict) + hardware: Mapping[str, Any] = field(default_factory=dict) + pause: PauseStatus = field(default_factory=PauseStatus) + log_path: str = "" + outputs: Mapping[str, str] = field(default_factory=dict) + + def as_dict(self) -> dict[str, Any]: + return asdict(self) + + +__all__ = [ + "AcquisitionStatus", + "CalibrationStatus", + "MotionStatus", + "PauseStatus", + "TaskStatus", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py index cb2a4bb..2bdcb99 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py @@ -1,5 +1,19 @@ """Pure curve and axis fitting.""" from .curve import FitResult, isotonic_nonincreasing +from .coupling import ( + CouplingFit, LinearMimicFit, PairedJointObservation, RelativeMimicEvidence, + curve_travel_rad, fit_coupling_model, fit_standard_mimic, fit_relative_standard_mimic, +) +from .rotation_curve import RotationCurve, RotationObservation, fit_rotation_curve -__all__ = ["FitResult", "isotonic_nonincreasing"] +__all__ = [ + "CouplingFit", + "FitResult", + "curve_travel_rad", + "fit_coupling_model", + "isotonic_nonincreasing", + "LinearMimicFit", "PairedJointObservation", "RelativeMimicEvidence", + "fit_standard_mimic", "fit_relative_standard_mimic", + "RotationCurve", "RotationObservation", "fit_rotation_curve", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/circle_geometry.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/circle_geometry.py new file mode 100644 index 0000000..a3951b3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/circle_geometry.py @@ -0,0 +1,144 @@ +"""Generic circular statistics and robust image-plane circle fitting.""" +from __future__ import annotations +import math +from typing import Sequence +import numpy as np +from scipy.optimize import least_squares + +def wrap_angle_rad(angle_rad: float) -> float: + """Wrap one finite angle to [-pi, pi).""" + angle = float(angle_rad) + if not math.isfinite(angle): + raise ValueError("angle must be finite") + return (angle + math.pi) % (2.0 * math.pi) - math.pi + + +def signed_angle_difference_rad(angle_rad: float, reference_rad: float) -> float: + """Return the wrapped signed rotation from reference to angle.""" + return wrap_angle_rad(float(angle_rad) - float(reference_rad)) + + +def circular_mean_rad(angles_rad: Sequence[float]) -> float: + """Return the circular mean of at least one finite angle.""" + angles = np.asarray(angles_rad, dtype=float) + if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)): + raise ValueError("angles_rad must contain finite angles") + vector = complex( + float(np.mean(np.cos(angles))), + float(np.mean(np.sin(angles))), + ) + if abs(vector) < 1.0e-12: + raise ValueError("angles do not have a unique circular mean") + return wrap_angle_rad(math.atan2(vector.imag, vector.real)) + + +def circular_median_rad(angles_rad: Sequence[float]) -> float: + """Return a robust circular median around the circular mean.""" + angles = np.asarray(angles_rad, dtype=float) + reference = circular_mean_rad(angles) + deltas = np.asarray( + [signed_angle_difference_rad(value, reference) for value in angles], + dtype=float, + ) + return wrap_angle_rad(reference + float(np.median(deltas))) + + +def circular_std_rad(angles_rad: Sequence[float]) -> float: + """Return circular standard deviation in radians.""" + angles = np.asarray(angles_rad, dtype=float) + if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)): + raise ValueError("angles_rad must contain finite angles") + resultant = float( + math.hypot( + float(np.mean(np.cos(angles))), + float(np.mean(np.sin(angles))), + ) + ) + resultant = min(1.0, max(1.0e-15, resultant)) + return math.sqrt(max(0.0, -2.0 * math.log(resultant))) + + +def maximum_pairwise_angle_difference_rad( + angles_rad: Sequence[float], +) -> float: + """Return the largest wrapped distance between any two angles.""" + angles = [float(value) for value in angles_rad] + if not angles or not all(math.isfinite(value) for value in angles): + raise ValueError("angles_rad must contain finite angles") + return max( + ( + abs(signed_angle_difference_rad(left, right)) + for index, left in enumerate(angles) + for right in angles[index + 1:] + ), + default=0.0, + ) + + +def _fit_circle(points_xy: np.ndarray) -> tuple[np.ndarray, float]: + """Fit a geometric circle with robust nonlinear least squares.""" + points = np.asarray(points_xy, dtype=float) + if points.ndim != 2 or points.shape[1] != 2 or len(points) < 3: + raise ValueError("circle fit requires at least three 2D points") + origin = np.mean(points, axis=0) + centered = points - origin + matrix = np.column_stack( + (2.0 * centered[:, 0], 2.0 * centered[:, 1], np.ones(len(points))) + ) + rhs = np.sum(np.square(centered), axis=1) + solution, _, rank, _ = np.linalg.lstsq(matrix, rhs, rcond=None) + if rank < 3: + raise ValueError("trajectory points are degenerate") + centre0 = solution[:2] + origin + radius0_squared = ( + float(solution[2]) + float(np.dot(solution[:2], solution[:2])) + ) + if radius0_squared <= 0.0: + raise ValueError("trajectory circle radius is invalid") + radius0 = math.sqrt(radius0_squared) + + def residual(parameters: np.ndarray) -> np.ndarray: + centre = parameters[:2] + radius = parameters[2] + return np.linalg.norm(points - centre, axis=1) - radius + + initial_error = residual(np.asarray([*centre0, radius0], dtype=float)) + robust_scale = max( + 0.25, + 1.4826 + * float( + np.median( + np.abs(initial_error - float(np.median(initial_error))) + ) + ), + ) + result = least_squares( + residual, + np.asarray([*centre0, radius0], dtype=float), + loss="soft_l1", + f_scale=robust_scale, + max_nfev=1000, + ) + centre = np.asarray(result.x[:2], dtype=float) + radius = float(abs(result.x[2])) + if not result.success or not np.all(np.isfinite(centre)): + raise ValueError("trajectory circle optimization failed") + if not math.isfinite(radius) or radius <= 0.0: + raise ValueError("trajectory circle radius is invalid") + return centre, radius + + +def _trajectory_arc_rad(points_xy: np.ndarray, centre_xy: np.ndarray) -> float: + angles = np.mod( + np.arctan2( + points_xy[:, 1] - centre_xy[1], + points_xy[:, 0] - centre_xy[0], + ), + 2.0 * math.pi, + ) + if len(angles) < 2: + return 0.0 + angles = np.sort(angles) + gaps = np.diff(np.r_[angles, angles[0] + 2.0 * math.pi]) + return float(2.0 * math.pi - np.max(gaps)) + diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py new file mode 100644 index 0000000..88c8000 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/command_mapping.py @@ -0,0 +1,98 @@ +"""Steady command calibration against independently observed Tag angles. + +Feedback is NOT a command label and is never the target of this regression. +The visual reference/axis was frozen by the continuous training acquisition. +""" + +from dataclasses import replace +import math +import numpy as np + +from ..domain.result import JointMapping +from ..urdf.acceptance import angular_metrics +from .curve import isotonic_nonincreasing +from .spatial import measure_joint_curve_observation + + +def fit_command_mappings(profile, fit, records_by_joint): + mappings, metrics, applicability = {}, {}, {} + for joint in sorted(fit.output_mappings): + donor = profile.zero.transferred_zero_sources.get(joint, joint) + rows = records_by_joint.get(donor, ()) + if not rows or any(r.get("sample_phase") != "steady" for r in rows): + raise ValueError(f"steady_command_observations_missing:{joint}") + curve = fit.curves[donor] + gauge = float(np.interp(fit.reference_inputs[donor], curve.circle["input_knots"], curve.angle_rad)) + # Byte curves already carry their shared coordinate reference. Native + # curves retain the arbitrary observed Tag reference in fit.curves. + if profile.command.unit == "u8": + gauge = 0.0 + observed = {r["sample_id"]: measure_joint_curve_observation(curve, + quaternion_xyzw=r.get("relative_quaternion_xyzw"), + image_relative_xy_px=r.get("image_relative_xy_px"))-gauge for r in rows} + if len(observed) != len(rows): + raise ValueError("duplicate steady visual image") + channel = fit.output_mappings[joint].motor_index + source_channel = fit.output_mappings[donor].motor_index + branches = {} + task = next(t for t in profile.motion.tasks if donor in t.joints) + lo, hi = sorted((task.start_value, task.end_value)) + train_knots = np.linspace(lo, hi, profile.acquisition.steady_training_nodes) + if profile.command.unit == "u8": + train_knots = np.floor(train_knots+0.5) + knots = tuple(sorted(set(float(v) for v in train_knots))) + sign = profile.command.joint_directions[source_channel] + for direction in ("increasing", "decreasing"): + values = [] + for knot in knots: + medians = [] + for cycle in (0, 1, 2): + selected = [r for r in rows if r["cycle"] == cycle and r["direction"] == direction + and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9)] + if len(selected) < profile.acquisition.steady_minimum_samples: + raise ValueError(f"steady_training_incomplete:{joint}:{cycle}:{direction}:{knot}") + for r in selected: + if not math.isclose(float(r[f"command_{profile.command.unit}"]), knot, abs_tol=1e-9): + raise ValueError("steady observation captured during command motion") + medians.append(float(np.median([observed[r["sample_id"]] for r in selected]))) + values.append(float(np.median(medians))) + values = np.asarray(values) + projected = -sign*isotonic_nonincreasing(-sign*values) + if np.max(np.abs(projected-values)) > math.radians(1): + raise ValueError(f"steady_command_not_monotonic:{joint}:{direction}") + branches[direction] = tuple(float(v) for v in projected) + mean = tuple((a+b)/2 for a, b in zip(branches["increasing"], branches["decreasing"])) + mapping = JointMapping(joint, channel, f"command_{profile.command.unit}", knots, + mean, branches["increasing"], branches["decreasing"], donor if donor != joint else None) + validation = [r for r in rows if r["cycle"] == 3] + expected = np.r_[lo, (np.asarray(knots[:-1])+knots[1:])/2, hi] + if profile.command.unit == "u8": + # Acquisition interleaves the unrounded grid, then encodes bytes. + grid = np.linspace(lo, hi, profile.acquisition.steady_training_nodes) + expected = np.floor(np.r_[lo, (grid[:-1]+grid[1:])/2, hi]+0.5) + for direction in ("increasing", "decreasing"): + for knot in set(expected): + if sum(r["direction"] == direction and math.isclose(float(r["steady_target"]), knot, abs_tol=1e-9) + for r in validation) < profile.acquisition.steady_minimum_samples: + raise ValueError(f"steady_holdout_incomplete:{joint}:{direction}:{knot}") + errors = [mapping.evaluate(float(r[f"command_{profile.command.unit}"]), r["direction"]) + - observed[r["sample_id"]] for r in validation] + quality = angular_metrics(errors) + if not quality.passed: + raise ValueError(f"steady_command_holdout_failed:{joint}:{quality}") + mappings[joint] = mapping + from dataclasses import asdict + metrics[joint] = {**asdict(quality), "independently_measured": donor == joint, + "source_joint": donor, "training_cycles": [0, 1, 2], "holdout_cycle": 3} + training = [r for r in rows if r["cycle"] in {0, 1, 2}] + vectors = np.asarray([r[f"command_vector_{profile.command.unit}"] for r in training]) + held = {str(i): float(np.median(vectors[:, i])) for i in range(profile.command.command_count) + if i != source_channel} + for i, value in held.items(): + if np.max(np.abs(vectors[:, int(i)]-value)) > 1e-8: + raise ValueError(f"scan_changed_multiple_command_channels:{donor}:{i}") + applicability[joint] = {"task": task.key, "held_command_values": held, + "scope": "single_channel_at_declared_pose", "arbitrary_multiaxis_validated": False, + "branch_initialization": "full_range_endpoint_reset"} + return replace(fit, command_mappings=mappings, command_holdout_metrics=metrics, + command_applicability=applicability) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py new file mode 100644 index 0000000..ea1816f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/coupling.py @@ -0,0 +1,347 @@ +"""Model-independent active/passive joint coupling fitting.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Protocol, Sequence + +import numpy as np +from scipy.optimize import least_squares + + +class DirectionalCurve(Protocol): + decreasing_rad: Sequence[float] + increasing_rad: Sequence[float] + + +@dataclass(frozen=True) +class LinearMimicFit: + """The one relationship that a standard URDF can actually serialize.""" + + source_joint: str + target_joint: str + multiplier: float + offset_rad: float + offset_observed: bool + training_cycles: tuple[int, ...] + residuals_rad: tuple[float, ...] + + +@dataclass(frozen=True) +class PairedJointObservation: + """Two independent visual angles from the same image and frozen mounts.""" + + sample_id: str + cycle: int + direction: str + source_rad: float + target_rad: float + + +@dataclass(frozen=True) +class RelativeMimicEvidence: + fit: LinearMimicFit + # Nuisance phase between the two frozen visual references, NOT a URDF zero. + installation_phase_rad: float + training_sample_ids: tuple[str, ...] + holdout_sample_ids: tuple[str, ...] + holdout_errors_rad: tuple[float, ...] + + +def fit_relative_standard_mimic( + source_joint: str, target_joint: str, + observations: Sequence[PairedJointObservation], *, cad_offset_rad: float, +) -> RelativeMimicEvidence: + """Fit a standard linear mimic from paired visual motion, not SDK priors. + + Unknown mounting angles make the absolute passive intercept unobservable. + A training-only nuisance phase aligns the *visual references*, while the + published CAD intercept is preserved. It cannot be re-estimated on holdout + or used as an origin/mimic correction. Both directions share one slope. + """ + rows = tuple(observations) + identities = [row.sample_id for row in rows] + if any(not value for value in identities) or len(set(identities)) != len(identities): + raise ValueError("paired mimic image identities are missing or duplicated") + if any(row.cycle not in (0, 1, 2, 3) or row.direction not in {"increasing", "decreasing"} + or not math.isfinite(row.source_rad) or not math.isfinite(row.target_rad) + for row in rows): + raise ValueError("invalid paired mimic observation") + for cycle in range(4): + for direction in ("increasing", "decreasing"): + if sum(row.cycle == cycle and row.direction == direction for row in rows) < 40: + raise ValueError(f"missing paired mimic observations: {target_joint}:{cycle}:{direction}") + training = tuple(row for row in rows if row.cycle != 3) + holdout = tuple(row for row in rows if row.cycle == 3) + relative_fit = fit_standard_mimic(source_joint, target_joint, + [row.source_rad for row in training], [row.target_rad for row in training], + cycles=[row.cycle for row in training], offset_rad=None, offset_observable=True) + errors = tuple(relative_fit.multiplier * row.source_rad + relative_fit.offset_rad - row.target_rad + for row in holdout) + # Final-file FK remains a separate mandatory gate; this is the prerequisite + # that a single standard mimic can even explain the independent motion. + for label, residuals in (("training", relative_fit.residuals_rad), ("holdout", errors)): + degrees = np.rad2deg(np.abs(residuals)) + if np.mean(degrees) > 1 or np.percentile(degrees, 95) > 2 or np.max(degrees) > 3: + raise ValueError( + f"standard_urdf_mimic_not_expressive:joint={target_joint}:phase={label}:" + f"mae_deg={np.mean(degrees):.6f}:p95_deg={np.percentile(degrees, 95):.6f}:" + f"maximum_deg={np.max(degrees):.6f}; 标准 URDF 线性 mimic 表达能力不足或观测不一致" + ) + if not math.isfinite(cad_offset_rad): + raise ValueError("passive CAD offset must be finite") + fit = LinearMimicFit(source_joint, target_joint, relative_fit.multiplier, + float(cad_offset_rad), False, (0, 1, 2), relative_fit.residuals_rad) + return RelativeMimicEvidence(fit, relative_fit.offset_rad, + tuple(row.sample_id for row in training), tuple(row.sample_id for row in holdout), errors) + + +def fit_standard_mimic( + source_joint: str, + target_joint: str, + active_rad: Sequence[float], + passive_rad: Sequence[float], + *, + cycles: Sequence[int], + offset_rad: float | None, + offset_observable: bool = False, +) -> LinearMimicFit: + """Fit actual synchronous observations, never a vendor-generated curve. + + A static offset is fixed to the CAD/coordinate-conversion value unless an + independent datum makes it observable. Unknown tag mounting cannot justify + fitting an intercept. Numerical fitting is training-only; the serialized + URDF and independent fourth cycle decide acceptance afterwards. + """ + x, y = np.asarray(active_rad, dtype=float), np.asarray(passive_rad, dtype=float) + identifiers = np.asarray(cycles) + if x.ndim != 1 or x.shape != y.shape or x.shape != identifiers.shape or x.size < 40: + raise ValueError("mimic fitting requires at least 40 aligned observations") + if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)): + raise ValueError("mimic observations must be finite") + if not np.all(np.isin(identifiers, (0, 1, 2))) or set(identifiers.tolist()) != {0, 1, 2}: + raise ValueError("mimic fitting requires three training cycles; holdout is forbidden") + if not source_joint or not target_joint or source_joint == target_joint: + raise ValueError("mimic source and target must be distinct") + if float(np.ptp(x)) <= math.radians(1.0): + raise ValueError("mimic source travel is not observable") + if offset_rad is None: + if not offset_observable: + raise ValueError("unobservable passive static offset must retain CAD") + design, rhs = np.column_stack((x, np.ones_like(x))), y + else: + if not math.isfinite(float(offset_rad)): + raise ValueError("fixed mimic offset must be finite") + design, rhs = x[:, None], y - float(offset_rad) + initial, _, rank, _ = np.linalg.lstsq(design, rhs, rcond=None) + if rank != design.shape[1]: + raise ValueError("mimic parameters are not independently observable") + residual = rhs - design @ initial + scale = max(math.radians(0.1), 1.4826 * float(np.median(np.abs(residual - np.median(residual))))) + solution = least_squares(lambda parameters: design @ parameters - rhs, + initial, loss="soft_l1", f_scale=scale) + if not solution.success or not np.all(np.isfinite(solution.x)): + raise ValueError("linear mimic fit failed") + multiplier = float(solution.x[0]) + offset = float(solution.x[1]) if offset_rad is None else float(offset_rad) + return LinearMimicFit(source_joint, target_joint, multiplier, offset, + offset_rad is None, (0, 1, 2), tuple(float(v) for v in (multiplier*x + offset - y))) + + +@dataclass(frozen=True) +class CouplingFit: + source_joint: str + target_joint: str + model: str + coefficients: tuple[float, ...] + urdf_mimic_multiplier: float + urdf_mimic_policy: str + cycle_coefficients: tuple[tuple[float, ...], ...] + maximum_cycle_range: float + maximum_cycle_prediction_range_rad: float + residual_rms_rad: float + residual_p95_rad: float + residual_max_rad: float + + @property + def multiplier(self) -> float: + return float(self.coefficients[0]) + + @property + def cycle_multipliers(self) -> tuple[float, ...]: + return tuple(float(values[0]) for values in self.cycle_coefficients) + + @property + def mujoco_polycoef(self) -> tuple[float, ...]: + return (0.0, *self.coefficients, *(0.0,) * (5 - len(self.coefficients))) + + +def curve_travel_rad(fit: DirectionalCurve) -> float: + """Return the mean travel of the decreasing and increasing curves.""" + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + if decreasing.size < 2 or increasing.size < 2: + raise ValueError("directional curves require at least two knots") + travel = 0.5 * ( + float(decreasing[0] - decreasing[-1]) + + float(increasing[0] - increasing[-1]) + ) + if not math.isfinite(travel) or travel <= 0.0: + raise ValueError("fitted travel must be finite and positive") + return travel + + +def _coupling_regression( + active: Sequence[float], passive: Sequence[float], *, degree: int +) -> tuple[tuple[float, ...], np.ndarray]: + x = np.asarray(active, dtype=float) + y = np.asarray(passive, dtype=float) + if x.shape != y.shape or x.ndim != 1 or x.size < 16: + raise ValueError("coupling curves must be aligned finite vectors") + if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)): + raise ValueError("coupling curves must be finite") + if degree not in {1, 2}: + raise ValueError("coupling degree must be one or two") + if float(x @ x) <= 1.0e-9: + raise ValueError("active coupling source has insufficient travel") + design = np.column_stack([x ** power for power in range(1, degree + 1)]) + initial = np.linalg.lstsq(design, y, rcond=None)[0] + scale = max( + math.radians(0.25), + float(np.median(np.abs(y - design @ initial))), + ) + fitted = least_squares( + lambda value: y - design @ value, + np.asarray(initial, dtype=float), + loss="soft_l1", + f_scale=scale, + ) + if not fitted.success or not np.all(np.isfinite(fitted.x)): + raise ValueError("robust through-origin coupling regression failed") + coefficients = tuple(float(value) for value in fitted.x) + grid = np.linspace(0.0, float(np.max(x)), 256) + derivative = np.full_like(grid, coefficients[0]) + if degree == 2: + derivative += 2.0 * coefficients[1] * grid + if float(np.min(derivative)) < -1.0e-7: + raise ValueError("coupling model is not monotonic") + return coefficients, y - design @ fitted.x + + +def fit_coupling_model( + source_joint: str, + target_joint: str, + active_fit: DirectionalCurve, + passive_fit: DirectionalCurve, + *, + model: str, + cycle_curve_pairs: Sequence[ + tuple[Sequence[float], Sequence[float]] + ] = (), + minimum_multiplier: float = 0.5, + maximum_multiplier: float = 1.5, + maximum_cycle_range: float = 0.03, + maximum_cycle_prediction_range_rad: float = math.radians(1.0), + maximum_residual_p95_rad: float = math.radians(2.0), + maximum_residual_rad: float = math.radians(3.0), +) -> CouplingFit: + """Fit a profile-selected coupling primitive with release-grade metrics.""" + supported = {"linear_mimic", "quadratic_runtime", "direction_aware_knots"} + if model not in supported: + raise ValueError(f"unsupported coupling model: {model}") + source_travel = curve_travel_rad(active_fit) + target_travel = curve_travel_rad(passive_fit) + endpoint_multiplier = float(target_travel / source_travel) + if model == "direction_aware_knots": + if not minimum_multiplier <= endpoint_multiplier <= maximum_multiplier: + raise ValueError( + f"{target_joint} coupling endpoint ratio is outside " + f"[{minimum_multiplier}, {maximum_multiplier}]" + ) + return CouplingFit( + source_joint, target_joint, model, (endpoint_multiplier,), + endpoint_multiplier, "endpoint_linear_fallback", (), + 0.0, 0.0, 0.0, 0.0, 0.0, + ) + + degree = 1 if model == "linear_mimic" else 2 + active = np.concatenate(( + np.asarray(active_fit.decreasing_rad, dtype=float), + np.asarray(active_fit.increasing_rad, dtype=float), + )) + passive = np.concatenate(( + np.asarray(passive_fit.decreasing_rad, dtype=float), + np.asarray(passive_fit.increasing_rad, dtype=float), + )) + coefficients, residual = _coupling_regression(active, passive, degree=degree) + multiplier = coefficients[0] + if not minimum_multiplier <= multiplier <= maximum_multiplier: + raise ValueError( + f"{target_joint} coupling linear term is outside " + f"[{minimum_multiplier}, {maximum_multiplier}]" + ) + cycle_coefficients = tuple( + _coupling_regression(source, target, degree=degree)[0] + for source, target in cycle_curve_pairs + ) + cycle_multipliers = tuple(values[0] for values in cycle_coefficients) + cycle_range = ( + 0.0 if len(cycle_multipliers) < 2 + else float(max(cycle_multipliers) - min(cycle_multipliers)) + ) + if cycle_range > maximum_cycle_range: + raise ValueError( + f"{target_joint} coupling linear-term cycle range exceeds " + f"{maximum_cycle_range}" + ) + cycle_prediction_range = 0.0 + if len(cycle_coefficients) >= 2: + grid = np.linspace(0.0, float(np.max(active)), 256) + predictions = np.asarray([ + sum(value * grid ** (index + 1) for index, value in enumerate(values)) + for values in cycle_coefficients + ]) + cycle_prediction_range = float(np.max(np.ptp(predictions, axis=0))) + if cycle_prediction_range > maximum_cycle_prediction_range_rad: + raise ValueError( + f"{target_joint} coupling cycle prediction range exceeds limit" + ) + absolute = np.abs(residual) + rms = float(np.sqrt(np.mean(np.square(residual)))) + p95 = float(np.percentile(absolute, 95.0)) + maximum = float(np.max(absolute)) + if p95 > maximum_residual_p95_rad or maximum > maximum_residual_rad: + raise ValueError( + "coupling_residual_exceeds:" + f"joint={target_joint}:model={model}:" + f"linear_term={multiplier:.6f}:" + f"p95_deg={math.degrees(p95):.3f}:" + f"maximum_deg={math.degrees(maximum):.3f}:" + f"p95_limit_deg={math.degrees(maximum_residual_p95_rad):.3f}:" + f"maximum_limit_deg={math.degrees(maximum_residual_rad):.3f}" + ) + return CouplingFit( + source_joint=source_joint, + target_joint=target_joint, + model=model, + coefficients=coefficients, + urdf_mimic_multiplier=( + multiplier if model == "linear_mimic" else endpoint_multiplier + ), + urdf_mimic_policy=( + "exact_linear" if model == "linear_mimic" + else "endpoint_linear_fallback" + ), + cycle_coefficients=cycle_coefficients, + maximum_cycle_range=cycle_range, + maximum_cycle_prediction_range_rad=cycle_prediction_range, + residual_rms_rad=rms, + residual_p95_rad=p95, + residual_max_rad=maximum, + ) + + +__all__ = ["CouplingFit", "DirectionalCurve", "curve_travel_rad", "fit_coupling_model", + "LinearMimicFit", "fit_standard_mimic", "PairedJointObservation", + "RelativeMimicEvidence", "fit_relative_standard_mimic"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/image_curve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/image_curve.py new file mode 100644 index 0000000..3175618 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/image_curve.py @@ -0,0 +1,74 @@ +"""Training-frozen angular phase of an observed image-plane circle. + +Only use this primitive where the Profile declares the projected circle +observable. Final standard-URDF SE(3) replay remains mandatory: a good image +circle alone cannot certify physical angles in an oblique projection. +""" + +import math + +import numpy as np + +from ..domain.measurement import JointCurveFit +from .circle_geometry import _fit_circle, _trajectory_arc_rad +from .trajectory_geometry import _fit_joint_curve + + +def image_phase(point, circle): + point, centre, reference = (np.asarray(value, dtype=float) for value in + (point, circle["center_xy_px"], circle["reference_xy_px"])) + vector = point - centre + if point.shape != (2,) or not np.all(np.isfinite(point)) or np.linalg.norm(vector) < 1e-9: + raise ValueError("invalid image trajectory observation") + return float(circle["orientation_sign"]) * math.atan2( + float(reference[0]*vector[1] - reference[1]*vector[0]), float(reference @ vector)) + + +def fit_joint_image_curve(records, *, zero_command_u8=255, + canonical_zero_direction="decreasing", maximum_radial_rms_px=2., + maximum_radial_p95_px=3.5, minimum_radius_px=20., minimum_arc_rad=math.radians(15)): + samples = list(records) + if not samples or any(int(row["cycle"]) not in {0, 1, 2} for row in samples): + raise ValueError("image curve requires training observations only") + points = np.asarray([row["image_relative_xy_px"] for row in samples], dtype=float) + if points.shape != (len(samples), 2) or not np.all(np.isfinite(points)): + raise ValueError("image curve requires finite two-dimensional observations") + centre, radius = _fit_circle(points) + radial = np.linalg.norm(points-centre, axis=1)-radius + quality = {"radial_rms_px": float(np.sqrt(np.mean(radial**2))), + "radial_p95_px": float(np.percentile(np.abs(radial), 95)), + "radius_px": radius, "arc_rad": float(_trajectory_arc_rad(points, centre))} + if (quality["radial_rms_px"] > maximum_radial_rms_px + or quality["radial_p95_px"] > maximum_radial_p95_px + or radius < minimum_radius_px or quality["arc_rad"] < minimum_arc_rad): + raise ValueError("image_trajectory_not_observable") + # One observed reference for all training directions/cycles. Choose it + # near nominal baseline, then interpolate a single coordinate gauge. + indices = [index for index, row in enumerate(samples) + if canonical_zero_direction is None or row["direction"] == canonical_zero_direction] + if not indices: + raise ValueError("canonical image direction is missing") + index = min(indices, key=lambda i: abs(int(samples[i]["command_u8"])-zero_command_u8)) + reference = points[index]-centre + reference /= np.linalg.norm(reference) + circle = {"space": "image_2d", "center_xy_px": centre.tolist(), + "reference_xy_px": reference.tolist(), "orientation_sign": 1., "radius_px": radius, + "training_cycles": sorted({int(row["cycle"]) for row in samples}), + "input_domain": "feedback_u8", "input_knots": list(range(256)), + "zero_command_u8": int(zero_command_u8)} + values = np.asarray([image_phase(point, circle) for point in points]) + commands = np.asarray([int(row["command_u8"]) for row in samples]) + trend = float((commands-np.mean(commands)) @ (values-np.mean(values))) + if abs(trend) < 1e-12: + raise ValueError("image motion direction is not observable") + if trend > 0: + values = -values + circle["orientation_sign"] = -1. + curves, correction, hysteresis = _fit_joint_curve(samples, values, preserve_direction_offset=True) + reference_angle = float(curves["angle_rad" if canonical_zero_direction is None else canonical_zero_direction+"_rad"][int(zero_command_u8)]) + turn = reference_angle / circle["orientation_sign"] + rotation = np.array([[math.cos(turn), -math.sin(turn)], [math.sin(turn), math.cos(turn)]]) + circle["reference_xy_px"] = (rotation @ reference).tolist() + curves = {name: tuple(float(value-reference_angle) for value in values) for name, values in curves.items()} + return JointCurveFit(curves["angle_rad"], curves["decreasing_rad"], curves["increasing_rad"], + circle, correction, hysteresis, quality) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py new file mode 100644 index 0000000..77213e8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/motion_fit.py @@ -0,0 +1,121 @@ +"""Profile-selected native measurement primitives and coordinate gauges. + +The curve reference is only a coordinate choice. CAD zero is solved later +from independently observed spatial axes; neither SDK zero nor an endpoint +is treated as a mechanical datum. +""" + +from dataclasses import dataclass, replace +from typing import Mapping + +import numpy as np + +from ..domain.measurement import JointCurveFit +from ..urdf.acceptance import angular_metrics +from .coupling import fit_relative_standard_mimic +from .observed_motion import ( + compare_native_views, fit_byte_observed_motion, fit_native_observed_rotation, + paired_visual_observations, +) + + +@dataclass(frozen=True) +class MotionFit: + observed_curves: Mapping[str, JointCurveFit] + coordinate_curves: Mapping[str, JointCurveFit] + holdout_errors_rad: Mapping[str, tuple[float, ...]] + mimic_evidence: Mapping + cross_view_metrics: Mapping + reference_inputs: Mapping[str, float] + + +def channel_for_joint(profile, joint): + seen = set() + while joint not in profile.command.command_index_by_joint: + if joint in seen or joint not in profile.zero.mimic_source_by_joint: + raise ValueError(f"measured joint has no SDK source:{joint}") + seen.add(joint) + joint = profile.zero.mimic_source_by_joint[joint] + return profile.command.command_index_by_joint[joint] + + +def joint_input_direction(profile, source_model, joint): + """A passive measurement follows its declared CAD mimic chain's sign.""" + sign, current, seen = 1, joint, set() + while current not in profile.command.command_index_by_joint: + if current in seen: + raise ValueError("cyclic mimic input binding") + seen.add(current) + cad = source_model.joints[current] + if cad.mimic_joint != profile.zero.mimic_source_by_joint.get(current) or cad.mimic_multiplier == 0: + raise ValueError(f"invalid measured mimic input binding:{current}") + sign *= 1 if cad.mimic_multiplier > 0 else -1 + current = cad.mimic_joint + return sign * profile.command.joint_directions[profile.command.command_index_by_joint[current]] + + +def native_coordinate_curve(curve, baseline): + """Choose one training-supported gauge, never extend a fitted domain. + + Both directional branches subtract the SAME reference, retaining actual + backlash. If nominal baseline lies outside observed support, the nearest + measured input is the reference and is explicitly recorded as such. + """ + inputs = curve.circle["input_knots"] + reference_input = float(np.clip(baseline, inputs[0], inputs[-1])) + reference_angle = float(np.interp(reference_input, inputs, curve.angle_rad)) + branches = {name: tuple(float(value - reference_angle) for value in getattr(curve, name)) + for name in ("angle_rad", "increasing_rad", "decreasing_rad")} + return replace(curve, **branches, circle={**curve.circle, + "coordinate_reference_input": reference_input, + "coordinate_reference_angle_rad": reference_angle}), reference_input + + +def fit_profile_motion(profile, source_model, records_by_joint, *, cross_view_records=None): + expected = {name for name, spec in profile.measurement.measurements.items() + if spec.view is not None and spec.kind != "axis_cross_view_validation"} + if set(records_by_joint) != expected: + raise ValueError("fitting observations differ from the Profile") + if any(profile.measurement.measurements[name].kind not in {"relative_rotation", "urdf_axis_chain", "curve"} for name in expected): + raise ValueError("measurement primitive is not implemented in this pipeline") + mimic_sources = {name: profile.zero.mimic_source_by_joint[name] for name in profile.zero.fitted_mimic_joints} + if profile.command.unit == "u8": + observed, holdout, evidence = fit_byte_observed_motion(records_by_joint, + source_model=source_model, mimic_sources=mimic_sources, + baseline_by_joint={name: profile.command.baseline_values[channel_for_joint(profile, name)] for name in expected}, + direction_by_joint={name: joint_input_direction(profile, source_model, name) for name in expected}, + image_curve_joints=profile.measurement.image_curve_joints, input_domain=profile.curve_input_domain) + coordinates = observed + references = {name: float(profile.command.baseline_values[channel_for_joint(profile, name)]) + for name in observed} + else: + observed, coordinates, holdout, evidence, references = {}, {}, {}, {}, {} + for name, rows in sorted(records_by_joint.items()): + curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain, + input_to_joint_direction=joint_input_direction(profile, source_model, name)) + observed[name], holdout[name] = curve, errors + coordinates[name], references[name] = native_coordinate_curve(curve, + profile.command.baseline_values[channel_for_joint(profile, name)]) + for target, source in mimic_sources.items(): + cad = source_model.joints[target] + if cad.mimic_joint != source: + raise ValueError(f"mimic source differs from CAD:{target}") + evidence[target] = fit_relative_standard_mimic(source, target, + paired_visual_observations(records_by_joint[source], records_by_joint[target], + observed[source], observed[target]), cad_offset_rad=cad.mimic_offset) + secondary = cross_view_records or {} + if set(secondary) != set(profile.measurement.cross_view_sources): + raise ValueError("required cross-view observations are incomplete") + metrics = {} + for name, rows in secondary.items(): + curve, errors = fit_native_observed_rotation(rows, input_domain=profile.curve_input_domain, + input_to_joint_direction=joint_input_direction(profile, source_model, name)) + if "input_knots" not in observed[name].circle: + raise ValueError("cross-view comparison requires native input knots") + comparison = compare_native_views(observed[name], curve) + if comparison.get("direction_disagrees", False): + raise ValueError(f"cross-view motion direction disagrees:{name}") + quality = angular_metrics(errors) + metrics[name] = {**comparison, "holdout_mae_deg": quality.mae_deg, + "holdout_p95_deg": quality.p95_deg, "holdout_max_deg": quality.maximum_deg} + return MotionFit(observed, coordinates, holdout, evidence, metrics, references) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py new file mode 100644 index 0000000..0e25ddf --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/observed_motion.py @@ -0,0 +1,151 @@ +"""Shared visual motion fitting and independently paired standard mimics.""" + +from __future__ import annotations + +from typing import Any, Mapping + +from .coupling import PairedJointObservation, fit_relative_standard_mimic +from .spatial import fit_rotation_joint_curve, measure_joint_curve_observation, joint_curve_holdout_errors +from .image_curve import fit_joint_image_curve +from ..urdf.acceptance import angular_metrics +from ..domain.measurement import JointCurveFit +from .rotation_curve import RotationObservation, fit_rotation_curve + + +def image_identity(row: Mapping[str, Any]) -> str: + identity = str(row.get("sample_id", "")) + if not identity and row.get("view") and row.get("image_stamp_ns") is not None: + identity = f"{row['view']}:{int(row['image_stamp_ns'])}" + if not identity: + raise ValueError("visual fitting requires recorded image identity") + return identity + + +def paired_visual_observations(source_rows, target_rows, source_fit, target_fit): + def indexed(rows): + output = {} + for row in rows: + identity = image_identity(row) + if identity in output: + raise ValueError("duplicated paired visual image identity") + output[identity] = row + return output + source, target = indexed(source_rows), indexed(target_rows) + output = [] + for identity in sorted(source.keys() & target.keys()): + a, b = source[identity], target[identity] + if (a["cycle"], a["direction"]) != (b["cycle"], b["direction"]): + raise ValueError("paired visual image has different cycle/direction") + output.append(PairedJointObservation(identity, int(a["cycle"]), str(a["direction"]), + measure_joint_curve_observation(source_fit, quaternion_xyzw=a.get("relative_quaternion_xyzw"), + image_relative_xy_px=a.get("image_relative_xy_px")), + measure_joint_curve_observation(target_fit, quaternion_xyzw=b.get("relative_quaternion_xyzw"), + image_relative_xy_px=b.get("image_relative_xy_px")))) + return output + + +def fit_native_observed_rotation(rows, *, input_domain, input_to_joint_direction): + """Native-coordinate 3+1 hinge primitive with the shared geometric result. + + The metadata carries real knots, not a synthetic byte command domain. + Scalar angle zero is only a fixed Tag reference, never a CAD datum. + """ + import numpy as np + observations = [RotationObservation(image_identity(row), float(row[input_domain]), + tuple(row["relative_quaternion_xyzw"]), int(row["cycle"]), str(row["direction"])) + for row in rows] + for direction in ("increasing", "decreasing"): + if sum(row.cycle == 3 and row.direction == direction for row in observations) < 40: + raise ValueError("holdout requires 40 images in each direction") + fitted = fit_rotation_curve([row for row in observations if row.cycle != 3], + input_domain=input_domain, sdk_to_joint_direction=input_to_joint_direction) + errors = fitted.holdout_errors([row for row in observations if row.cycle == 3]) + metrics = angular_metrics(errors) + if not metrics.passed: + raise ValueError("independent_rotation_holdout_failed") + hysteresis = float(np.max(np.abs(np.asarray(fitted.increasing_rad) - fitted.decreasing_rad))) + curve = JointCurveFit(fitted.angle_rad, fitted.decreasing_rad, fitted.increasing_rad, + {"space": "relative_rotation_3d", "input_domain": input_domain, "input_knots": fitted.knots, + "reference_quaternion_xyzw": fitted.reference_xyzw, "axis_xyz": fitted.axis_xyz, + "training_cycles": [0, 1, 2], "training_sample_ids": sorted(fitted.training_sample_ids)}, + fitted.maximum_monotonic_correction_rad, hysteresis, {"sample_count": float(len(observations)), + "holdout_mae_deg": metrics.mae_deg, "holdout_p95_deg": metrics.p95_deg, + "holdout_max_deg": metrics.maximum_deg}) + return curve, errors + + +def compare_native_views(primary, secondary): + """Diagnostic comparison in their common physical input interval. + + Neither curve is replaced or recalibrated from the other view. Absolute + mount phase differs between Tags, so centre each frozen branch for this + diagnostic alone; final file FK validates the actual fixed mounts. + """ + import numpy as np + a, b = primary.circle["input_knots"], secondary.circle["input_knots"] + lower, upper = max(a[0], b[0]), min(a[-1], b[-1]) + if upper <= lower: + raise ValueError("cross-view native input supports do not overlap") + inputs = np.linspace(lower, upper, 65) + left = np.interp(inputs, a, primary.angle_rad) + right = np.interp(inputs, b, secondary.angle_rad) + if (left[-1] - left[0]) * (right[-1] - right[0]) <= 0: + return {"direction_disagrees": 1.0} + result = {"travel_difference_rad": abs(float(np.ptp(left) - np.ptp(right)))} + for field in ("angle_rad", "increasing_rad", "decreasing_rad"): + x = np.interp(inputs, a, getattr(primary, field)) + y = np.interp(inputs, b, getattr(secondary, field)) + delta = (x - np.mean(x)) - (y - np.mean(y)) + result[f"{field}_rms_difference_rad"] = float(np.sqrt(np.mean(delta ** 2))) + return result + + +def fit_byte_observed_motion(records_by_joint, *, source_model, mimic_sources, + baseline_by_joint=None, image_curve_joints=(), input_domain="feedback_u8", direction_by_joint=None): + """3+1 frozen-reference byte-domain hinge fit; no per-cycle re-zeroing.""" + curves, errors = {}, {} + for joint, rows in sorted(records_by_joint.items()): + # The retained byte primitive calls its abscissa command_u8. Adapt a + # PRIVATE numerical input, never overwrite the captured command. + rows = [dict(row, command_u8=int(round(float(row[input_domain])))) for row in rows] + ids = [image_identity(row) for row in rows] + if len(set(ids)) != len(ids): + raise ValueError(f"duplicate visual fitting image:{joint}") + for cycle in range(4): + for direction in ("increasing", "decreasing"): + if sum(int(row["cycle"]) == cycle and row["direction"] == direction for row in rows) < 40: + raise ValueError(f"missing bidirectional visual samples:{joint}:{cycle}:{direction}") + training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}] + holdout = [row for row in rows if int(row["cycle"]) == 3] + baseline = int((baseline_by_joint or {}).get(joint, 255)) + primitive = fit_joint_image_curve if joint in image_curve_joints else fit_rotation_joint_curve + curve = primitive(training, zero_command_u8=baseline, canonical_zero_direction=None) + from dataclasses import replace + sign = int((direction_by_joint or {}).get(joint, -1)) + if sign not in {-1, 1}: + raise ValueError("byte SDK direction must be explicitly signed") + if sign == 1: + circle = dict(curve.circle) + if circle["space"] == "image_2d": + circle["orientation_sign"] = -float(circle["orientation_sign"]) + else: + circle["axis_xyz"] = [-float(v) for v in circle["axis_xyz"]] + # The legacy primitive chooses an arbitrary axis orientation with + # a negative input slope. Reorient BOTH the observation axis and + # every branch to the declared SDK direction, never just the table. + curve = replace(curve, circle=circle, **{field: tuple(-v for v in getattr(curve, field)) + for field in ("angle_rad", "increasing_rad", "decreasing_rad")}) + curve = replace(curve, circle={**curve.circle, "input_domain": input_domain, "input_knots": tuple(range(256))}) + residuals = joint_curve_holdout_errors(curve, holdout, zero_command_u8=baseline) + if not angular_metrics(residuals).passed: + raise ValueError(f"independent_rotation_holdout_failed:{joint}") + curves[joint], errors[joint] = curve, residuals + evidence = {} + for target, source in mimic_sources.items(): + cad = source_model.joints[target] + if cad.mimic_joint != source: + raise ValueError(f"mimic source differs from CAD:{target}") + evidence[target] = fit_relative_standard_mimic(source, target, + paired_visual_observations(records_by_joint[source], records_by_joint[target], curves[source], curves[target]), + cad_offset_rad=cad.mimic_offset) + return curves, errors, evidence diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py new file mode 100644 index 0000000..621a2e3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/rotation_curve.py @@ -0,0 +1,159 @@ +"""Native-domain hinge measurements with a single training-frozen Tag frame. + +This primitive measures *relative* motion. Its arbitrary reference angle is +not a CAD zero; spatial calibration must supply that separate datum. Neither +holdout observations nor SDK endpoints are allowed to set that datum. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from .curve import isotonic_nonincreasing + + +@dataclass(frozen=True) +class RotationObservation: + sample_id: str + input_value: float + quaternion_xyzw: tuple[float, float, float, float] + cycle: int + direction: str + + +@dataclass(frozen=True) +class RotationCurve: + input_domain: str + knots: tuple[float, ...] + increasing_rad: tuple[float, ...] + decreasing_rad: tuple[float, ...] + reference_xyzw: tuple[float, float, float, float] + axis_xyz: tuple[float, float, float] + sdk_to_joint_direction: int + training_sample_ids: frozenset[str] + maximum_monotonic_correction_rad: float = 0.0 + + @property + def angle_rad(self) -> tuple[float, ...]: + return tuple((a + b) / 2 for a, b in zip(self.increasing_rad, self.decreasing_rad)) + + def predict(self, value: float, direction: str) -> float: + if direction not in {"increasing", "decreasing"}: + raise ValueError("curve direction must be a native SDK direction") + if not math.isfinite(value) or not self.knots[0] <= value <= self.knots[-1]: + raise ValueError("input outside measured curve domain") + return float(np.interp(value, self.knots, getattr(self, direction + "_rad"))) + + def measure(self, quaternion_xyzw: Sequence[float]) -> tuple[float, float]: + relative = Rotation.from_quat(self.reference_xyzw).inv() * _rotation(quaternion_xyzw) + vector = relative.as_rotvec() + axis = np.asarray(self.axis_xyz) + angle = float(vector @ axis) + predicted = Rotation.from_rotvec(axis * angle) + off_axis_error = float((predicted.inv() * relative).magnitude()) + return angle, off_axis_error + + def holdout_errors(self, observations: Sequence[RotationObservation]) -> tuple[float, ...]: + _validate_observations(observations, cycles={3}) + if any(row.sample_id in self.training_sample_ids for row in observations): + raise ValueError("holdout image identity overlaps training") + reference = Rotation.from_quat(self.reference_xyzw) + axis = np.asarray(self.axis_xyz) + # Full SO(3) residual, not just its projection onto the fitted axis: + # a slipped/tilted Tag cannot pass by retaining a plausible scalar angle. + return tuple(float(((reference * Rotation.from_rotvec( + axis * self.predict(row.input_value, row.direction) + )).inv() * _rotation(row.quaternion_xyzw)).magnitude()) for row in observations) + + +def _rotation(quaternion: Sequence[float]) -> Rotation: + values = np.asarray(quaternion, dtype=float) + if values.shape != (4,) or not np.all(np.isfinite(values)) or np.linalg.norm(values) < 1e-12: + raise ValueError("Tag quaternion must be finite and nondegenerate") + return Rotation.from_quat(values) + + +def _validate_observations(rows: Sequence[RotationObservation], *, cycles: set[int]) -> None: + if not rows: + raise ValueError("rotation observations are missing") + ids = [row.sample_id for row in rows] + if not all(ids) or len(set(ids)) != len(ids): + raise ValueError("rotation image identities must be nonempty and unique") + for row in rows: + if row.cycle not in cycles or row.direction not in {"increasing", "decreasing"}: + raise ValueError("rotation cycle/direction is outside the requested data partition") + if not math.isfinite(row.input_value): + raise ValueError("rotation input must be finite") + _rotation(row.quaternion_xyzw) + + +def fit_rotation_curve( + observations: Sequence[RotationObservation], *, input_domain: str, + sdk_to_joint_direction: int, knot_count: int = 65, +) -> RotationCurve: + """Fit three complete training cycles without integerizing physical inputs. + + A fixed reference and axis are learned once. The two branches keep their + measured hysteresis; per-cycle/per-direction recentering is forbidden. + The resulting support is the common observed native-input interval. + """ + if input_domain not in {"feedback_rad", "command_rad", "feedback_u8", "command_u8"}: + raise ValueError("an explicit native SDK input domain is required") + if sdk_to_joint_direction not in {-1, 1} or knot_count < 32: + raise ValueError("invalid direction or curve knot count") + _validate_observations(observations, cycles={0, 1, 2}) + required = {(cycle, direction) for cycle in range(3) for direction in ("increasing", "decreasing")} + groups = {(row.cycle, row.direction) for row in observations} + if groups != required: + raise ValueError("three complete bidirectional training cycles are required") + for cycle, direction in required: + if sum(row.cycle == cycle and row.direction == direction for row in observations) < 40: + raise ValueError("each training direction requires 40 unique observations") + inputs = np.asarray([row.input_value for row in observations]) + if input_domain.endswith("u8") and (np.min(inputs) < 0 or np.max(inputs) > 255): + raise ValueError("byte observations are outside the SDK domain") + rotations = Rotation.from_quat([row.quaternion_xyzw for row in observations]) + # Use one observed reference, never assert that its input is a CAD zero. + reference = rotations[int(np.argmin(inputs))] + vectors = (reference.inv() * rotations).as_rotvec() + _, singular, vh = np.linalg.svd(vectors, full_matrices=False) + if not singular.size or singular[0] < math.radians(1): + raise ValueError("hinge rotation is not observable") + axis = vh[0] + angles = vectors @ axis + trend = float((inputs - np.mean(inputs)) @ (angles - np.mean(angles))) + if abs(trend) <= 1e-12: + raise ValueError("SDK/visual motion direction is not observable") + if math.copysign(1, trend) != sdk_to_joint_direction: + axis, angles = -axis, -angles + if np.ptp(angles) >= math.pi - math.radians(1): + raise ValueError("hinge travel exceeds this rotation primitive's unambiguous interval") + supports = [] + for direction in ("increasing", "decreasing"): + selected = [i for i, row in enumerate(observations) if row.direction == direction] + x = inputs[selected] + supports.append((float(np.min(x)), float(np.max(x)))) + lower, upper = max(s[0] for s in supports), min(s[1] for s in supports) + if upper <= lower: + raise ValueError("directional SDK supports do not overlap") + knots = np.linspace(lower, upper, knot_count) + branches = {} + maximum_correction = 0.0 + for direction in ("increasing", "decreasing"): + mask = np.asarray([row.direction == direction for row in observations]) + x, y = inputs[mask], angles[mask] + order = np.argsort(x, kind="stable") + unique, starts = np.unique(x[order], return_index=True) + # Median repeated encoder observations before projecting monotonically. + medians = np.asarray([np.median(group) for group in np.split(y[order], starts[1:])]) + projected = -sdk_to_joint_direction * isotonic_nonincreasing(-sdk_to_joint_direction * medians) + maximum_correction = max(maximum_correction, float(np.max(np.abs(projected - medians)))) + branches[direction] = tuple(float(v) for v in np.interp(knots, unique, projected)) + return RotationCurve(input_domain, tuple(float(v) for v in knots), + branches["increasing"], branches["decreasing"], tuple(reference.as_quat()), + tuple(axis), sdk_to_joint_direction, frozenset(row.sample_id for row in observations), maximum_correction) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py new file mode 100644 index 0000000..9d4c50d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/session.py @@ -0,0 +1,155 @@ +"""Profile-selected geometry/curve/mimic fitting, with no model imports.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import math +from pathlib import Path +from typing import Mapping + +from ..domain.profile import CalibrationProfile +from ..domain.result import CalibrationResult, JointMapping +from ..urdf.kinematics import UrdfKinematicModel +from .motion_fit import channel_for_joint, fit_profile_motion, joint_input_direction +from .spatial import ZeroCalibrationProfile, fit_joint_axis_measurement, solve_urdf_zero_offsets, with_depth_free_axis_projection + + +@dataclass(frozen=True) +class SpatialHandContract: + side: str + layout_id: str + active_joints: tuple[str, ...] + reference_finger: str = "" + palm_orientation_sources: Mapping[str, str] = field(default_factory=dict) + minimum_palm_orientation_sources: int = 0 + stable_cross_view_cone_bias: bool = False + + +def compile_spatial_profile(profile: CalibrationProfile) -> ZeroCalibrationProfile: + data = profile.zero.spatial + allowed = {"root_anchor_joints", "axis_parent_joint", "phase_parent_joint", + "offset_observer_joint", "base_pose_strategy", "orientation_anchor_joint", + "directed_base_axis_joints", "depth_free_axis_projection", "axis_order", + "same_view_axis_pair_by_offset", + "parallel_root_pattern", + "accept_validated_zero_in_confidence_interval", "project_axis_gauge_before_image"} + if not data or set(data) - allowed: + raise ValueError("spatial observation graph is missing or has unsupported fields") + direct = tuple(profile.zero.direct_zero_joints) + observers = dict(data.get("offset_observer_joint", {})) + if set(observers) != set(direct): + raise ValueError("every required zero must have an explicit observable axis/phase datum") + axes = tuple(data.get("axis_order", profile.zero.axis_joints)) + if set(axes) != set(profile.zero.axis_joints) or len(set(axes)) != len(axes): + raise ValueError("spatial axis order differs from declared observations") + return ZeroCalibrationProfile( + hand=SpatialHandContract(profile.key.side, profile.key.layout, tuple(profile.zero.active_joints), + stable_cross_view_cone_bias=profile.measurement.stable_cross_view_cone_bias), + direct_zero_joints=direct, axis_joints=axes, + inherited_zero_joints=dict(profile.zero.transferred_zero_sources), + inherited_static_zero_joints=dict(profile.zero.transferred_zero_sources), + constrained_circle_joints=frozenset(axes), + root_anchor_joints=frozenset(data.get("root_anchor_joints", ())), + axis_parent_joint=dict(data.get("axis_parent_joint", {})), + phase_parent_joint=dict(data.get("phase_parent_joint", {})), + offset_observer_joint=observers, + same_view_axis_pair_by_offset={name: tuple(pair) for name, pair in data.get("same_view_axis_pair_by_offset", {}).items()}, + fixed_direct_zero_offsets_rad={}, static_output_zero_offsets_rad={}, + base_pose_strategy=str(data.get("base_pose_strategy", "full_hand")), + orientation_anchor_joint=data.get("orientation_anchor_joint"), + directed_base_axis_joints=frozenset(data.get("directed_base_axis_joints", ())), + accept_validated_zero_in_confidence_interval=bool(data.get("accept_validated_zero_in_confidence_interval", False)), + project_axis_gauge_before_image=bool(data.get("project_axis_gauge_before_image", False)), + parallel_root_pattern=bool(data.get("parallel_root_pattern", False))) + + +def fit_profile_calibration(profile: CalibrationProfile, source_urdf: Path, records_by_joint, + *, cross_view_records=None) -> CalibrationResult: + """Shared native-domain motion, spatial zero and standard mimic fit. + + No mechanical endpoint or range-centre fallback can replace a requested + but unobservable spatial zero. The fourth cycle never updates the fit. + """ + if profile.zero.endpoint_anchor_by_joint: + raise ValueError("unverified mechanical endpoints cannot define CAD zeros") + required_pose_fields = {"parent_pose_common", "child_pose_common", + "relative_translation_xyz_m", "view_normal_common_xyz", + "camera_center_common_xyz_m", f"state_{profile.command.unit}"} + for joint in profile.zero.axis_joints: + rows = records_by_joint.get(joint, ()) + if not rows or any(not required_pose_fields.issubset(row) for row in rows): + raise ValueError(f"spatial calibration requires identified common-frame pose observations:{joint}") + model = UrdfKinematicModel(source_urdf) + zero_profile = compile_spatial_profile(profile) + motion = fit_profile_motion(profile, model, records_by_joint, cross_view_records=cross_view_records) + curves = motion.coordinate_curves + observations = [] + for cycle in range(4): + by_joint = {} + for joint in zero_profile.axis_joints: + rows = records_by_joint[joint] + if profile.command.unit == "rad": + rows = [dict(row, input_value=float(row[profile.curve_input_domain]), state_values=row["state_rad"]) + for row in rows] + else: + rows = [dict(row, command_u8=int(round(float(row[profile.curve_input_domain])))) for row in rows] + parent = zero_profile.phase_parent_joint.get(joint) + if parent is not None and parent not in by_joint: + raise ValueError("spatial axis order must place phase parents first") + task = next(task for task in profile.motion.tasks if joint in task.joints) + baseline = (task.start_value if profile.command.unit == "rad" + else profile.command.baseline_values[channel_for_joint(profile, joint)]) + axis = fit_joint_axis_measurement(joint, rows, cycle=cycle, zero_command_u8=baseline, + input_to_joint_direction=joint_input_direction(profile, model, joint), + constrained_circle_joints=zero_profile.constrained_circle_joints, + view_normal_common_xyz=rows[0]["view_normal_common_xyz"], + canonical_zero_direction="decreasing", + axis_common_constraint=None if parent is None else by_joint[parent].axis_common_xyz, + separate_axial_residual=True) + if profile.zero.spatial.get("depth_free_axis_projection", False): + axis = with_depth_free_axis_projection(axis, rows[0]["camera_center_common_xyz_m"]) + observations.append(axis) + by_joint[joint] = axis + zero = solve_urdf_zero_offsets(source_urdf=source_urdf, measurements=observations, + curves=curves, motor_by_joint={joint: channel_for_joint(profile, joint) for joint in curves}, + zero_profile=zero_profile, training_cycles=(0, 1, 2), validation_cycle=3, + maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20), + maximum_validation_mae_rad=math.radians(1), maximum_validation_p95_rad=math.radians(2), + maximum_validation_error_rad=math.radians(3), maximum_confidence_half_width_rad=math.radians(1), + maximum_pose_axis_line_rms_m=0.0015) + if not zero.passed: + raise ValueError("spatial_zero_not_observable_or_inaccurate:" + str(zero.failure_reasons)) + offsets = {name: float(zero.direct_offsets_rad[name]) for name in profile.zero.direct_zero_joints} + methods = {name: "urdf_serial_axis_geometry" for name in offsets} + for name in profile.zero.active_joints - offsets.keys() - profile.zero.transferred_zero_sources.keys(): + if name not in profile.zero.cad_frozen_joints: + raise ValueError(f"active static zero has no measured or CAD-retention policy:{name}") + offsets[name], methods[name] = 0.0, "source_cad_zero_profile_excluded" + for target, donor in profile.zero.transferred_zero_sources.items(): + offsets[target] = offsets[donor] + methods[target] = "transferred_static_zero_on_own_cad" + mappings = {} + for joint, channel in profile.command.command_index_by_joint.items(): + if joint not in profile.zero.active_joints: + continue + donor = profile.zero.transferred_zero_sources.get(joint, joint) + curve = curves[donor] + knots = tuple(curve.circle.get("input_knots", range(256))) + branches = (curve.angle_rad, curve.increasing_rad, curve.decreasing_rad) + if profile.command.unit == "u8": + # Legacy primitive tables may extend to 0/255. Publish only the + # intersection actually measured in both training directions. + import numpy as np + training = records_by_joint[donor] + support = [[float(r[profile.curve_input_domain]) for r in training + if r["cycle"] in {0, 1, 2} and r["direction"] == direction] + for direction in ("increasing", "decreasing")] + lo, hi = math.ceil(max(min(s) for s in support)), math.floor(min(max(s) for s in support)) + knots = tuple(float(v) for v in range(lo, hi+1)) + branches = tuple(tuple(float(v) for v in np.interp(knots, range(256), values)) for values in branches) + mappings[joint] = JointMapping(joint, channel, profile.curve_input_domain, knots, + *branches, + donor if donor != joint else None) + return CalibrationResult(motion.observed_curves, mappings, offsets, methods, + motion.mimic_evidence, motion.holdout_errors_rad, zero, + motion.cross_view_metrics, motion.reference_inputs) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial.py new file mode 100644 index 0000000..eb2f687 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial.py @@ -0,0 +1,47 @@ +"""Stable spatial API; numerical implementations live in spatial_solver.""" + +from .spatial_solver.types import ZeroCalibrationProfile as ZeroCalibrationProfile +from .spatial_solver.geometry_helpers import circle_direction_is_constrained as circle_direction_is_constrained +from .spatial_solver.geometry_helpers import select_cross_view_roll_direction_source as select_cross_view_roll_direction_source +from .spatial_solver.geometry_helpers import ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 as ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 +from .spatial_solver.geometry_helpers import AXIS_POINT_MINIMUM_ROTATION_RAD as AXIS_POINT_MINIMUM_ROTATION_RAD +from .spatial_solver.geometry_helpers import AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD as AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD +from .spatial_solver.geometry_helpers import AXIS_POINT_RESIDUAL_SCALE_M as AXIS_POINT_RESIDUAL_SCALE_M +from .spatial_solver.geometry_helpers import ZERO_MINIMUM_AXIS_CONE_RAD as ZERO_MINIMUM_AXIS_CONE_RAD +from .spatial_solver.geometry_helpers import _zero_sensitive_axis_error_rad as _zero_sensitive_axis_error_rad +from .spatial_solver.geometry_helpers import _axis_cone_mismatch_rad as _axis_cone_mismatch_rad +from .spatial_solver.geometry_helpers import _vector as _vector +from .spatial_solver.geometry_helpers import _pose_matrix as _pose_matrix +from .spatial_solver.geometry_helpers import _relative_rotation as _relative_rotation +from .spatial_solver.rotation_curves import _reference_group_key as _reference_group_key +from .spatial_solver.rotation_curves import _canonical_reference_records as _canonical_reference_records +from .spatial_solver.rotation_curves import _spatial_input as _spatial_input +from .spatial_solver.rotation_curves import _interpolate_reference_rotation as _interpolate_reference_rotation +from .spatial_solver.rotation_curves import _near_zero_records as _near_zero_records +from .spatial_solver.rotation_curves import _baseline_reference as _baseline_reference +from .spatial_solver.rotation_curves import baseline_hysteresis_by_cycle_rad as baseline_hysteresis_by_cycle_rad +from .spatial_solver.rotation_curves import fit_rotation_joint_curve as fit_rotation_joint_curve +from .spatial_solver.rotation_curves import measure_rotation_joint_observation as measure_rotation_joint_observation +from .spatial_solver.rotation_curves import measure_joint_curve_observation as measure_joint_curve_observation +from .spatial_solver.rotation_curves import rotation_curve_holdout_errors as rotation_curve_holdout_errors +from .spatial_solver.rotation_curves import joint_curve_holdout_errors as joint_curve_holdout_errors +from .spatial_solver.types import JointAxisMeasurement as JointAxisMeasurement +from .spatial_solver.types import PalmOrientationMeasurement as PalmOrientationMeasurement +from .spatial_solver.geometry_helpers import PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 as PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 +from .spatial_solver.geometry_helpers import PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD as PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD +from .spatial_solver.geometry_helpers import PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT as PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT +from .spatial_solver.geometry_helpers import PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE as PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE +from .spatial_solver.axis_observations import _incremental_common_rotation_axis as _incremental_common_rotation_axis +from .spatial_solver.axis_observations import fit_partial_palm_orientation_measurement as fit_partial_palm_orientation_measurement +from .spatial_solver.axis_observations import fit_partial_palm_orientation_measurements as fit_partial_palm_orientation_measurements +from .spatial_solver.axis_lines import with_depth_free_axis_projection as with_depth_free_axis_projection +from .spatial_solver.axis_lines import cross_view_side_line_source as cross_view_side_line_source +from .spatial_solver.axis_lines import axis_line_uses_depth_free_interpretation_plane as axis_line_uses_depth_free_interpretation_plane +from .spatial_solver.axis_lines import refit_axis_line_group_with_shared_radius as refit_axis_line_group_with_shared_radius +from .spatial_solver.axis_lines import maximum_axis_line_cycle_spread_m as maximum_axis_line_cycle_spread_m +from .spatial_solver.axis_lines import axis_line_cycle_rms_m as axis_line_cycle_rms_m +from .spatial_solver.axis_lines import _fit_axis_point_from_pose_trajectory as _fit_axis_point_from_pose_trajectory +from .spatial_solver.axis_observations import fit_joint_axis_measurement as fit_joint_axis_measurement +from .spatial_solver.types import ZeroSolveResult as ZeroSolveResult +from .spatial_solver.geometry_helpers import _angles_from_state as _angles_from_state +from .spatial_solver.solve import solve_urdf_zero_offsets as solve_urdf_zero_offsets diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/__init__.py new file mode 100644 index 0000000..d40bebb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/__init__.py @@ -0,0 +1 @@ +"""Shared spatial mathematics; import the stable spatial facade for public APIs.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py new file mode 100644 index 0000000..2fe9046 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/acceptance.py @@ -0,0 +1,310 @@ +"""Unchanged spatial acceptance gates and public result assembly.""" + +from __future__ import annotations + +import math + +import numpy as np + +from .stages import ( + AcceptanceDecision, + CycleEvidence, + GeometryEvidence, + HoldoutEvidence, + LineEvidence, + ObservabilityEvidence, + OffsetLimits, + TrainingFit, + ZeroProblem, + ZeroSolveOptions, +) +from .types import ZeroSolveResult + + +def decide_acceptance( + *, + cycles: CycleEvidence, + fit: TrainingFit, + geometry_evidence: GeometryEvidence, + holdout: HoldoutEvidence, + limits: OffsetLimits, + observability: ObservabilityEvidence, + options: ZeroSolveOptions, + problem: ZeroProblem, +) -> AcceptanceDecision: + applied_training = cycles.applied_training + confidence_half_widths = cycles.confidence_half_widths + cycle_consistent = cycles.cycle_consistent + diagnostic_offset_limits = limits.diagnostic_offset_limits + fixed_offsets = problem.fixed_offsets + improvement_confidence_lower = holdout.improvement_confidence_lower + improvement_passed = holdout.improvement_passed + inconsistent_cycles = cycles.inconsistent_cycles + insignificant_large = cycles.insignificant_large + maximum_confidence_half_width_rad = options.maximum_confidence_half_width_rad + maximum_observability_condition_number = options.maximum_observability_condition_number + maximum_offset_rad = options.maximum_offset_rad + maximum_validation_error_rad = options.maximum_validation_error_rad + maximum_validation_mae_rad = options.maximum_validation_mae_rad + maximum_validation_p95_rad = options.maximum_validation_p95_rad + observability_condition_number = observability.observability_condition_number + observability_parameter_count = observability.observability_parameter_count + observability_rank = observability.observability_rank + observation_failures = geometry_evidence.observation_failures + offset_limits = limits.offset_limits + output_offsets = problem.output_offsets + palm_orientation_validation_passed = holdout.palm_orientation_validation_passed + product_finger_rolls = limits.product_finger_rolls + profile = problem.profile + training_offsets = fit.training_offsets + validation_errors = holdout.validation_errors + + finger_roll_common_mode = ( + float( + np.median( + [training_offsets[name] for name in product_finger_rolls] + ) + ) + if profile.parallel_root_pattern + and product_finger_rolls + else 0.0 + ) + configured_limit_exceeded: list[str] = [] + for name, limit in zip(profile.direct_zero_joints, offset_limits): + if name in fixed_offsets: + continue + # A post-solve mechanical endpoint datum is the value that will be + # published for this joint. The visual root-axis scalar remains a + # nuisance gauge used for holdout geometry and must not be compared + # with the safety bound of a different, endpoint-anchored output. + checked_offset = output_offsets.get(name, training_offsets[name]) + if name in product_finger_rolls: + # The four roll motors share the same electrical centre and the + # absolute palm axial datum is recovered from the root-line + # pattern. Protect the independently assembled finger-to-finger + # deviations with the strict finger bound; protect their shared + # common mode with the unchanged global zero bound. Treating the + # same common datum as four independent failures is both + # over-counting and sensitive to the palm-frame gauge. + checked_offset -= finger_roll_common_mode + if abs(checked_offset) > limit + math.radians(0.01): + configured_limit_exceeded.append(name) + if ( + product_finger_rolls + and abs(finger_roll_common_mode) + > maximum_offset_rad + math.radians(0.01) + ): + configured_limit_exceeded.append("finger_mcp_roll_common_mode") + diagnostic_bound_hits: list[str] = [] + for name, limit in zip( + profile.direct_zero_joints, diagnostic_offset_limits + ): + if name in fixed_offsets: + continue + checked_offset = output_offsets.get(name, training_offsets[name]) + if name in product_finger_rolls: + # Match the configured-limit and publication convention above. + # The raw common roll is a fitted-palm-frame gauge; only the + # finger-to-finger deviation is a physical zero correction. + checked_offset -= finger_roll_common_mode + if abs(checked_offset) >= limit - math.radians(0.01): + diagnostic_bound_hits.append(name) + failure_reasons: dict[str, str] = {} + if not palm_orientation_validation_passed: + failure_reasons["palm_orientation"] = ( + "palm_orientation_holdout_too_large" + ) + requires_full_observability = bool( + profile.parallel_root_pattern + and profile.base_pose_strategy == "full_hand" + ) + if ( + requires_full_observability + and observability_rank < observability_parameter_count + ): + failure_reasons["palm_and_static_zero"] = ( + "zero_observation_jacobian_rank_deficient" + ) + elif ( + requires_full_observability + and observability_condition_number + > maximum_observability_condition_number + ): + failure_reasons["palm_and_static_zero"] = ( + "zero_observation_jacobian_ill_conditioned" + ) + for name in configured_limit_exceeded: + failure_reasons[name] = "zero_offset_exceeds_configured_limit" + for name in diagnostic_bound_hits: + failure_reasons[name] = "zero_offset_reached_diagnostic_bound" + for name in inconsistent_cycles: + failure_reasons[name] = "zero_offset_cycle_difference_too_large" + for name in insignificant_large: + failure_reasons[name] = "zero_offset_not_statistically_significant" + if maximum_confidence_half_width_rad is not None: + for name, half_width in confidence_half_widths.items(): + if half_width > maximum_confidence_half_width_rad: + failure_reasons[name] = ( + "zero_offset_confidence_interval_too_wide" + ) + # This solver publishes rotational encoder zeros only. A post-fit CAD to + # measured axis-line displacement is invariant to the joint's own zero + # and cannot be repaired by changing that rotational parameter. Keep the + # per-joint and aggregate values in ZeroSolveResult for geometry audit, + # but do not misclassify a fixed link-origin/Tag-depth discrepancy as a + # failed rotational holdout. Axis-point *fit* quality is still guarded + # above for every phase observation that actually uses line position. + if not improvement_passed: + for name, value in applied_training.items(): + if ( + name not in fixed_offsets + and value != 0.0 + and improvement_confidence_lower.get(name, 0.0) <= 0.0 + ): + failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" + # Geometry failures are the root cause and must not be hidden by the + # downstream validation symptom produced by the same bad observation. + failure_reasons.update(observation_failures) + passed = bool( + validation_errors.size + == len(profile.direct_zero_joints) - len(fixed_offsets) + and float(np.mean(validation_errors)) <= maximum_validation_mae_rad + and float(np.percentile(validation_errors, 95.0)) + <= maximum_validation_p95_rad + and ( + maximum_validation_error_rad is None + or float(np.max(validation_errors)) + <= maximum_validation_error_rad + ) + and cycle_consistent + and not insignificant_large + and not configured_limit_exceeded + and not diagnostic_bound_hits + and not observation_failures + and ( + not requires_full_observability + or ( + observability_rank == observability_parameter_count + and observability_condition_number + <= maximum_observability_condition_number + ) + ) + and not any( + reason == "zero_offset_confidence_interval_too_wide" + for reason in failure_reasons.values() + ) + and improvement_passed + and palm_orientation_validation_passed + ) + + return AcceptanceDecision( + finger_roll_common_mode=finger_roll_common_mode, + failure_reasons=failure_reasons, + passed=passed, + ) + + +def assemble_result( + validation_cycle: int, + *, + cycles: CycleEvidence, + decision: AcceptanceDecision, + fit: TrainingFit, + geometry_evidence: GeometryEvidence, + holdout: HoldoutEvidence, + limits: OffsetLimits, + lines: LineEvidence, + observability: ObservabilityEvidence, + problem: ZeroProblem, +): + applied_training = cycles.applied_training + axis_cone_bias_classification_by_joint = geometry_evidence.axis_cone_bias_classification_by_joint + axis_cone_mismatch_by_joint = geometry_evidence.axis_cone_mismatch_by_joint + axis_line_rms = lines.axis_line_rms + base_rotation = fit.base_rotation + base_translation = fit.base_translation + confidence_half_widths = cycles.confidence_half_widths + cycle_values = cycles.cycle_values + failure_reasons = decision.failure_reasons + finger_roll_common_mode = decision.finger_roll_common_mode + improvement_by_joint = holdout.improvement_by_joint + improvement_confidence_lower = holdout.improvement_confidence_lower + observability_condition_number = observability.observability_condition_number + observability_parameter_count = observability.observability_parameter_count + observability_rank = observability.observability_rank + offset_covariance = observability.offset_covariance + original_error_by_joint = holdout.original_error_by_joint + output_offsets = problem.output_offsets + passed = decision.passed + product_finger_rolls = limits.product_finger_rolls + profile = problem.profile + training_cycle_ids = cycles.training_cycle_ids + uncertainties = cycles.uncertainties + validation_error_by_joint = holdout.validation_error_by_joint + validation_errors = holdout.validation_errors + validation_line_error_by_joint = lines.validation_line_error_by_joint + + # Never refit a model that has passed its holdout with the validation + # cycle. The published offsets are exactly the frozen training result + # that produced ``validation_errors`` above. + final_offsets = dict(applied_training) + # Apply independently validated assembly datums only after trajectory + # fitting and holdout validation. A mechanical prior must define the + # written artifact without perturbing downstream yaw/pitch estimates. + final_offsets.update(output_offsets) + if profile.parallel_root_pattern and product_finger_rolls: + # The camera solve observes the four roll axes in a fitted palm frame. + # Rotation of that frame about their shared datum is a gauge, not four + # independent finger assembly errors. The product command 127/CAD + # pose defines the common straight-ahead datum; publish only each + # finger's robust deviation from the four-finger median. Validation + # above remains in the observation gauge, so no measured residual is + # discarded. + for name in product_finger_rolls: + final_offsets[name] = ( + float(final_offsets[name]) - finger_roll_common_mode + ) + + # Every active joint must be present in the runtime payload/URDF writer, + # but absence of an absolute observation is not evidence for the + # reference finger's assembly offset. Preserve the source-CAD zero for + # those independent motors while continuing to share their dynamic curve. + all_offsets = { + name: 0.0 for name in profile.hand.active_joints + } + all_offsets.update(final_offsets) + for target, source in profile.inherited_static_zero_joints.items(): + all_offsets[target] = final_offsets[source] + return ZeroSolveResult( + direct_offsets_rad=final_offsets, + all_active_offsets_rad=all_offsets, + base_translation_xyz_m=tuple(float(value) for value in base_translation), + base_quaternion_xyzw=tuple(float(value) for value in base_rotation.as_quat()), + validation_errors_rad=tuple(float(value) for value in validation_errors), + validation_error_by_joint_rad=validation_error_by_joint, + validation_line_error_by_joint_m=validation_line_error_by_joint, + axis_line_rms_m=axis_line_rms, + passed=passed, + cycle_offsets_rad={ + name: tuple(float(value) for value in values) + for name, values in cycle_values.items() + }, + offset_uncertainty_rad=uncertainties, + offset_confidence_half_width_rad=confidence_half_widths, + training_cycles=training_cycle_ids, + validation_cycle=int(validation_cycle), + validation_original_error_by_joint_rad=original_error_by_joint, + validation_improvement_by_joint_rad=improvement_by_joint, + validation_improvement_confidence_lower_rad=( + improvement_confidence_lower + ), + observability_rank=observability_rank, + observability_parameter_count=observability_parameter_count, + observability_condition_number=observability_condition_number, + offset_covariance_rad2=offset_covariance, + axis_cone_mismatch_by_joint_rad=axis_cone_mismatch_by_joint, + axis_cone_bias_classification_by_joint=( + axis_cone_bias_classification_by_joint + ), + failure_reasons=failure_reasons, + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_lines.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_lines.py new file mode 100644 index 0000000..6519a14 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_lines.py @@ -0,0 +1,603 @@ +"""Axis lines.""" + +from __future__ import annotations + +from dataclasses import replace +from typing import Any, Mapping, Sequence +import math + +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation +import numpy as np + +from ...geometry.rotation import robust_rotation_summary +from ..trajectory_geometry import _fit_circle_with_axis, _plane_basis +from .geometry_helpers import ( + AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD, + AXIS_POINT_MINIMUM_ROTATION_RAD, + AXIS_POINT_RESIDUAL_SCALE_M, + _pose_matrix, + _relative_rotation, + _vector, +) +from .rotation_curves import _baseline_reference, _canonical_reference_records, _near_zero_records +from .types import JointAxisMeasurement + + +def with_depth_free_axis_projection( + measurement: JointAxisMeasurement, + camera_center_common_xyz_m: Sequence[float], +) -> JointAxisMeasurement: + """Attach the source-camera interpretation plane of an axis line. + + The plane is defined by the camera centre and the fitted 3-D line, but is + only a projective observation: moving either fitted line point along its + optical ray leaves the plane unchanged. A fixed Tag mount changes the + moving point trajectory, not its recovered physical screw axis. + """ + camera_center = _vector( + camera_center_common_xyz_m, 3, name="axis camera centre" + ) + point = np.asarray(measurement.point_common_xyz_m, dtype=float) + axis = np.asarray(measurement.axis_common_xyz, dtype=float) + axis /= np.linalg.norm(axis) + ray = point - camera_center + ray_norm = float(np.linalg.norm(ray)) + if ray_norm <= 1.0e-6: + raise ValueError("axis point coincides with its source camera") + plane_normal = np.cross(ray / ray_norm, axis) + plane_norm = float(np.linalg.norm(plane_normal)) + if plane_norm <= 1.0e-6: + raise ValueError("axis projection is degenerate in its source camera") + plane_normal /= plane_norm + return replace( + measurement, + axis_point_camera_center_common_xyz_m=tuple( + float(value) for value in camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) for value in plane_normal + ), + ) + + +def cross_view_side_line_source( + measurement: JointAxisMeasurement, +) -> str | None: + """Return the side alias that supplied a fallback axis-line point.""" + sources = tuple( + str(source) + for source in getattr( + measurement, "pose_axis_line_source_joints", () + ) + ) + if ( + getattr(measurement, "axis_point_source", "") + in { + "side_circle_cross_view", + "side_circle_shared_radius_cross_view", + "side_interpretation_plane_cross_view", + } + and len(sources) == 1 + and sources[0].endswith("_side") + ): + return sources[0] + return None + + +def axis_line_uses_depth_free_interpretation_plane( + measurement: JointAxisMeasurement, +) -> bool: + """Return whether only the source-camera bearing is geometrically used.""" + return bool( + measurement.axis_point_source + in { + "side_interpretation_plane_cross_view", + "front_interpretation_plane_cross_view_validated", + } + and measurement.axis_point_camera_center_common_xyz_m is not None + ) + + +def refit_axis_line_group_with_shared_radius( + measurements: Sequence[JointAxisMeasurement], + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + canonical_zero_direction: str | None, +) -> tuple[JointAxisMeasurement, ...]: + """Refit repeated cross-view axis lines with one physical radius. + + A side-view roll alias observes the same child Tag and the same physical + lever arm in every cycle. Fitting an independent radius to each short, + near-edge-on arc leaves radius and circle centre strongly correlated; + sub-pixel PnP noise can then move the reported axis line by several + millimetres even though every trajectory has a low radial residual. + + Keep a separate centre (and therefore an independent line-repeatability + check) for every cycle, but solve one shared radius from all cycles. This + is a physical constraint rather than a relaxed quality gate: the returned + lines are still checked against the unchanged cycle RMS limit, and each + cycle's radial residual remains an independent hard check. + """ + group = tuple(measurements) + if len(group) < 2: + return group + cycles = [int(measurement.cycle) for measurement in group] + if len(set(cycles)) != len(cycles): + raise ValueError("shared-radius axis group contains duplicate cycles") + + entries: list[dict[str, Any]] = [] + initial_parameters: list[float] = [] + initial_radii: list[float] = [] + for measurement in group: + cycle_records = [ + dict(record) + for record in records + if int(record.get("cycle", -1)) == int(measurement.cycle) + ] + motion_records = [ + record + for record in cycle_records + if str(record.get("kind", "sample")) != "baseline_hold_sample" + ] + if len(motion_records) < 6: + motion_records = cycle_records + if len(motion_records) < 6: + raise ValueError( + f"cycle {measurement.cycle + 1} has too few shared-radius samples" + ) + + reference_records = _canonical_reference_records( + cycle_records, canonical_zero_direction + ) + zero_records = _near_zero_records( + reference_records, int(zero_command_u8) + ) + if not zero_records: + raise ValueError( + f"cycle {measurement.cycle + 1} has no shared-radius zero pose" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_translation = np.median(parent_poses[:, :3, 3], axis=0) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + parent_rotation = Rotation.from_quat(parent_quaternion) + common_axis = _vector( + measurement.axis_common_xyz, 3, name="shared-radius common axis" + ) + common_axis /= np.linalg.norm(common_axis) + parent_axis = parent_rotation.inv().apply(common_axis) + parent_axis /= np.linalg.norm(parent_axis) + + points = np.asarray( + [record["relative_translation_xyz_m"] for record in motion_records], + dtype=float, + ) + basis_x, basis_y = _plane_basis(parent_axis) + origin = np.mean(points, axis=0) + local = points - origin + points_xy = np.column_stack((local @ basis_x, local @ basis_y)) + initial = _fit_circle_with_axis(points, parent_axis) + initial_center = _vector( + initial["center_xyz_m"], 3, name="initial shared-radius centre" + ) + initial_parameters.extend( + [ + float((initial_center - origin) @ basis_x), + float((initial_center - origin) @ basis_y), + ] + ) + initial_radii.append(float(initial["radius_m"])) + entries.append( + { + "measurement": measurement, + "points": points, + "points_xy": points_xy, + "origin": origin, + "basis_x": basis_x, + "basis_y": basis_y, + "parent_axis": parent_axis, + "parent_rotation": parent_rotation, + "parent_translation": parent_translation, + } + ) + + median_radius = float(np.median(initial_radii)) + if median_radius <= 1.0e-6 or not math.isfinite(median_radius): + raise ValueError("shared-radius axis group has an invalid initial radius") + initial_value = np.asarray( + [*initial_parameters, median_radius], dtype=float + ) + + def residual(parameters: np.ndarray) -> np.ndarray: + radius = float(parameters[-1]) + return np.concatenate( + [ + np.linalg.norm( + entry["points_xy"] + - parameters[2 * index : 2 * index + 2], + axis=1, + ) + - radius + for index, entry in enumerate(entries) + ] + ) / 0.0005 + + lower = np.full(initial_value.shape, -np.inf, dtype=float) + upper = np.full(initial_value.shape, np.inf, dtype=float) + lower[-1] = max(1.0e-6, 0.5 * median_radius) + upper[-1] = 2.0 * median_radius + solution = least_squares( + residual, + initial_value, + bounds=(lower, upper), + loss="soft_l1", + f_scale=1.0, + max_nfev=3000, + ) + if not solution.success: + raise ValueError( + "shared-radius axis optimization failed: " + solution.message + ) + + shared_radius = float(solution.x[-1]) + if not math.isfinite(shared_radius): + raise ValueError("shared-radius axis optimization is non-finite") + result: list[JointAxisMeasurement] = [] + for index, entry in enumerate(entries): + center_xy = solution.x[2 * index : 2 * index + 2] + center_parent = ( + entry["origin"] + + float(center_xy[0]) * entry["basis_x"] + + float(center_xy[1]) * entry["basis_y"] + ) + # The coordinate along an infinite axis is a gauge. Retain the + # robust centre of this cycle's observed axial coordinates. + axial_offsets = ( + entry["points"] - center_parent + ) @ entry["parent_axis"] + center_parent += float(np.median(axial_offsets)) * entry["parent_axis"] + center_common = ( + entry["parent_rotation"].apply(center_parent) + + entry["parent_translation"] + ) + radial_residual = ( + np.linalg.norm( + entry["points_xy"] - center_xy, + axis=1, + ) + - shared_radius + ) + radial_rms = float(np.sqrt(np.mean(np.square(radial_residual)))) + measurement = entry["measurement"] + result.append( + replace( + measurement, + point_common_xyz_m=tuple( + float(value) for value in center_common + ), + radial_rms_m=max(float(measurement.radial_rms_m), radial_rms), + axis_point_source="side_circle_shared_radius_cross_view", + ) + ) + return tuple(result) + + +def maximum_axis_line_cycle_spread_m( + measurements: Sequence[JointAxisMeasurement], +) -> float: + """Measure repeatability of independently fitted near-parallel lines. + + Axis-line points have an arbitrary coordinate along their own direction. + Compare only the perpendicular displacement, symmetrically against both + fitted directions, so that the result remains meaningful with the small + allowed cycle-to-cycle direction variation. + """ + maximum = 0.0 + for left_index, left in enumerate(measurements): + left_axis = np.asarray(left.axis_common_xyz, dtype=float) + left_axis /= np.linalg.norm(left_axis) + left_point = np.asarray(left.point_common_xyz_m, dtype=float) + for right in measurements[left_index + 1 :]: + right_axis = np.asarray(right.axis_common_xyz, dtype=float) + right_axis /= np.linalg.norm(right_axis) + delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point + maximum = max( + maximum, + float(np.linalg.norm(np.cross(delta, left_axis))), + float(np.linalg.norm(np.cross(delta, right_axis))), + ) + return maximum + + +def axis_line_cycle_rms_m( + measurements: Sequence[JointAxisMeasurement], +) -> float: + """Return RMS line-position scatter about the four-cycle consensus. + + For parallel lines, the sum of squared pairwise distances divided by + ``n**2`` equals the mean squared distance from their centroid. Averaging + each pair's distance against both near-parallel directions preserves that + identity while avoiding an arbitrary choice of one cycle's direction. + """ + count = len(measurements) + if count < 2: + return 0.0 + squared_pairwise_sum = 0.0 + for left_index, left in enumerate(measurements): + left_axis = np.asarray(left.axis_common_xyz, dtype=float) + left_axis /= np.linalg.norm(left_axis) + left_point = np.asarray(left.point_common_xyz_m, dtype=float) + for right in measurements[left_index + 1 :]: + right_axis = np.asarray(right.axis_common_xyz, dtype=float) + right_axis /= np.linalg.norm(right_axis) + delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point + left_distance = float(np.linalg.norm(np.cross(delta, left_axis))) + right_distance = float(np.linalg.norm(np.cross(delta, right_axis))) + squared_pairwise_sum += 0.5 * ( + left_distance**2 + right_distance**2 + ) + return float(math.sqrt(squared_pairwise_sum / (count**2))) + + +def _fit_axis_point_from_pose_trajectory( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + axis_parent_xyz: Sequence[float], + angle_axis_parent_xyz: Sequence[float], + phase_reference_point_parent_xyz: Sequence[float], + 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. + + If ``T(q)`` maps the moving Tag into its parent Tag frame, then + ``T(q) @ inv(T(0))`` is a rotation about the physical joint axis and its + translation obeys ``(I - R(q)) p = t(q)``. Solving this equation over the + complete trajectory uses both pose orientation and translation and avoids + treating a monocular Tag-centre depth arc as ground-truth geometry. + """ + samples = [dict(record) for record in records] + reference_samples = _canonical_reference_records( + samples, canonical_zero_direction + ) + zero_records = _near_zero_records(reference_samples, zero_command_u8) + if not zero_records: + raise ValueError( + f"axis-point fit has no record near baseline {zero_command_u8}" + ) + + reference_rotation = Rotation.from_quat( + _baseline_reference(reference_samples, zero_command_u8) + ).as_matrix() + reference_translation = np.median( + np.asarray( + [record["relative_translation_xyz_m"] for record in zero_records], + dtype=float, + ), + axis=0, + ) + axis = _vector(axis_parent_xyz, 3, name="axis-point direction") + axis /= np.linalg.norm(axis) + angle_axis = _vector( + angle_axis_parent_xyz, 3, name="axis-point angle direction" + ) + angle_axis /= np.linalg.norm(angle_axis) + phase_reference_point = _vector( + phase_reference_point_parent_xyz, + 3, + name="axis-point phase reference", + ) + helper = ( + np.asarray([1.0, 0.0, 0.0]) + if abs(float(axis[0])) < 0.8 + else np.asarray([0.0, 1.0, 0.0]) + ) + basis_first = np.cross(axis, helper) + basis_first /= np.linalg.norm(basis_first) + basis = np.column_stack( + (basis_first, np.cross(axis, basis_first)) + ) + + view_normal_common = None + use_image_plane_projection = False + if view_normal_common_xyz is not None: + view_normal_common = _vector( + view_normal_common_xyz, 3, name="view normal" + ) + view_normal_common /= np.linalg.norm(view_normal_common) + reference_parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix( + _pose_matrix(record["parent_pose_common"])[:3, :3] + ).as_quat() + for record in zero_records + ] + )[0] + reference_parent_rotation = Rotation.from_quat( + reference_parent_quaternion + ) + reference_view_normal_parent = reference_parent_rotation.inv().apply( + view_normal_common + ) + reference_view_normal_parent /= np.linalg.norm( + reference_view_normal_parent + ) + use_image_plane_projection = abs( + float(axis @ reference_view_normal_parent) + ) >= math.cos(AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD) + + 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] = [] + reference_radial = reference_translation - phase_reference_point + reference_radial -= axis * float(reference_radial @ axis) + reference_radial_norm = float(np.linalg.norm(reference_radial)) + if reference_radial_norm < 1.0e-6: + raise ValueError("axis-point phase reference radius is degenerate") + reference_radial /= reference_radial_norm + for record in samples: + observed_rotation = Rotation.from_quat( + _relative_rotation(record) + ).as_matrix() + observed_delta_rotation = observed_rotation @ reference_rotation.T + raw_angle = float( + Rotation.from_matrix(observed_delta_rotation).as_rotvec() + @ angle_axis + ) + if abs(raw_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: + continue + observed_translation = _vector( + record["relative_translation_xyz_m"], + 3, + name="relative translation", + ) + observed_radial = observed_translation - phase_reference_point + observed_radial -= axis * float(observed_radial @ axis) + observed_radial_norm = float(np.linalg.norm(observed_radial)) + if observed_radial_norm <= 1.0e-6: + continue + observed_radial /= observed_radial_norm + raw_angles.append(raw_angle) + translation_angles.append( + math.atan2( + float(axis @ np.cross(reference_radial, observed_radial)), + float( + np.clip(reference_radial @ observed_radial, -1.0, 1.0) + ), + ) + ) + orientation_sign = 1.0 + if raw_angles and float( + np.sum(np.asarray(raw_angles) * np.asarray(translation_angles)) + ) < 0.0: + orientation_sign = -1.0 + + for record in samples: + observed_rotation = Rotation.from_quat( + _relative_rotation(record) + ).as_matrix() + observed_translation = _vector( + record["relative_translation_xyz_m"], + 3, + name="relative translation", + ) + observed_delta_rotation = observed_rotation @ reference_rotation.T + signed_angle = float( + Rotation.from_matrix(observed_delta_rotation).as_rotvec() + @ angle_axis + ) * orientation_sign + if abs(signed_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: + continue + # Direction and angle have deliberately separate sources. A trusted + # upstream/circle direction may replace a biased planar-PnP axis, while + # the relative-orientation trajectory still provides a repeatable + # scalar travel. Reconstruct the admissible revolute motion before + # solving its axis line instead of feeding a contradictory 3-D + # rotation into the translation equations. + # The rotation-trajectory axis is undirected. Align its scalar sign + # with the selected physical axis using the Tag-centre trajectory; + # this remains robust even when a trusted upstream direction replaces + # a badly biased planar-PnP orientation axis. + delta_rotation = Rotation.from_rotvec( + axis * signed_angle + ).as_matrix() + delta_translation = ( + observed_translation + - delta_rotation @ reference_translation + ) + # A screw-driven revolute joint may carry real translation along its + # axis. That component is in the null space of (I - R), contains no + # information about the perpendicular axis-line point, and must not + # inflate or bias the line fit as if the mechanism were pure rotary. + axis_projection = ( + np.eye(3) - np.outer(axis, axis) + if allow_axial_translation + else np.eye(3) + ) + 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( + _pose_matrix(record["parent_pose_common"])[:3, :3] + ) + view_normal_parent = parent_rotation.inv().apply( + view_normal_common + ) + view_normal_parent /= np.linalg.norm(view_normal_parent) + image_projection = np.eye(3) - np.outer( + view_normal_parent, view_normal_parent + ) + projection = image_projection @ axis_projection + # A single end-on camera cannot distinguish absolute depth from a + # shift along the revolute axis. Keep the observable image-plane + # equations only; adding a free baseline-depth variable makes the + # axis point rank-deficient and pretends that this gauge is + # observable. Parallel phase chains are later compared in the + # same view, where their common depth gauge cancels. + used_image_plane_projection = True + matrices.append(projection @ axis_point_matrix) + translations.append(projection @ delta_translation) + + if len(matrices) < 6: + raise ValueError("axis-point fit has too few observable pose samples") + matrix = np.vstack(matrices) + translation = np.concatenate(translations) + singular_values = np.linalg.svd(matrix, compute_uv=False) + if ( + singular_values.size < 2 + or singular_values[-1] <= 1.0e-9 + or singular_values[-1] / singular_values[0] < 1.0e-3 + ): + raise ValueError("axis-point pose trajectory is ill-conditioned") + + unknown_count = matrix.shape[1] + solution = least_squares( + lambda value: ( + matrix @ value - translation + ) / AXIS_POINT_RESIDUAL_SCALE_M, + np.zeros(unknown_count, dtype=float), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + if not solution.success: + raise ValueError( + "axis-point pose optimization failed: " + solution.message + ) + 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 + else "pose_trajectory_3d" + ) + return basis @ solution.x[:2], rms, source diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py new file mode 100644 index 0000000..3453e17 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/axis_observations.py @@ -0,0 +1,563 @@ +"""Axis observations.""" + +from __future__ import annotations + +from typing import Any, Mapping, Sequence +import math + +from scipy.spatial.transform import Rotation +import numpy as np + +from ...geometry.rotation import fit_rotation_axis, robust_rotation_summary +from ..trajectory_geometry import _fit_circle_with_axis, _fit_plane_axis +from .axis_lines import _fit_axis_point_from_pose_trajectory +from .geometry_helpers import ( + PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8, + PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE, + PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT, + PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD, + _pose_matrix, + _relative_rotation, + _vector, +) +from .rotation_curves import ( + _baseline_reference, + _canonical_reference_records, + _near_zero_records, + _spatial_input, +) +from .types import JointAxisMeasurement, PalmOrientationMeasurement + + +def _incremental_common_rotation_axis( + samples_by_bin: Mapping[ + tuple[str, int], Sequence[Mapping[str, Any]] + ], + *, + command_separation_u8: int = ( + PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 + ), + minimum_rotation_rad: float = ( + PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD + ), + minimum_pair_count: int = PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT, + consensus_percentile: float = ( + PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE + ), +) -> tuple[np.ndarray, int]: + """Fit one physical axis from robust local Tag rotations. + + A single baseline-to-endpoint logarithm is sensitive to smooth planar-PnP + curvature: the fitted axis then changes when the same Tag trajectory is + translated to another part of the image. Local finite rotations are + expressed directly in the common camera frame, and their undirected + weighted consensus estimates the physical revolute axis. The worst + quartile is discarded once more after the initial consensus, which is + enough to reject endpoint branch curvature without learning a + serial-specific yaw value or an image-position correction table. + """ + separation = int(command_separation_u8) + if separation < 4 or separation > 64: + raise ValueError("palm axis command separation must be in [4, 64]") + if not 0.0 < float(minimum_rotation_rad) < math.pi: + raise ValueError("palm axis minimum increment must be in (0, pi)") + if int(minimum_pair_count) < 6: + raise ValueError("palm axis minimum pair count must be at least six") + percentile = float(consensus_percentile) + if not 50.0 <= percentile <= 90.0: + raise ValueError("palm axis consensus percentile must be in [50, 90]") + + binned_common_rotations: dict[tuple[str, int], Rotation] = {} + for key, group in samples_by_bin.items(): + child_quaternions = [] + for record in group: + pose = record.get("child_pose_common") + if not isinstance(pose, Mapping): + continue + quaternion = pose.get("quaternion_xyzw") + if quaternion is not None: + child_quaternions.append(quaternion) + if not child_quaternions: + continue + binned_common_rotations[(str(key[0]), int(key[1]))] = ( + Rotation.from_quat( + robust_rotation_summary(child_quaternions)[0] + ) + ) + + directions: list[np.ndarray] = [] + weights: list[float] = [] + for direction in ("decreasing", "increasing"): + commands = sorted( + command + for candidate_direction, command in binned_common_rotations + if candidate_direction == direction + ) + for start_command in commands: + candidates = [ + command + for command in commands + if separation <= command - start_command <= separation + 2 + ] + if not candidates: + continue + end_command = candidates[0] + start = binned_common_rotations[(direction, start_command)] + end = binned_common_rotations[(direction, end_command)] + local_rotvec = (start.inv() * end).as_rotvec() + common_rotvec = start.apply(local_rotvec) + increment = float(np.linalg.norm(common_rotvec)) + if increment < float(minimum_rotation_rad): + continue + directions.append(common_rotvec / increment) + weights.append(increment) + + if len(directions) < int(minimum_pair_count): + raise ValueError( + "palm axis has too few full-stroke local rotation pairs: " + f"{len(directions)}/{int(minimum_pair_count)}" + ) + vectors = np.asarray(directions, dtype=float) + increments = np.asarray(weights, dtype=float) + + def consensus(selected: np.ndarray) -> np.ndarray: + scatter = np.einsum( + "n,ni,nj->ij", + increments[selected], + vectors[selected], + vectors[selected], + ) + eigenvalues, eigenvectors = np.linalg.eigh(scatter) + if not np.all(np.isfinite(eigenvalues)): + raise ValueError("palm axis local-rotation consensus is invalid") + axis = eigenvectors[:, -1] + return axis / np.linalg.norm(axis) + + selected = np.ones(len(vectors), dtype=bool) + axis = consensus(selected) + # Two deterministic refinement rounds prevent the initial scatter from + # being pulled toward a dense endpoint-bias cluster. + for _ in range(2): + deviations = np.arccos( + np.clip(np.abs(vectors @ axis), -1.0, 1.0) + ) + limit = float(np.percentile(deviations, percentile)) + selected = deviations <= limit + if int(np.count_nonzero(selected)) < int(minimum_pair_count): + raise ValueError("palm axis robust consensus retained too few pairs") + axis = consensus(selected) + return axis, int(np.count_nonzero(selected)) + + +def fit_partial_palm_orientation_measurement( + source_joint: str, + model_joint: str, + records: Sequence[Mapping[str, Any]], + *, + cycle: int, + zero_command_u8: int, + minimum_arc_rad: float = math.radians(15.0), + maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), + maximum_command_distance_u8: int = 255, +) -> PalmOrientationMeasurement: + """Fit a physical axis within a configured window around encoder zero.""" + cycle_samples = [ + dict(record) + for record in records + if int(record.get("cycle", -1)) == int(cycle) + ] + if len(cycle_samples) < 12: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few visible samples" + ) + if not 0.0 < float(minimum_arc_rad) < math.pi: + raise ValueError("palm orientation minimum arc must be in (0, pi)") + if float(maximum_rotation_orthogonal_rms_rad) <= 0.0: + raise ValueError( + "palm orientation rotation residual limit must be positive" + ) + command_distance = int(maximum_command_distance_u8) + if command_distance < 1 or command_distance > 255: + raise ValueError( + "palm orientation command distance must be in [1, 255]" + ) + zero = int(zero_command_u8) + samples = [ + record + for record in cycle_samples + if abs(int(record["command_u8"]) - zero) <= command_distance + ] + if len(samples) < 12: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few zero-adjacent " + "visible samples" + ) + reference = Rotation.from_quat( + _baseline_reference(samples, int(zero_command_u8)) + ) + samples_by_bin: dict[tuple[str, int], list[Mapping[str, Any]]] = {} + for record in samples: + key = ( + str(record.get("direction", "")), + int(record["command_u8"]), + ) + samples_by_bin.setdefault(key, []).append(record) + if len(samples_by_bin) < 6: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few visible command bins" + ) + binned_rotations = [ + Rotation.from_quat( + robust_rotation_summary( + [_relative_rotation(record) for record in group] + )[0] + ) + for group in samples_by_bin.values() + ] + vectors = [ + (reference.inv() * rotation).as_rotvec() + for rotation in binned_rotations + ] + axis_child = fit_rotation_axis( + vectors, [command for _direction, command in samples_by_bin] + ) + angles = np.asarray( + [float(vector @ axis_child) for vector in vectors], dtype=float + ) + observed_arc = float(np.ptp(angles)) + orthogonal_rms = float( + np.sqrt( + np.mean( + [ + np.linalg.norm( + vector - float(vector @ axis_child) * axis_child + ) + ** 2 + for vector in vectors + ] + ) + ) + ) + if observed_arc < float(minimum_arc_rad): + raise ValueError( + f"{source_joint} cycle {cycle + 1} visible rotation arc " + f"{math.degrees(observed_arc):.3f}deg is below " + f"{math.degrees(minimum_arc_rad):.3f}deg" + ) + if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad): + raise ValueError( + f"{source_joint} cycle {cycle + 1} zero-adjacent rotation " + f"residual {math.degrees(orthogonal_rms):.3f}deg exceeds " + f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg" + ) + zero_records = _near_zero_records(samples, int(zero_command_u8)) + if not zero_records: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has no visible zero pose" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + axis_parent = reference.apply(axis_child) + axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent) + axis_common /= np.linalg.norm(axis_common) + axis_estimator = "baseline_relative_so3" + incremental_pair_count = 0 + # Product recordings persist the selected moving-Tag pose in the common + # camera frame. Use the complete visible stroke only for a local-motion + # axis consensus; retain the zero-adjacent fit above as the unchanged arc + # and residual quality gate. Legacy/synthetic records without the common + # child pose keep their previous estimator exactly. + if any( + isinstance(record.get("child_pose_common"), Mapping) + for record in cycle_samples + ): + full_stroke_bins: dict[ + tuple[str, int], list[Mapping[str, Any]] + ] = {} + for record in cycle_samples: + key = ( + str(record.get("direction", "")), + int(record["command_u8"]), + ) + full_stroke_bins.setdefault(key, []).append(record) + incremental_axis, incremental_pair_count = ( + _incremental_common_rotation_axis(full_stroke_bins) + ) + if float(incremental_axis @ axis_common) < 0.0: + incremental_axis = -incremental_axis + axis_common = incremental_axis + axis_estimator = "robust_full_stroke_local_so3_v1" + state = np.median( + np.asarray( + [record["state_u8"] for record in zero_records], dtype=float + ), + axis=0, + ) + return PalmOrientationMeasurement( + source_joint=str(source_joint), + model_joint=str(model_joint), + cycle=int(cycle), + axis_common_xyz=tuple(float(value) for value in axis_common), + condition_state_u8=tuple(float(value) for value in state), + observed_arc_rad=observed_arc, + rotation_orthogonal_rms_rad=orthogonal_rms, + axis_estimator=axis_estimator, + incremental_pair_count=incremental_pair_count, + ) + + +def fit_partial_palm_orientation_measurements( + *, + sources: Mapping[str, str], + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + motor_by_source: Mapping[str, int], + baseline_command_u8: Sequence[int], + cycles: Sequence[int], + minimum_sources: int, + minimum_arc_rad: float = math.radians(15.0), + maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), + maximum_command_distance_u8: int = 255, +) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]: + """Fit every usable optional source and require configured coverage.""" + source_map = {str(name): str(model) for name, model in sources.items()} + minimum = int(minimum_sources) + if not source_map: + if minimum != 0: + raise ValueError( + "palm orientation minimum is non-zero without sources" + ) + return (), {} + if minimum < 2 or minimum > len(source_map): + raise ValueError( + "palm orientation minimum source count must be between two " + "and the configured source count" + ) + missing_motors = sorted(set(source_map) - set(motor_by_source)) + if missing_motors: + raise ValueError( + "palm orientation sources are missing motor mappings: " + + ",".join(missing_motors) + ) + fitted: list[PalmOrientationMeasurement] = [] + rejected: dict[str, str] = {} + for cycle in (int(value) for value in cycles): + cycle_fitted: list[PalmOrientationMeasurement] = [] + for source_joint, model_joint in source_map.items(): + motor = int(motor_by_source[source_joint]) + try: + measurement = fit_partial_palm_orientation_measurement( + source_joint, + model_joint, + records_by_joint.get(source_joint, ()), + cycle=cycle, + zero_command_u8=int(baseline_command_u8[motor]), + minimum_arc_rad=minimum_arc_rad, + maximum_rotation_orthogonal_rms_rad=( + maximum_rotation_orthogonal_rms_rad + ), + maximum_command_distance_u8=( + maximum_command_distance_u8 + ), + ) + except Exception as error: + rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error) + continue + cycle_fitted.append(measurement) + if len(cycle_fitted) < minimum: + cycle_reasons = { + key: value + for key, value in rejected.items() + if key.endswith(f":cycle{cycle + 1}") + } + raise ValueError( + f"palm orientation cycle {cycle + 1} has " + f"{len(cycle_fitted)}/{minimum} usable sources: " + + "; ".join( + f"{key}={value}" for key, value in cycle_reasons.items() + ) + ) + fitted.extend(cycle_fitted) + return tuple(fitted), rejected + + +def fit_joint_axis_measurement( + joint: str, + records: Sequence[Mapping[str, Any]], + *, + cycle: int, + zero_command_u8: int, + axis_common_constraint: Sequence[float] | None = None, + constrained_circle_joints: frozenset[str] = frozenset(), + view_normal_common_xyz: Sequence[float] | None = None, + canonical_zero_direction: str | None = None, + separate_axial_residual: bool = False, + input_to_joint_direction: int = -1, +) -> JointAxisMeasurement: + """Fit one physical screw axis from one complete scan cycle.""" + samples = [ + dict(record) for record in records if int(record["cycle"]) == int(cycle) + ] + if len(samples) < 12: + raise ValueError(f"{joint} cycle {cycle + 1} has too few samples") + points = np.asarray( + [record["relative_translation_xyz_m"] for record in samples], dtype=float + ) + free_circle_axis, free_plane_rms = _fit_plane_axis([points]) + singular_values = np.linalg.svd( + points - np.mean(points, axis=0), compute_uv=False + ) + circle_axis_observability = ( + 0.0 + if singular_values.size < 2 or singular_values[0] <= 1.0e-12 + else float(singular_values[1] / singular_values[0]) + ) + + reference_samples = _canonical_reference_records( + samples, canonical_zero_direction + ) + reference = Rotation.from_quat( + _baseline_reference(reference_samples, zero_command_u8) + ) + rotation_vectors = [ + (reference.inv() * Rotation.from_quat(_relative_rotation(record))).as_rotvec() + for record in samples + ] + commands = [_spatial_input(record) for record in samples] + if "input_value" in samples[0]: + if input_to_joint_direction not in {-1, 1}: + raise ValueError("native spatial input requires a fixed direction") + vectors = np.asarray(rotation_vectors, dtype=float) + _, singular, vh = np.linalg.svd(vectors, full_matrices=False) + if not singular.size or singular[0] <= 1e-6: + raise ValueError("native rotation axis is unobservable") + rotation_axis_child = vh[0] + trend = float((np.asarray(commands) - np.mean(commands)) @ (vectors @ rotation_axis_child)) + if abs(trend) <= 1e-12: + raise ValueError("native rotation direction is unobservable") + if np.sign(trend) != input_to_joint_direction: + rotation_axis_child = -rotation_axis_child + else: + rotation_axis_child = fit_rotation_axis(rotation_vectors, commands) + # reference maps the child Tag frame at baseline into the parent Tag + # frame. The quaternion delta axis is expressed in that child frame, + # while the fitted centre circle is expressed in the parent frame. This + # conversion is what makes arbitrary Tag mounting rotations harmless. + rotation_axis = reference.apply(rotation_axis_child) + if float(rotation_axis @ free_circle_axis) < 0.0: + free_circle_axis = -free_circle_axis + disagreement = math.acos( + float(np.clip(rotation_axis @ free_circle_axis, -1.0, 1.0)) + ) + + zero_records = _near_zero_records(reference_samples, zero_command_u8) + if not zero_records: + raise ValueError( + f"{joint} cycle has no record near baseline {zero_command_u8}" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_translation = np.median(parent_poses[:, :3, 3], axis=0) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + parent_rotation = Rotation.from_quat(parent_quaternion) + axis_direction_source = "rotation" + + if axis_common_constraint is not None: + common_axis = _vector( + axis_common_constraint, 3, name="common axis constraint" + ) + common_axis /= np.linalg.norm(common_axis) + fitted_axis = parent_rotation.inv().apply(common_axis) + if float(fitted_axis @ rotation_axis) < 0.0: + fitted_axis = -fitted_axis + circle = _fit_circle_with_axis(points, fitted_axis) + plane_rms = float(circle["plane_rms_m"]) + axis_direction_source = "upstream_constraint" + elif str(joint) in constrained_circle_joints: + # The side/front views are close to end-on for these axes. Monocular + # planar-PnP depth bias can therefore tilt even an apparently smooth, + # low-residual Tag-centre circle. The relative orientation trajectory + # observes the screw-axis direction directly and is invariant to an + # arbitrary fixed Tag mounting rotation; use it to constrain the 3-D + # circle and estimate only the axis line. Never switch the direction + # source based on a rotation-vs-circle threshold: that discontinuity + # makes otherwise identical repeat cycles choose different models. + fitted_axis = rotation_axis + circle = _fit_circle_with_axis(points, fitted_axis) + plane_rms = float(circle["plane_rms_m"]) + else: + # Oblique trajectories with an observable 3-D motion plane retain the + # independent rotation/centre cross-check and fuse both estimates. + free_circle = _fit_circle_with_axis(points, free_circle_axis) + fitted_axis = rotation_axis + free_circle_axis + if float(np.linalg.norm(fitted_axis)) < 1.0e-9: + raise ValueError(f"{joint} rotation and centre axes are opposed") + fitted_axis /= np.linalg.norm(fitted_axis) + circle = free_circle + plane_rms = max( + float(free_plane_rms), float(circle["plane_rms_m"]) + ) + 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, + zero_command_u8=zero_command_u8, + axis_parent_xyz=fitted_axis, + angle_axis_parent_xyz=rotation_axis, + 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=separate_axial, + residual_diagnostics=residual_diagnostics, + ) + ) + point_common = parent_rotation.apply(point_parent) + parent_translation + state = np.median( + np.asarray([record["state_values"] if "state_values" in record else record["state_u8"] + for record in zero_records], dtype=float), + axis=0, + ) + return JointAxisMeasurement( + joint=str(joint), + cycle=int(cycle), + axis_common_xyz=tuple(float(value) for value in axis_common), + point_common_xyz_m=tuple(float(value) for value in point_common), + condition_state_u8=tuple(float(value) for value in state), + plane_rms_m=float(plane_rms), + radial_rms_m=float(circle["radial_rms_m"]), + rotation_circle_axis_difference_rad=float(disagreement), + view_normal_common_xyz=( + None + if view_normal_common_xyz is None + else tuple( + float(value) + for value in _vector( + view_normal_common_xyz, 3, name="view normal" + ) + ) + ), + axis_direction_source=axis_direction_source, + 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),), + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/base_pose.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/base_pose.py new file mode 100644 index 0000000..4e76bb1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/base_pose.py @@ -0,0 +1,440 @@ +"""Base localization selected by geometric constraints, shared by all profiles.""" + +from __future__ import annotations + +from typing import Mapping, Sequence +import math + +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation +import numpy as np + +from .geometry_helpers import ZERO_MINIMUM_AXIS_CONE_RAD +from .residuals import ObservationGeometry +from .stages import TrainingProblem +from .types import JointAxisMeasurement + + +def fit_base_pose( + selected: Sequence[JointAxisMeasurement], + pose_offsets: Mapping[str, float] | None = None, + *, + problem: TrainingProblem, + geometry: ObservationGeometry, +) -> tuple[Rotation, np.ndarray]: + profile = problem.profile + zero_offsets = problem.zero_offsets + curves = geometry.curves + predicted_local = geometry.predicted_local + + pose_zero_offsets = zero_offsets if pose_offsets is None else pose_offsets + anchors_by_joint = { + name: [item for item in selected if item.joint == name] + for name in profile.root_anchor_joints + } + if any(not items for items in anchors_by_joint.values()): + raise ValueError("zero solve is missing root axis anchors") + + def undirected_axis_average( + values: Sequence[Sequence[float]], + ) -> np.ndarray: + vectors = np.asarray(values, dtype=float) + reference = vectors[0] / np.linalg.norm(vectors[0]) + signs = np.where(vectors @ reference < 0.0, -1.0, 1.0) + result = np.sum(vectors * signs[:, None], axis=0) + norm = float(np.linalg.norm(result)) + if norm < 1.0e-9: + raise ValueError("root axis average is degenerate") + return result / norm + + root_names = tuple(sorted(profile.root_anchor_joints)) + predicted_axes: dict[str, np.ndarray] = {} + predicted_points: dict[str, np.ndarray] = {} + observed_axes: dict[str, np.ndarray] = {} + observed_points: dict[str, np.ndarray] = {} + for name in root_names: + items = anchors_by_joint[name] + predicted = [predicted_local(item, pose_zero_offsets) for item in items] + predicted_axes[name] = undirected_axis_average( + [axis for axis, _ in predicted] + ) + predicted_points[name] = np.median( + np.asarray([point for _, point in predicted]), axis=0 + ) + observed_axes[name] = undirected_axis_average( + [item.axis_common_xyz for item in items] + ) + observed_points[name] = np.median( + np.asarray( + [item.point_common_xyz_m for item in items], dtype=float + ), + axis=0, + ) + + if ( + profile.parallel_root_pattern + and profile.base_pose_strategy == "full_hand" + ): + # In the product layout none of the four finger-roll zeros is a + # mechanical prior. Using one finger's pitch axis to orient the + # palm would therefore absorb that finger's roll zero into the + # palm pose. The named positions of five parallel root lines do + # observe rotation about their common direction, independent of + # all five root-joint zeros. Fit that transverse line pattern and + # leave the unobservable translation along the common direction + # at zero; all zero-sensitive phase residuals use line-to-line + # differences and are invariant to that gauge. + predicted_common = undirected_axis_average( + [predicted_axes[name] for name in root_names] + ) + observed_common = undirected_axis_average( + [observed_axes[name] for name in root_names] + ) + predicted_center = np.mean( + np.asarray([predicted_points[name] for name in root_names]), + axis=0, + ) + observed_center = np.mean( + np.asarray([observed_points[name] for name in root_names]), + axis=0, + ) + predicted_pattern = { + name: ( + (delta := predicted_points[name] - predicted_center) + - predicted_common * float(delta @ predicted_common) + ) + for name in root_names + } + observed_pattern = { + name: ( + (delta := observed_points[name] - observed_center) + - observed_common * float(delta @ observed_common) + ) + for name in root_names + } + if max( + np.linalg.norm(value) for value in predicted_pattern.values() + ) < 0.003: + raise ValueError("root axis-line pattern is degenerate") + if max( + np.linalg.norm(value) for value in observed_pattern.values() + ) < 0.003: + raise ValueError("observed root axis-line pattern is degenerate") + + def align_axis(source: np.ndarray, target: np.ndarray) -> Rotation: + source = source / np.linalg.norm(source) + target = target / np.linalg.norm(target) + cross = np.cross(source, target) + cross_norm = float(np.linalg.norm(cross)) + dot = float(np.clip(source @ target, -1.0, 1.0)) + if cross_norm > 1.0e-10: + return Rotation.from_rotvec( + cross / cross_norm * math.atan2(cross_norm, dot) + ) + if dot > 0.0: + return Rotation.identity() + basis = np.eye(3)[int(np.argmin(np.abs(source)))] + axis = np.cross(source, basis) + axis /= np.linalg.norm(axis) + return Rotation.from_rotvec(axis * math.pi) + + candidates: list[tuple[float, Rotation, np.ndarray]] = [] + for sign in (1.0, -1.0): + signed_observed_axis = sign * observed_common + axis_rotation = align_axis( + predicted_common, signed_observed_axis + ) + mapped_pattern = { + name: axis_rotation.apply(predicted_pattern[name]) + for name in root_names + } + cosine = sum( + float(mapped_pattern[name] @ observed_pattern[name]) + for name in root_names + ) + sine = sum( + float( + signed_observed_axis + @ np.cross( + mapped_pattern[name], observed_pattern[name] + ) + ) + for name in root_names + ) + phase_rotation = Rotation.from_rotvec( + signed_observed_axis * math.atan2(sine, cosine) + ) + rotation = phase_rotation * axis_rotation + translation_samples = [] + for name in root_names: + delta = observed_points[name] - rotation.apply( + predicted_points[name] + ) + translation_samples.append( + delta + - signed_observed_axis + * float(delta @ signed_observed_axis) + ) + translation = np.median( + np.asarray(translation_samples), axis=0 + ) + + # Root-axis points recovered from planar Tags occasionally + # contain a large but repeatable depth/line-position outlier. + # A plain Procrustes sum lets one such point rotate the palm + # frame and then makes the same bias look like a common zero + # offset on all four finger-roll joints. Refine only the + # rigid palm pose with a millimetre-scale robust loss. At + # least three mutually consistent named root lines therefore + # determine the transverse pattern while an outlier remains + # visible in the line diagnostics below. + transverse_helper = np.eye(3)[ + int(np.argmin(np.abs(signed_observed_axis))) + ] + transverse_first = np.cross( + signed_observed_axis, transverse_helper + ) + transverse_first /= np.linalg.norm(transverse_first) + transverse_second = np.cross( + signed_observed_axis, transverse_first + ) + + def robust_root_pattern_residual( + value: np.ndarray, + ) -> np.ndarray: + candidate_rotation = ( + Rotation.from_rotvec( + signed_observed_axis * float(value[0]) + ) + * rotation + ) + candidate_translation = ( + translation + + transverse_first * float(value[1]) + + transverse_second * float(value[2]) + ) + residuals: list[float] = [] + for name in root_names: + delta = observed_points[name] - ( + candidate_rotation.apply(predicted_points[name]) + + candidate_translation + ) + residuals.extend( + ( + float(delta @ transverse_first) / 0.001, + float(delta @ transverse_second) / 0.001, + ) + ) + return np.asarray(residuals, dtype=float) + + root_pattern_solution = least_squares( + robust_root_pattern_residual, + np.zeros(3, dtype=float), + bounds=( + np.asarray([-math.pi, -0.25, -0.25]), + np.asarray([math.pi, 0.25, 0.25]), + ), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + if not root_pattern_solution.success: + raise ValueError("robust root axis-line pattern fit failed") + rotation = ( + Rotation.from_rotvec( + signed_observed_axis + * float(root_pattern_solution.x[0]) + ) + * rotation + ) + translation = ( + translation + + transverse_first * float(root_pattern_solution.x[1]) + + transverse_second * float(root_pattern_solution.x[2]) + ) + axis_errors: list[float] = [] + for item in selected: + predicted_axis, _ = predicted_local(item, pose_zero_offsets) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + axis_errors.append( + math.acos( + abs( + float( + np.clip( + rotation.apply(predicted_axis) + @ observed_axis, + -1.0, + 1.0, + ) + ) + ) + ) + ) + line_errors = [] + for name in root_names: + predicted_point = ( + rotation.apply(predicted_points[name]) + translation + ) + delta = observed_points[name] - predicted_point + line_errors.append( + float( + np.linalg.norm( + delta + - signed_observed_axis + * float(delta @ signed_observed_axis) + ) + ) + ) + score = float(np.mean(np.square(axis_errors))) + float( + np.mean(np.square(np.asarray(line_errors) / 0.01)) + ) + candidates.append((score, rotation, translation)) + _, rotation, translation = min( + candidates, key=lambda item: item[0] + ) + non_root_items = [ + item + for item in selected + if item.joint not in profile.root_anchor_joints + ] + if not non_root_items: + raise ValueError( + "non-parallel axes are required for palm axial translation" + ) + + def axial_translation_residual(value: np.ndarray) -> np.ndarray: + candidate_translation = ( + translation + observed_common * float(value[0]) + ) + residuals: list[float] = [] + for item in non_root_items: + predicted_axis, predicted_point = predicted_local( + item, pose_zero_offsets + ) + predicted_axis = rotation.apply(predicted_axis) + predicted_point = ( + rotation.apply(predicted_point) + + candidate_translation + ) + observed_axis = np.asarray( + item.axis_common_xyz, dtype=float + ) + if float(predicted_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + delta = np.asarray( + item.point_common_xyz_m, dtype=float + ) - predicted_point + perpendicular = delta - observed_axis * float( + delta @ observed_axis + ) + residuals.extend( + float(component) / 0.001 + for component in perpendicular + ) + return np.asarray(residuals, dtype=float) + + axial_solution = least_squares( + axial_translation_residual, + np.asarray([0.0]), + bounds=(np.asarray([-1.0]), np.asarray([1.0])), + loss="soft_l1", + f_scale=1.0, + ) + if not axial_solution.success: + raise ValueError("palm axial translation fit failed") + translation = ( + translation + + observed_common * float(axial_solution.x[0]) + ) + return rotation, translation + + # Legacy layouts have only one observed finger chain. Their + # non-parallel reference-finger pitch axis remains the orientation + # anchor; the product profile above deliberately does not use it. + primary = max( + root_names, + key=lambda name: float( + np.ptp(np.asarray(curves[name].angle_rad)) + ), + ) + orientation_anchor = profile.orientation_anchor_joint + if not orientation_anchor: + raise ValueError("serial zero solve requires an orientation anchor") + orientation_items = [ + item for item in selected if item.joint == orientation_anchor + ] + if not orientation_items: + raise ValueError( + f"zero solve is missing palm orientation anchor " + f"{orientation_anchor}" + ) + predicted_orientation_axis = undirected_axis_average( + [ + predicted_local(item, pose_zero_offsets)[0] + for item in orientation_items + ] + ) + observed_orientation_axis = undirected_axis_average( + [item.axis_common_xyz for item in orientation_items] + ) + + def frame_from_two_axes( + primary_axis: np.ndarray, orientation_axis: np.ndarray + ) -> np.ndarray: + first = primary_axis / np.linalg.norm(primary_axis) + second = orientation_axis - first * float(orientation_axis @ first) + second_norm = float(np.linalg.norm(second)) + if second_norm < math.sin(ZERO_MINIMUM_AXIS_CONE_RAD): + raise ValueError("palm orientation axes are nearly parallel") + second /= second_norm + return np.column_stack((first, second, np.cross(first, second))) + + predicted_frame = frame_from_two_axes( + predicted_axes[primary], + predicted_orientation_axis, + ) + candidates: list[tuple[float, Rotation, np.ndarray]] = [] + for primary_sign in (1.0, -1.0): + for orientation_sign in (1.0, -1.0): + observed_frame = frame_from_two_axes( + primary_sign * observed_axes[primary], + orientation_sign * observed_orientation_axis, + ) + rotation = Rotation.from_matrix( + observed_frame @ predicted_frame.T + ) + translation = np.median( + np.asarray( + [ + observed_points[name] + - rotation.apply(predicted_points[name]) + for name in root_names + ] + ), + axis=0, + ) + + # Resolve both undirected-axis sign branches using every + # distinct measured joint. This selects a palm-frame branch; + # it does not fit link geometry or encoder offsets. + errors_by_joint: dict[str, list[float]] = {} + for item in selected: + predicted_axis, _ = predicted_local(item, pose_zero_offsets) + predicted_axis = rotation.apply(predicted_axis) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + alignment = float( + np.clip(predicted_axis @ observed_axis, -1.0, 1.0) + ) + error = math.acos( + alignment + if item.joint in profile.directed_base_axis_joints + else abs(alignment) + ) + errors_by_joint.setdefault(item.joint, []).append(error) + branch_score = sum( + min(float(np.median(values)), math.radians(30.0)) ** 2 + for values in errors_by_joint.values() + ) + candidates.append((branch_score, rotation, translation)) + _, rotation, translation = min(candidates, key=lambda item: item[0]) + return rotation, translation diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py new file mode 100644 index 0000000..da35c05 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/geometry_helpers.py @@ -0,0 +1,216 @@ +"""Geometry helpers.""" + +from __future__ import annotations + +from typing import Any, Mapping, Sequence +import math + +from scipy.spatial.transform import Rotation +import numpy as np + +from ...domain.measurement import JointCurveFit + + +def circle_direction_is_constrained( + joint_name: str, constrained_circle_joints: frozenset[str] +) -> bool: + """Match the constraint used by both canonical and side aliases.""" + name = str(joint_name) + return name in constrained_circle_joints or name.endswith("_side") + + +def select_cross_view_roll_direction_source( + primary_cone_residuals_rad: Sequence[float], + secondary_cone_residuals_rad: Sequence[float], + maximum_cone_mismatch_rad: float, +) -> str: + """Choose one roll-axis view for the complete repeated-sweep group. + + The choice must not be made independently for every cycle. A residual + sitting just either side of the cone gate can otherwise alternate the + selected camera and turn a fixed cross-view bias into a false cycle-axis + spread. The secondary view wins only with a three-quarter consensus and + a median residual inside the unchanged geometry gate; ambiguous groups + retain the primary view. + """ + primary = np.asarray(primary_cone_residuals_rad, dtype=float) + secondary = np.asarray(secondary_cone_residuals_rad, dtype=float) + if ( + primary.ndim != 1 + or secondary.ndim != 1 + or len(primary) != len(secondary) + or len(primary) < 3 + or not np.all(np.isfinite(primary)) + or not np.all(np.isfinite(secondary)) + or float(maximum_cone_mismatch_rad) <= 0.0 + ): + return "primary" + limit = float(maximum_cone_mismatch_rad) + required_consensus = int(math.ceil(0.75 * len(primary))) + primary_passes = int(np.count_nonzero(primary <= limit)) + secondary_passes = int(np.count_nonzero(secondary <= limit)) + if ( + secondary_passes >= required_consensus + and primary_passes < required_consensus + and float(np.median(secondary)) <= limit + and float(np.median(primary)) > limit + ): + return "secondary" + return "primary" + + +ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 + + +AXIS_POINT_MINIMUM_ROTATION_RAD = math.radians(3.0) + + +AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD = math.radians(45.0) + + +AXIS_POINT_RESIDUAL_SCALE_M = 0.001 + + +ZERO_MINIMUM_AXIS_CONE_RAD = math.radians(15.0) + + +def _zero_sensitive_axis_error_rad( + predicted_axis: Sequence[float], + observed_axis: Sequence[float], + parent_axis: Sequence[float], +) -> float: + """Return only the axis error that a parent-joint zero can change. + + Rotating a downstream axis about its parent preserves their mutual cone + angle. The component normal to their plane is therefore the observable + encoder-zero error; cone-angle mismatch belongs to fixed geometry/PnP and + must not push a zero offset or fail its holdout validation. + """ + predicted = _vector(predicted_axis, 3, name="predicted axis") + predicted /= np.linalg.norm(predicted) + observed = _vector(observed_axis, 3, name="observed axis") + observed /= np.linalg.norm(observed) + parent = _vector(parent_axis, 3, name="parent axis") + parent /= np.linalg.norm(parent) + if float(predicted @ observed) < 0.0: + observed = -observed + predicted_projected = predicted - parent * float(predicted @ parent) + observed_projected = observed - parent * float(observed @ parent) + predicted_norm = float(np.linalg.norm(predicted_projected)) + observed_norm = float(np.linalg.norm(observed_projected)) + minimum_projection = math.sin(ZERO_MINIMUM_AXIS_CONE_RAD) + if min(predicted_norm, observed_norm) < minimum_projection: + raise ValueError( + "parent and downstream axes have an unobservable cone angle" + ) + predicted_projected /= predicted_norm + observed_projected /= observed_norm + return math.atan2( + float(parent @ np.cross(predicted_projected, observed_projected)), + float( + np.clip(predicted_projected @ observed_projected, -1.0, 1.0) + ), + ) + + +def _axis_cone_mismatch_rad( + predicted_axis: Sequence[float], + observed_axis: Sequence[float], + parent_axis: Sequence[float], +) -> float: + """Return zero-invariant parent/downstream cone-angle disagreement.""" + predicted = _vector(predicted_axis, 3, name="predicted axis") + predicted /= np.linalg.norm(predicted) + observed = _vector(observed_axis, 3, name="observed axis") + observed /= np.linalg.norm(observed) + parent = _vector(parent_axis, 3, name="parent axis") + parent /= np.linalg.norm(parent) + predicted_cone = math.acos( + abs(float(np.clip(parent @ predicted, -1.0, 1.0))) + ) + observed_cone = math.acos( + abs(float(np.clip(parent @ observed, -1.0, 1.0))) + ) + return abs(predicted_cone - observed_cone) + + +def _vector(value: Sequence[float], size: int, *, name: str) -> np.ndarray: + result = np.asarray(value, dtype=float) + if result.shape != (size,) or not np.all(np.isfinite(result)): + raise ValueError(f"{name} must contain {size} finite values") + return result + + +def _pose_matrix(payload: Mapping[str, Any]) -> np.ndarray: + translation = _vector(payload["translation_xyz_m"], 3, name="translation") + quaternion = _vector(payload["quaternion_xyzw"], 4, name="quaternion") + quaternion /= np.linalg.norm(quaternion) + result = np.eye(4) + result[:3, :3] = Rotation.from_quat(quaternion).as_matrix() + result[:3, 3] = translation + return result + + +def _relative_rotation(record: Mapping[str, Any]) -> np.ndarray: + quaternion = _vector( + record["relative_quaternion_xyzw"], 4, name="relative quaternion" + ) + return quaternion / np.linalg.norm(quaternion) + + +PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 = 24 + + +PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD = math.radians(0.3) + + +PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT = 24 + + +PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE = 75.0 + + +def _angles_from_state( + state_u8: Sequence[float], + *, + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + inherited_zero_joints: Mapping[str, str] | None = None, +) -> dict[str, float]: + state = np.asarray(state_u8, dtype=float) + result: dict[str, float] = {} + for joint, motor in motor_by_joint.items(): + source = (inherited_zero_joints or {}).get(joint, joint) + if source not in curves: + continue + curve = curves[source] + value = float(state[int(motor)]) + if not math.isfinite(value): + raise ValueError("nonfinite spatial condition feedback") + knots = curve.circle.get("input_knots") + if knots is not None: + if not knots[0] <= value <= knots[-1]: + raise ValueError(f"spatial condition outside measured native support:{joint}:{value}") + result[joint] = float(np.interp(value, knots, curve.angle_rad)) + else: + if not 0 <= value <= 255: + raise ValueError(f"spatial condition outside byte domain:{joint}") + result[joint] = float(curve.angle_rad[int(np.rint(value))]) + return result + + +def robust_circular_location(values: Sequence[float]) -> float: + array = np.asarray(values, dtype=float) + if array.size == 0: + raise ValueError("circular residual set is empty") + centre = math.atan2( + float(np.mean(np.sin(array))), + float(np.mean(np.cos(array))), + ) + centred = np.arctan2(np.sin(array - centre), np.cos(array - centre)) + return float( + math.atan2( + math.sin(centre + float(np.median(centred))), + math.cos(centre + float(np.median(centred))), + ) + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py new file mode 100644 index 0000000..c1c76ed --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/optimization.py @@ -0,0 +1,124 @@ +"""Ordered zero-offset optimization using training observations only.""" + +from __future__ import annotations + +from functools import partial +from typing import Mapping, Sequence +import math + +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation +import numpy as np + +from .base_pose import fit_base_pose +from .residuals import ObservationGeometry +from .stages import OffsetLimits, TrainingProblem +from .types import JointAxisMeasurement + + +def optimise_offsets( + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + initial: Mapping[str, float] | None = None, + *, + limits: OffsetLimits, + problem: TrainingProblem, + geometry: ObservationGeometry, +) -> dict[str, float]: + diagnostic_offset_limits = limits.diagnostic_offset_limits + fixed_offsets = problem.fixed_offsets + offset_limits = limits.offset_limits + profile = problem.profile + angular_error_samples = geometry.angular_error_samples + + result = {name: 0.0 for name in profile.direct_zero_joints} + if initial is not None: + result.update( + { + name: float(initial[name]) + for name in profile.direct_zero_joints + } + ) + result.update(fixed_offsets) + for name, diagnostic_limit, configured_limit in zip( + profile.direct_zero_joints, + diagnostic_offset_limits, + offset_limits, + ): + if name in fixed_offsets: + continue + has_axis_pair = name in profile.same_view_axis_pair_by_offset + limit = float( + configured_limit if has_axis_pair else diagnostic_limit + ) + + def residual(value: np.ndarray) -> np.ndarray: + candidate = dict(result) + candidate[name] = float(value[0]) + samples = angular_error_samples( + candidate, selected, base_rotation, base_translation + )[name] + return np.asarray( + [ + math.atan2(math.sin(item), math.cos(item)) + / math.radians(1.0) + for item in samples + ], + dtype=float, + ) + + starts = [0.0, -0.5 * limit, 0.5 * limit] + if initial is not None: + starts.append(float(initial[name])) + solutions = [ + least_squares( + residual, + np.asarray( + [np.clip(start, -limit + 1.0e-9, limit - 1.0e-9)] + ), + bounds=(np.asarray([-limit]), np.asarray([limit])), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + for start in starts + ] + solution = min( + solutions, key=lambda item: float(np.sum(np.square(item.fun))) + ) + if not solution.success: + raise ValueError( + f"URDF zero optimization failed for {name}: " + f"{solution.message}" + ) + result[name] = float(solution.x[0]) + return result + + +def solve_selected( + selected: Sequence[JointAxisMeasurement], + initial: Mapping[str, float] | None = None, + *, + limits: OffsetLimits, + problem: TrainingProblem, + geometry: ObservationGeometry, +) -> tuple[Rotation, np.ndarray, dict[str, float]]: + profile = problem.profile + + fit_pose = partial(fit_base_pose, geometry=geometry, problem=problem) + fit_offsets = partial(optimise_offsets, geometry=geometry, problem=problem, limits=limits) + rotation, translation = fit_pose(selected) + offsets = fit_offsets( + selected, rotation, translation, initial=initial + ) + if profile.base_pose_strategy == "serial_chain": + # The first pose estimate only supplies a branch for the invariant + # same-view yaw solve. Refit after yaw is known so that the + # downstream pitch phase is evaluated in the corrected serial + # thumb frame, then freeze that pose for holdout validation. + rotation, translation = fit_pose(selected, offsets) + offsets = fit_offsets( + selected, rotation, translation, initial=offsets + ) + return rotation, translation, offsets diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py new file mode 100644 index 0000000..e7245cd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/preparation.py @@ -0,0 +1,292 @@ +"""Input validation and separation of training and independent validation.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Mapping, Sequence +import math + +import numpy as np + +from ...urdf.kinematics import UrdfKinematicModel +from .stages import OffsetLimits, ZeroProblem, ZeroSolveOptions +from .types import JointAxisMeasurement, PalmOrientationMeasurement, ZeroCalibrationProfile + + +def prepare_zero_problem( + *, + source_urdf: str | Path, + measurements: Sequence[JointAxisMeasurement], + palm_orientation_measurements: Sequence[PalmOrientationMeasurement], + training_cycles: Sequence[int], + validation_cycle: int, + options: ZeroSolveOptions, + hand_type: str | None, + tag_layout: str | None, + fixed_direct_zero_offsets_rad: Mapping[str, float] | None, + static_output_zero_offsets_rad: Mapping[str, float] | None, + zero_profile: ZeroCalibrationProfile | None, +) -> ZeroProblem: + """Validate protected inputs and separate observations before optimization.""" + maximum_offset_rad = options.maximum_offset_rad + finger_maximum_offset_rad = options.finger_maximum_offset_rad + joint_maximum_offset_rad = options.joint_maximum_offset_rad + maximum_cycle_difference_rad = options.maximum_cycle_difference_rad + minimum_applied_offset_rad = options.minimum_applied_offset_rad + significance_sigma = options.significance_sigma + maximum_validation_error_rad = options.maximum_validation_error_rad + maximum_confidence_half_width_rad = options.maximum_confidence_half_width_rad + maximum_axis_cone_mismatch_rad = options.maximum_axis_cone_mismatch_rad + maximum_systematic_axis_cone_bias_rad = options.maximum_systematic_axis_cone_bias_rad + maximum_pose_axis_line_rms_m = options.maximum_pose_axis_line_rms_m + maximum_observability_condition_number = options.maximum_observability_condition_number + if zero_profile is None: + raise ValueError("zero_profile is required; the core has no model defaults") + profile = zero_profile + hand_type = profile.hand.side if hand_type is None else hand_type + tag_layout = profile.hand.layout_id if tag_layout is None else tag_layout + if profile.hand.side != str(hand_type).lower(): + raise ValueError("zero profile hand side does not match hand_type") + if profile.hand.layout_id != str(tag_layout): + raise ValueError("zero profile layout does not match tag_layout") + model = UrdfKinematicModel(source_urdf) + training = [m for m in measurements if m.cycle in set(training_cycles)] + validation = [m for m in measurements if m.cycle == int(validation_cycle)] + expected = set(profile.axis_joints) + if ( + {m.joint for m in training} != expected + or {m.joint for m in validation} != expected + ): + raise ValueError("axis measurements do not contain all required joints/cycles") + configured_palm_sources = { + str(name): str(model_joint) + for name, model_joint in ( + profile.hand.palm_orientation_sources or {} + ).items() + } + orientation_measurements = tuple(palm_orientation_measurements) + if configured_palm_sources: + minimum_orientation_sources = int( + profile.hand.minimum_palm_orientation_sources + ) + unknown_orientation_sources = sorted( + { + item.source_joint + for item in orientation_measurements + if item.source_joint not in configured_palm_sources + or configured_palm_sources[item.source_joint] + != item.model_joint + } + ) + if unknown_orientation_sources: + raise ValueError( + "palm orientation measurements do not match the product " + "profile: " + + ",".join(unknown_orientation_sources) + ) + required_orientation_cycles = { + *(int(value) for value in training_cycles), + int(validation_cycle), + } + for cycle in required_orientation_cycles: + cycle_sources = { + item.source_joint + for item in orientation_measurements + if int(item.cycle) == cycle + } + if len(cycle_sources) < minimum_orientation_sources: + raise ValueError( + f"palm orientation cycle {cycle + 1} contains " + f"{len(cycle_sources)}/{minimum_orientation_sources} " + "qualified sources" + ) + elif orientation_measurements: + raise ValueError( + "palm orientation measurements were supplied without a profile" + ) + orientation_validation = tuple( + item + for item in orientation_measurements + if int(item.cycle) == int(validation_cycle) + ) + orientation_by_model_cycle: dict[ + tuple[str, int], PalmOrientationMeasurement + ] = {} + for item in orientation_measurements: + key = (str(item.model_joint), int(item.cycle)) + if key in orientation_by_model_cycle: + raise ValueError( + "palm orientation has multiple sources for " + f"{key[0]} cycle {key[1] + 1}" + ) + orientation_by_model_cycle[key] = item + if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad: + raise ValueError("finger maximum offset must be positive and no larger than thumb") + joint_limits = { + str(name): float(value) + for name, value in dict(joint_maximum_offset_rad or {}).items() + } + unknown_joint_limits = sorted(set(joint_limits) - set(profile.direct_zero_joints)) + if unknown_joint_limits: + raise ValueError( + "joint maximum offsets contain unknown direct joints: " + + ",".join(unknown_joint_limits) + ) + if any( + not math.isfinite(value) or not 0.0 < value <= math.radians(90.0) + for value in joint_limits.values() + ): + raise ValueError("joint maximum offsets must be finite and in (0, 90deg]") + if maximum_cycle_difference_rad <= 0.0: + raise ValueError("maximum cycle difference must be positive") + if minimum_applied_offset_rad < 0.0 or significance_sigma < 0.0: + raise ValueError("zero significance thresholds must be non-negative") + if maximum_axis_cone_mismatch_rad <= 0.0: + raise ValueError("maximum axis cone mismatch must be positive") + if ( + maximum_systematic_axis_cone_bias_rad is not None + and ( + maximum_systematic_axis_cone_bias_rad + <= maximum_axis_cone_mismatch_rad + or maximum_systematic_axis_cone_bias_rad > math.radians(90.0) + ) + ): + raise ValueError( + "maximum systematic axis cone bias must exceed the precision " + "limit and be at most 90deg" + ) + if maximum_pose_axis_line_rms_m <= 0.0: + raise ValueError("maximum pose axis-line RMS must be positive") + if maximum_observability_condition_number <= 1.0: + raise ValueError( + "maximum observability condition number must be greater than one" + ) + if ( + maximum_validation_error_rad is not None + and maximum_validation_error_rad <= 0.0 + ): + raise ValueError("maximum validation error must be positive") + if ( + maximum_confidence_half_width_rad is not None + and maximum_confidence_half_width_rad <= 0.0 + ): + raise ValueError("maximum confidence half width must be positive") + fixed_offsets = { + str(name): float(value) + for name, value in ( + profile.fixed_direct_zero_offsets_rad + if fixed_direct_zero_offsets_rad is None + else fixed_direct_zero_offsets_rad + ).items() + } + unknown_fixed_offsets = sorted( + set(fixed_offsets) - set(profile.direct_zero_joints) + ) + if unknown_fixed_offsets: + raise ValueError( + "fixed direct zero offsets contain unknown joints: " + + ",".join(unknown_fixed_offsets) + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in fixed_offsets.values() + ): + raise ValueError( + "fixed direct zero offsets must be finite and within +/-90deg" + ) + output_offsets = { + str(name): float(value) + for name, value in ( + profile.static_output_zero_offsets_rad + if static_output_zero_offsets_rad is None + else static_output_zero_offsets_rad + ).items() + } + unknown_output_offsets = sorted( + set(output_offsets) - set(profile.direct_zero_joints) + ) + if unknown_output_offsets: + raise ValueError( + "static output zero offsets contain unknown joints: " + + ",".join(unknown_output_offsets) + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in output_offsets.values() + ): + raise ValueError( + "static output zero offsets must be finite and within +/-90deg" + ) + zero_offsets = {name: 0.0 for name in profile.direct_zero_joints} + zero_offsets.update(fixed_offsets) + + return ZeroProblem( + profile=profile, + model=model, + training=training, + validation=validation, + configured_palm_sources=configured_palm_sources, + orientation_validation=orientation_validation, + orientation_by_model_cycle=orientation_by_model_cycle, + joint_limits=joint_limits, + fixed_offsets=fixed_offsets, + output_offsets=output_offsets, + zero_offsets=zero_offsets, + ) + + +def prepare_offset_limits( + *, + options: ZeroSolveOptions, + problem: ZeroProblem, +) -> OffsetLimits: + finger_maximum_offset_rad = options.finger_maximum_offset_rad + joint_limits = problem.joint_limits + maximum_offset_rad = options.maximum_offset_rad + profile = problem.profile + + product_finger_rolls = tuple(profile.common_mode_zero_joints) + offset_limits = np.asarray( + [ + joint_limits.get( + name, + ( + finger_maximum_offset_rad + if name in profile.small_offset_joints + else maximum_offset_rad + ), + ) + for name in profile.direct_zero_joints + ], + dtype=float, + ) + # Do not make a configured safety limit the numerical optimizer's bound. + # Otherwise a genuine out-of-range estimate and a modelling failure both + # collapse to exactly +/-20 or +/-3 degrees, which hides the magnitude and + # encourages pointless rescans. Search farther for diagnostics, then keep + # the original configured limits as unchanged pass/fail gates below. + diagnostic_offset_limits = np.minimum( + math.radians(90.0), + np.maximum(3.0 * offset_limits, offset_limits + math.radians(5.0)), + ) + if profile.parallel_root_pattern: + # The absolute rotation of the fitted palm frame about the four + # parallel roll axes is a shared gauge. Each physical finger zero is + # only its deviation from the four-finger median, but the raw scalar + # solves include that common mode. Give those four diagnostic solves + # enough range for the unchanged global common-mode limit plus the + # unchanged per-finger deviation limit; applying the per-finger bound + # before removing the gauge clips every solve to the same value and + # destroys the observable deviations. + for index, name in enumerate(profile.direct_zero_joints): + if name in product_finger_rolls: + diagnostic_offset_limits[index] = min( + math.radians(90.0), + maximum_offset_rad + offset_limits[index], + ) + + return OffsetLimits( + product_finger_rolls=product_finger_rolls, + offset_limits=offset_limits, + diagnostic_offset_limits=diagnostic_offset_limits, + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py new file mode 100644 index 0000000..66f34f5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/residuals.py @@ -0,0 +1,335 @@ +"""CAD observation residuals with explicit, immutable geometry inputs.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from typing import Mapping, Sequence +import math + +from scipy.spatial.transform import Rotation +import numpy as np + +from ...domain.measurement import JointCurveFit +from ...urdf.kinematics import UrdfKinematicModel +from .geometry_helpers import ( + _angles_from_state, + _zero_sensitive_axis_error_rad, + robust_circular_location, +) +from .types import JointAxisMeasurement, PalmOrientationMeasurement, ZeroCalibrationProfile + + +@dataclass(frozen=True) +class ObservationGeometry: + """CAD and frozen observations; no fitted state or acceptance policy.""" + profile: ZeroCalibrationProfile + model: UrdfKinematicModel + curves: Mapping[str, JointCurveFit] + motor_by_joint: Mapping[str, int] + orientation_by_model_cycle: Mapping[tuple[str, int], PalmOrientationMeasurement] + + def for_cycles(self, cycles: Sequence[int]) -> ObservationGeometry: + selected = set(cycles) + return replace(self, orientation_by_model_cycle={ + key: value for key, value in self.orientation_by_model_cycle.items() if key[1] in selected + }) + + def predicted_local( + self, + measurement: JointAxisMeasurement, + offsets: Mapping[str, float], + ) -> tuple[np.ndarray, np.ndarray]: + curves = self.curves + model = self.model + motor_by_joint = self.motor_by_joint + profile = self.profile + angles = _angles_from_state( + ( + measurement.condition_state_u8 + if measurement.condition_command_u8 is None + else measurement.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + return model.axis_line( + measurement.joint, + zero_offsets=offsets, + joint_angles=angles, + ) + + def predicted_palm_orientation_local( + self, + measurement: PalmOrientationMeasurement, + offsets: Mapping[str, float], + ) -> np.ndarray: + curves = self.curves + model = self.model + motor_by_joint = self.motor_by_joint + profile = self.profile + angles = _angles_from_state( + measurement.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + axis, _ = model.axis_line( + measurement.model_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + return axis / np.linalg.norm(axis) + + def phase_error( + self, + predicted_parent_axis: np.ndarray, + predicted_parent_point: np.ndarray, + predicted_child_point: np.ndarray, + observed_parent_axis: np.ndarray, + observed_parent_point: np.ndarray, + observed_child_point: np.ndarray, + view_normal_common: Sequence[float] | None = None, + ) -> float: + profile = self.profile + parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis) + observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis) + if float(parent_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + predicted_delta = predicted_child_point - predicted_parent_point + observed_delta = observed_child_point - observed_parent_point + predicted_radial = predicted_delta - parent_axis * float( + predicted_delta @ parent_axis + ) + observed_radial = observed_delta - observed_axis * float( + observed_delta @ observed_axis + ) + angle_axis = parent_axis + if view_normal_common is not None: + view_normal = np.asarray(view_normal_common, dtype=float) + normal_norm = float(np.linalg.norm(view_normal)) + if normal_norm > 1.0e-9: + view_normal /= normal_norm + # When looking approximately along the rotation axis, image + # x/y contains the complete radial phase while optical depth + # is both unnecessary and much noisier for a 16 mm planar + # Tag. For an edge-on axis the depth component is genuinely + # needed for observability, so retain the full 3-D residual. + if abs(float(parent_axis @ view_normal)) >= math.cos( + math.radians(45.0) + ): + # 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_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: + angle_axis = -angle_axis + predicted_radius = float(np.linalg.norm(predicted_radial)) + observed_radius = float(np.linalg.norm(observed_radial)) + if min(predicted_radius, observed_radius) < 0.003: + raise ValueError("parallel-axis radial phase is not observable") + predicted_radial /= predicted_radius + observed_radial /= observed_radius + return math.atan2( + float(angle_axis @ np.cross(predicted_radial, observed_radial)), + float(np.clip(predicted_radial @ observed_radial, -1.0, 1.0)), + ) + + def same_view_axis_pair_errors( + self, + offset_joint: str, + selected: Sequence[JointAxisMeasurement], + offsets: Mapping[str, float], + ) -> tuple[float, ...]: + """Return the camera-frame-invariant residual for a serial-axis pair. + + This observation intentionally has two mirror roots. The optimiser + disambiguates them with the original cross-view estimate, but the + pair residual alone determines the final numerical zero. + """ + curves = self.curves + model = self.model + motor_by_joint = self.motor_by_joint + orientation_by_model_cycle = self.orientation_by_model_cycle + profile = self.profile + pair = profile.same_view_axis_pair_by_offset.get(offset_joint) + if pair is None: + return () + anchor_joint, observer_joint = pair + + def angle(left: np.ndarray, right: np.ndarray) -> float: + cosine = float(left @ right) / float( + np.linalg.norm(left) * np.linalg.norm(right) + ) + return math.acos(abs(float(np.clip(cosine, -1.0, 1.0)))) + + errors: list[float] = [] + for cycle in sorted({int(item.cycle) for item in selected}): + anchor = orientation_by_model_cycle.get((anchor_joint, cycle)) + observer = orientation_by_model_cycle.get( + (observer_joint, cycle) + ) + if anchor is None or observer is None: + return () + angles = _angles_from_state( + observer.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + predicted_anchor, _ = model.axis_line( + anchor_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + predicted_observer, _ = model.axis_line( + observer_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + observed_anchor = np.asarray( + anchor.axis_common_xyz, dtype=float + ) + observed_observer = np.asarray( + observer.axis_common_xyz, dtype=float + ) + errors.append( + angle(predicted_anchor, predicted_observer) + - angle(observed_anchor, observed_observer) + ) + return tuple(errors) + + def angular_error_samples( + self, + offsets: Mapping[str, float], + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + ) -> dict[str, tuple[float, ...]]: + curves = self.curves + model = self.model + motor_by_joint = self.motor_by_joint + phase_error = self.phase_error + predicted_local = self.predicted_local + profile = self.profile + same_view_axis_pair_errors = self.same_view_axis_pair_errors + by_key = {(item.joint, item.cycle): item for item in selected} + errors: dict[str, list[float]] = { + name: [] for name in profile.direct_zero_joints + } + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + pair_errors = same_view_axis_pair_errors( + offset_joint, selected, offsets + ) + if pair_errors: + errors[offset_joint].extend(pair_errors) + continue + observer_items = [ + item for item in selected if item.joint == observer_joint + ] + if not observer_items: + raise ValueError(f"zero observer is missing: {observer_joint}") + for item in observer_items: + observer_axis, observer_point = predicted_local(item, offsets) + observer_axis = base_rotation.apply(observer_axis) + observer_point = ( + base_rotation.apply(observer_point) + base_translation + ) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + if observer_joint in profile.axis_parent_joint: + parent_joint = profile.axis_parent_joint[observer_joint] + angles = _angles_from_state( + ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + parent_axis, _ = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis = base_rotation.apply(parent_axis) + error = _zero_sensitive_axis_error_rad( + observer_axis, observed_axis, parent_axis + ) + else: + parent_joint = profile.phase_parent_joint[observer_joint] + observed_parent = by_key.get((parent_joint, item.cycle)) + if observed_parent is None: + raise ValueError( + f"phase parent is missing: {parent_joint} cycle {item.cycle}" + ) + angles = _angles_from_state( + ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + parent_axis, parent_point = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis = base_rotation.apply(parent_axis) + parent_point = ( + base_rotation.apply(parent_point) + base_translation + ) + error = phase_error( + parent_axis, + parent_point, + observer_point, + np.asarray(observed_parent.axis_common_xyz, dtype=float), + np.asarray(observed_parent.point_common_xyz_m, dtype=float), + np.asarray(item.point_common_xyz_m, dtype=float), + item.view_normal_common_xyz, + ) + errors[offset_joint].append(float(error)) + return { + name: tuple(values) + for name, values in errors.items() + if values + } + + def angular_errors( + self, + offsets: Mapping[str, float], + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + ) -> dict[str, float]: + angular_error_samples = self.angular_error_samples + return { + name: robust_circular_location(values) + for name, values in angular_error_samples( + offsets, selected, base_rotation, base_translation + ).items() + } diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/rotation_curves.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/rotation_curves.py new file mode 100644 index 0000000..061d526 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/rotation_curves.py @@ -0,0 +1,423 @@ +"""Rotation curves.""" + +from __future__ import annotations + +from typing import Any, Mapping, Sequence +import math + +from scipy.spatial.transform import Rotation +import numpy as np + +from ...domain.measurement import JointCurveFit +from ...geometry.rotation import fit_rotation_axis, robust_rotation_summary +from ..trajectory_geometry import _fit_joint_curve +from .geometry_helpers import ZERO_REFERENCE_MAXIMUM_DISTANCE_U8, _relative_rotation, _vector + + +def _reference_group_key(record: Mapping[str, Any]) -> tuple[Any, Any]: + return record.get("cycle"), record.get("direction") + + +def _canonical_reference_records( + records: Sequence[Mapping[str, Any]], + canonical_zero_direction: str | None, +) -> list[Mapping[str, Any]]: + """Select the sole physical-zero branch when one is configured.""" + if canonical_zero_direction is None: + return list(records) + if canonical_zero_direction not in {"decreasing", "increasing"}: + raise ValueError("canonical zero direction is invalid") + selected = [ + record + for record in records + if str(record.get("direction")) == canonical_zero_direction + ] + if not selected: + raise ValueError( + f"canonical {canonical_zero_direction} zero branch has no samples" + ) + return selected + + +def _spatial_input(record: Mapping[str, Any]) -> float: + """Native input for geometry; the old byte spelling is read-only compatibility.""" + value = float(record["input_value"] if "input_value" in record else record["command_u8"]) + if not math.isfinite(value): + raise ValueError("nonfinite spatial observation input") + return value + + +def _interpolate_reference_rotation( + records: Sequence[Mapping[str, Any]], + zero_command_u8: int, + maximum_distance_u8: int | None = ZERO_REFERENCE_MAXIMUM_DISTANCE_U8, +) -> Rotation | None: + by_command: dict[float, list[np.ndarray]] = {} + for record in records: + command = _spatial_input(record) + by_command.setdefault(command, []).append(_relative_rotation(record)) + if not by_command: + return None + + rotations = { + command: Rotation.from_quat(robust_rotation_summary(values)[0]) + for command, values in by_command.items() + } + zero = float(zero_command_u8) + if zero in rotations: + return rotations[zero] + + lower = [command for command in rotations if command < zero] + upper = [command for command in rotations if command > zero] + lower_command = max(lower) if lower else None + upper_command = min(upper) if upper else None + if lower_command is not None and upper_command is not None: + lower_distance = zero - lower_command + upper_distance = upper_command - zero + if maximum_distance_u8 is None or max( + lower_distance, upper_distance + ) <= float(maximum_distance_u8): + lower_rotation = rotations[lower_command] + upper_rotation = rotations[upper_command] + fraction = lower_distance / (upper_command - lower_command) + delta = (lower_rotation.inv() * upper_rotation).as_rotvec() + return lower_rotation * Rotation.from_rotvec(delta * fraction) + + nearest_command = min(rotations, key=lambda command: abs(command - zero)) + if maximum_distance_u8 is None or abs( + nearest_command - zero + ) <= float(maximum_distance_u8): + return rotations[nearest_command] + return None + + +def _near_zero_records( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> list[Mapping[str, Any]]: + groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} + for record in records: + groups.setdefault(_reference_group_key(record), []).append(record) + selected: list[Mapping[str, Any]] = [] + zero = float(zero_command_u8) + for group in groups.values(): + distance = min(abs(_spatial_input(record) - zero) for record in group) + if "input_value" not in group[0] and distance > ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + continue + selected.extend( + record + for record in group + if abs(_spatial_input(record) - zero) == distance + ) + return selected + + +def _baseline_reference( + 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: + groups.setdefault(_reference_group_key(record), []).append(record) + values = [ + rotation.as_quat() + for group in groups.values() + if ( + rotation := _interpolate_reference_rotation( + group, + zero_command_u8, + maximum_distance_u8, + ) + ) + is not None + ] + if not values: + 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] + + +def baseline_hysteresis_by_cycle_rad( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + axis_xyz: Sequence[float] | None = None, +) -> tuple[float, ...]: + """Return decreasing/increasing joint-angle disagreement per cycle. + + Full SO(3) disagreement includes planar-PnP tilt noise that is orthogonal + to the fitted revolute axis. When an axis is supplied, report only the + physically meaningful component about that axis; the orthogonal component + remains covered by the independent rotation-model residual gate. + """ + samples = [dict(record) for record in records] + axis: np.ndarray | None = None + if axis_xyz is not None: + axis = _vector(axis_xyz, 3, name="baseline hysteresis axis") + norm = float(np.linalg.norm(axis)) + if norm <= 0.0: + raise ValueError("baseline hysteresis axis must be non-zero") + axis /= norm + result: list[float] = [] + for cycle in sorted({int(record["cycle"]) for record in samples}): + rotations: dict[str, Rotation] = {} + for direction in ("decreasing", "increasing"): + selected = [ + record + for record in samples + if int(record["cycle"]) == cycle + and str(record["direction"]) == direction + ] + reference = _interpolate_reference_rotation( + selected, zero_command_u8 + ) + if reference is None: + raise ValueError( + f"cycle {cycle} {direction} is missing baseline samples" + ) + rotations[direction] = reference + delta = ( + rotations["decreasing"].inv() + * rotations["increasing"] + ) + result.append( + float( + delta.magnitude() + if axis is None + else abs(float(delta.as_rotvec() @ axis)) + ) + ) + if not result: + raise ValueError("baseline hysteresis requires at least one cycle") + return tuple(result) + + +def fit_rotation_joint_curve( + records: Sequence[Mapping[str, Any]], + *, + 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: + raise ValueError("rotation trajectory requires at least 12 samples") + vectors: list[np.ndarray] = [] + commands: list[int] = [] + values_by_record: list[float] = [] + # One installation/reference for the entire training session. Recentring + # each cycle would absorb physical Tag slip into nuisance mounting angles. + reference = Rotation.from_quat(_baseline_reference( + _canonical_reference_records(samples, canonical_zero_direction), + zero_command_u8, zero_reference_maximum_distance_u8, + )) + for record in samples: + observed = Rotation.from_quat(_relative_rotation(record)) + vector = ( + reference.inv() * observed + ).as_rotvec() + vectors.append(vector) + commands.append(int(record["command_u8"])) + axis = fit_rotation_axis(vectors, commands) + values_by_record = [float(vector @ axis) for vector in vectors] + curves, correction, hysteresis = _fit_joint_curve( + samples, + values_by_record, + preserve_direction_offset=True, + require_observed_domain_endpoints=require_observed_domain_endpoints, + ) + canonical_key = "angle_rad" if canonical_zero_direction is None else f"{canonical_zero_direction}_rad" + shared_zero = float(curves[canonical_key][int(zero_command_u8)]) + for key in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(curves[key], dtype=float) - shared_zero + curves[key] = [round(float(value), 8) for value in values] + # Keep the stored measurement reference in exactly the same coordinate + # gauge as the shifted curve; do not apply this shift twice at validation. + reference = reference * Rotation.from_rotvec(axis * shared_zero) + if canonical_zero_direction is not None: + # Before the bridge has observed motion direction it must use the + # same branch that defines the URDF physical zero, never an average + # pose that the mechanism may not be able to occupy. + curves["angle_rad"] = list(curves[canonical_key]) + orthogonal = [ + float(np.linalg.norm(vector - float(vector @ axis) * axis)) + for vector in vectors + ] + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple(float(value) for value in curves["decreasing_rad"]), + increasing_rad=tuple(float(value) for value in curves["increasing_rad"]), + circle={ + "space": "relative_rotation_3d", + "axis_xyz": [float(value) for value in axis], + "zero_command_u8": int(zero_command_u8), + "reference_quaternion_xyzw": [ + float(value) for value in reference.as_quat() + ], + "canonical_zero_direction": canonical_zero_direction, + "reference_policy": "training_frozen_installation_v1", + "training_cycles": sorted({int(record["cycle"]) for record in samples}), + }, + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality={ + "rotation_orthogonal_rms_rad": float( + np.sqrt(np.mean(np.square(orthogonal))) + ), + "arc_rad": float( + max(curves["angle_rad"]) - min(curves["angle_rad"]) + ), + }, + ) + + +def measure_rotation_joint_observation( + fit: JointCurveFit, quaternion_xyzw: Sequence[float] +) -> float: + """Measure one parent-to-child orientation with a fitted 3-D curve.""" + if fit.circle.get("space") != "relative_rotation_3d": + raise ValueError("joint fit is not a relative-rotation curve") + reference = Rotation.from_quat( + _vector( + fit.circle["reference_quaternion_xyzw"], + 4, + name="reference quaternion", + ) + ) + observed = Rotation.from_quat( + _vector(quaternion_xyzw, 4, name="observed quaternion") + ) + axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") + axis /= np.linalg.norm(axis) + return float((reference.inv() * observed).as_rotvec() @ axis) + + +def measure_joint_curve_observation( + fit: JointCurveFit, + *, + quaternion_xyzw: Sequence[float] | None = None, + image_relative_xy_px: Sequence[float] | None = None, +) -> float: + """Measure one observation in the same space as its fitted curve.""" + if fit.circle.get("space") == "relative_rotation_3d": + if quaternion_xyzw is None: + raise ValueError("rotation observation quaternion is missing") + return measure_rotation_joint_observation(fit, quaternion_xyzw) + if fit.circle.get("space") != "image_2d": + raise ValueError("unsupported joint curve observation representation") + if image_relative_xy_px is None: + raise ValueError("image curve observation point is missing") + point = np.asarray(image_relative_xy_px, dtype=float) + centre = np.asarray(fit.circle["center_xy_px"], dtype=float) + reference = np.asarray(fit.circle["reference_xy_px"], dtype=float) + vector = point - centre + if ( + point.shape != (2,) + or not np.all(np.isfinite(point)) + or float(np.linalg.norm(vector)) < 1.0e-9 + ): + raise ValueError("image curve observation point is invalid") + return float(fit.circle["orientation_sign"]) * math.atan2( + float(reference[0] * vector[1] - reference[1] * vector[0]), + float(reference @ vector), + ) + + +def rotation_curve_holdout_errors( + fit: JointCurveFit, + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + zero_reference_maximum_distance_u8: int | None = ( + ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 + ), +) -> tuple[float, ...]: + """Validate against the frozen training reference, never refit holdout. + + The distance argument is retained for legacy callers but deliberately + cannot select a new reference from validation samples. + """ + samples = [dict(record) for record in records] + if not samples: + raise ValueError("holdout records are empty") + if int(fit.circle["zero_command_u8"]) != int(zero_command_u8): + raise ValueError("holdout cannot change the fitted zero coordinate") + training_cycles = set(fit.circle.get("training_cycles", ())) + if any(int(row["cycle"]) in training_cycles for row in samples): + raise ValueError("training cycles cannot enter independent holdout") + reference = Rotation.from_quat(_vector( + fit.circle["reference_quaternion_xyzw"], 4, name="frozen reference quaternion")) + axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") + axis /= np.linalg.norm(axis) + errors: list[float] = [] + for record in samples: + observed = Rotation.from_quat(_relative_rotation(record)) + angle = float((reference.inv() * observed).as_rotvec() @ axis) + command = int(record["command_u8"]) + direction = str(record["direction"]) + if not 0 <= command <= 255 or direction not in {"increasing", "decreasing"}: + raise ValueError("holdout SDK input or direction is invalid") + expected_curve = ( + fit.decreasing_rad + if direction == "decreasing" + else fit.increasing_rad + ) + expected = float(expected_curve[command]) + predicted = reference * Rotation.from_rotvec(axis * expected) + magnitude = float((predicted.inv() * observed).magnitude()) + # Include off-axis installation slip without losing the historical + # signed phase-error diagnostic used by reports. + errors.append(math.copysign(magnitude, angle - expected)) + return tuple(errors) + + +def joint_curve_holdout_errors( + fit: JointCurveFit, + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, +) -> tuple[float, ...]: + """Validate either a rotation or fixed-parent image-circle curve.""" + if fit.circle.get("space") == "relative_rotation_3d": + return rotation_curve_holdout_errors( + fit, records, zero_command_u8=zero_command_u8 + ) + if fit.circle.get("space") != "image_2d": + raise ValueError("unsupported joint curve holdout representation") + if not records: + raise ValueError("holdout records are empty") + errors: list[float] = [] + for record in records: + observed = measure_joint_curve_observation( + fit, + image_relative_xy_px=record["image_relative_xy_px"], + ) + command = int(record["command_u8"]) + direction = str(record["direction"]) + expected_curve = ( + fit.decreasing_rad + if direction == "decreasing" + else fit.increasing_rad + ) + errors.append(observed - float(expected_curve[command])) + return tuple(errors) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py new file mode 100644 index 0000000..85974f3 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/solve.py @@ -0,0 +1,103 @@ +"""Public spatial zero pipeline with frozen training and independent validation.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Mapping, Sequence +import math + +from ...domain.measurement import JointCurveFit +from .acceptance import assemble_result, decide_acceptance +from .optimization import solve_selected +from .preparation import prepare_offset_limits, prepare_zero_problem +from .residuals import ObservationGeometry +from .stages import TrainingFit, TrainingProblem, ZeroSolveOptions +from .statistics import estimate_cycle_evidence, estimate_observability +from .types import ( + JointAxisMeasurement, + PalmOrientationMeasurement, + ZeroCalibrationProfile, + ZeroSolveResult, +) +from .validation import check_observation_geometry, validate_axis_lines, validate_holdout + + +def solve_urdf_zero_offsets( + *, + source_urdf: str | Path, + measurements: Sequence[JointAxisMeasurement], + palm_orientation_measurements: Sequence[PalmOrientationMeasurement] = (), + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + training_cycles: Sequence[int] = (0, 1), + validation_cycle: int = 2, + maximum_offset_rad: float = math.radians(20.0), + finger_maximum_offset_rad: float = math.radians(3.0), + joint_maximum_offset_rad: Mapping[str, float] | None = None, + maximum_cycle_difference_rad: float = math.radians(0.75), + minimum_applied_offset_rad: float = math.radians(0.3), + significance_sigma: float = 3.0, + maximum_validation_mae_rad: float = math.radians(1.0), + maximum_validation_p95_rad: float = math.radians(2.0), + maximum_validation_error_rad: float | None = None, + maximum_confidence_half_width_rad: float | None = None, + maximum_axis_cone_mismatch_rad: float = math.radians(5.0), + maximum_systematic_axis_cone_bias_rad: float | None = None, + maximum_pose_axis_line_rms_m: float = 0.001, + maximum_observability_condition_number: float = 10000000000.0, + hand_type: str | None = None, + tag_layout: str | None = None, + fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, + static_output_zero_offsets_rad: Mapping[str, float] | None = None, + zero_profile: ZeroCalibrationProfile | None = None, +) -> ZeroSolveResult: + """Prepare, fit training only, validate the frozen result, then assemble.""" + options = ZeroSolveOptions( + maximum_offset_rad=maximum_offset_rad, + finger_maximum_offset_rad=finger_maximum_offset_rad, + joint_maximum_offset_rad=joint_maximum_offset_rad, + maximum_cycle_difference_rad=maximum_cycle_difference_rad, + minimum_applied_offset_rad=minimum_applied_offset_rad, + significance_sigma=significance_sigma, + maximum_validation_mae_rad=maximum_validation_mae_rad, + maximum_validation_p95_rad=maximum_validation_p95_rad, + maximum_validation_error_rad=maximum_validation_error_rad, + maximum_confidence_half_width_rad=maximum_confidence_half_width_rad, + maximum_axis_cone_mismatch_rad=maximum_axis_cone_mismatch_rad, + maximum_systematic_axis_cone_bias_rad=maximum_systematic_axis_cone_bias_rad, + maximum_pose_axis_line_rms_m=maximum_pose_axis_line_rms_m, + maximum_observability_condition_number=maximum_observability_condition_number, + ) + problem = prepare_zero_problem( + options=options, + source_urdf=source_urdf, + measurements=measurements, + palm_orientation_measurements=palm_orientation_measurements, + training_cycles=training_cycles, + validation_cycle=validation_cycle, + hand_type=hand_type, + tag_layout=tag_layout, + fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets_rad, + static_output_zero_offsets_rad=static_output_zero_offsets_rad, + zero_profile=zero_profile, + ) + geometry = ObservationGeometry(problem.profile, problem.model, curves, motor_by_joint, problem.orientation_by_model_cycle) + training_geometry = geometry.for_cycles(training_cycles) + training_problem = TrainingProblem(problem.profile, problem.training, problem.fixed_offsets, problem.zero_offsets) + limits = prepare_offset_limits(problem=problem, options=options) + rotation, translation, offsets = solve_selected( + problem=training_problem, limits=limits, geometry=training_geometry, selected=problem.training) + fit = TrainingFit(rotation, translation, offsets) + observability = estimate_observability(problem=training_problem, fit=fit, geometry=training_geometry) + geometry_evidence = check_observation_geometry(problem=problem, options=options, + geometry=geometry, selected=list(measurements), offsets=offsets, rotation=rotation) + cycles = estimate_cycle_evidence(problem=training_problem, limits=limits, fit=fit, options=options, + geometry=training_geometry, measurements=problem.training, training_cycles=training_cycles) + holdout = validate_holdout(problem=problem, fit=fit, cycles=cycles, options=options, + geometry=geometry, measurements=measurements) + lines = validate_axis_lines(problem=problem, fit=fit, cycles=cycles, geometry=geometry) + decision = decide_acceptance(problem=problem, limits=limits, fit=fit, options=options, + observability=observability, geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout) + return assemble_result(problem=problem, limits=limits, fit=fit, observability=observability, + geometry_evidence=geometry_evidence, cycles=cycles, holdout=holdout, lines=lines, + decision=decision, validation_cycle=validation_cycle) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/stages.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/stages.py new file mode 100644 index 0000000..e9f4cc9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/stages.py @@ -0,0 +1,121 @@ +"""Explicit inputs and evidence passed between spatial solution stages.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Mapping, Sequence + +from scipy.spatial.transform import Rotation +import numpy as np + +from ...urdf.kinematics import UrdfKinematicModel +from .types import JointAxisMeasurement, PalmOrientationMeasurement, ZeroCalibrationProfile + + +@dataclass(frozen=True) +class ZeroProblem: + profile: ZeroCalibrationProfile + model: UrdfKinematicModel + training: Sequence[JointAxisMeasurement] + validation: Sequence[JointAxisMeasurement] + configured_palm_sources: Mapping[str, str] + orientation_validation: Sequence[PalmOrientationMeasurement] + orientation_by_model_cycle: Mapping[tuple[str, int], PalmOrientationMeasurement] + joint_limits: Mapping[str, float] + fixed_offsets: Mapping[str, float] + output_offsets: Mapping[str, float] + zero_offsets: Mapping[str, float] + + +@dataclass(frozen=True) +class TrainingProblem: + """Training-only view: the optimizer cannot access holdout observations.""" + + profile: ZeroCalibrationProfile + training: Sequence[JointAxisMeasurement] + fixed_offsets: Mapping[str, float] + zero_offsets: Mapping[str, float] + + +@dataclass(frozen=True) +class OffsetLimits: + product_finger_rolls: tuple[str, ...] + offset_limits: np.ndarray + diagnostic_offset_limits: np.ndarray + + +@dataclass(frozen=True) +class TrainingFit: + base_rotation: Rotation + base_translation: np.ndarray + training_offsets: Mapping[str, float] + + +@dataclass(frozen=True) +class ObservabilityEvidence: + observability_rank: int + observability_parameter_count: int + observability_condition_number: float + offset_covariance: Mapping[str, float] + + +@dataclass(frozen=True) +class GeometryEvidence: + observation_failures: Mapping[str, str] + axis_cone_mismatch_by_joint: Mapping[str, float] + axis_cone_bias_classification_by_joint: Mapping[str, str] + + +@dataclass(frozen=True) +class CycleEvidence: + cycle_values: Mapping[str, Sequence[float]] + cycle_models: Mapping[int, tuple[Rotation, np.ndarray]] + training_cycle_ids: tuple[int, ...] + uncertainties: Mapping[str, float] + confidence_half_widths: Mapping[str, float] + cycle_consistent: bool + inconsistent_cycles: Sequence[str] + insignificant_large: Sequence[str] + applied_training: Mapping[str, float] + + +@dataclass(frozen=True) +class HoldoutEvidence: + validation_error_by_joint: Mapping[str, float] + original_error_by_joint: Mapping[str, float] + improvement_by_joint: Mapping[str, float] + improvement_confidence_lower: Mapping[str, float] + improvement_passed: bool + palm_orientation_validation_passed: bool + validation_errors: np.ndarray + + +@dataclass(frozen=True) +class LineEvidence: + validation_line_error_by_joint: Mapping[str, float] + axis_line_rms: float + + +@dataclass(frozen=True) +class AcceptanceDecision: + finger_roll_common_mode: float + failure_reasons: Mapping[str, str] + passed: bool + + +@dataclass(frozen=True) +class ZeroSolveOptions: + maximum_offset_rad: float + finger_maximum_offset_rad: float + joint_maximum_offset_rad: Mapping[str, float] | None + maximum_cycle_difference_rad: float + minimum_applied_offset_rad: float + significance_sigma: float + maximum_validation_mae_rad: float + maximum_validation_p95_rad: float + maximum_validation_error_rad: float | None + maximum_confidence_half_width_rad: float | None + maximum_axis_cone_mismatch_rad: float + maximum_systematic_axis_cone_bias_rad: float | None + maximum_pose_axis_line_rms_m: float + maximum_observability_condition_number: float diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py new file mode 100644 index 0000000..6ad3f2d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/statistics.py @@ -0,0 +1,249 @@ +"""Training observability, cycle repeatability and offset confidence.""" + +from __future__ import annotations + +from functools import partial +from typing import Sequence +import math + +from scipy.spatial.transform import Rotation +from scipy.stats import t as student_t +import numpy as np + +from .optimization import optimise_offsets +from .residuals import ObservationGeometry +from .stages import ( + CycleEvidence, + ObservabilityEvidence, + OffsetLimits, + TrainingFit, + TrainingProblem, + ZeroSolveOptions, +) +from .types import JointAxisMeasurement + + +def observability_residual( + parameters: np.ndarray, + *, + problem: TrainingProblem, + geometry: ObservationGeometry, +): + profile = problem.profile + training = problem.training + predicted_local = geometry.predicted_local + + rotation = Rotation.from_rotvec(parameters[:3]) + translation = parameters[3:6] * 0.05 + offsets = { + name: float(value) + for name, value in zip( + profile.direct_zero_joints, parameters[6:] + ) + } + residuals: list[float] = [] + for item in training: + predicted_axis, predicted_point = predicted_local(item, offsets) + predicted_axis = rotation.apply(predicted_axis) + predicted_axis /= np.linalg.norm(predicted_axis) + predicted_point = rotation.apply(predicted_point) + translation + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + observed_axis /= np.linalg.norm(observed_axis) + if float(predicted_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + observed_point = np.asarray(item.point_common_xyz_m, dtype=float) + predicted_moment = np.cross(predicted_point, predicted_axis) + observed_moment = np.cross(observed_point, observed_axis) + residuals.extend(float(value) for value in predicted_axis - observed_axis) + residuals.extend( + float(value) / 0.05 + for value in predicted_moment - observed_moment + ) + return np.asarray(residuals, dtype=float) + + +def estimate_observability( + *, + fit: TrainingFit, + problem: TrainingProblem, + geometry: ObservationGeometry, +) -> ObservabilityEvidence: + base_rotation = fit.base_rotation + base_translation = fit.base_translation + profile = problem.profile + training_offsets = fit.training_offsets + + residual = partial(observability_residual, problem=problem, geometry=geometry) + observability_parameters = np.asarray( + [ + *base_rotation.as_rotvec(), + *(base_translation / 0.05), + *( + training_offsets[name] + for name in profile.direct_zero_joints + ), + ], + dtype=float, + ) + observability_base_residual = residual( + observability_parameters + ) + observability_jacobian = np.empty( + ( + observability_base_residual.size, + observability_parameters.size, + ), + dtype=float, + ) + finite_difference_step = 1.0e-6 + for column in range(observability_parameters.size): + positive = observability_parameters.copy() + negative = observability_parameters.copy() + positive[column] += finite_difference_step + negative[column] -= finite_difference_step + observability_jacobian[:, column] = ( + residual(positive) + - residual(negative) + ) / (2.0 * finite_difference_step) + singular_values = np.linalg.svd( + observability_jacobian, compute_uv=False + ) + singular_threshold = ( + 0.0 + if singular_values.size == 0 + else float(singular_values[0]) * 1.0e-7 + ) + observability_rank = int( + np.count_nonzero(singular_values > singular_threshold) + ) + observability_parameter_count = int(observability_parameters.size) + observability_condition_number = ( + float("inf") + if observability_rank < observability_parameter_count + else float(singular_values[0] / singular_values[-1]) + ) + residual_dof = max( + 1, + observability_base_residual.size - observability_parameter_count, + ) + residual_variance = float( + observability_base_residual @ observability_base_residual + ) / residual_dof + covariance = residual_variance * np.linalg.pinv( + observability_jacobian.T @ observability_jacobian, + rcond=1.0e-12, + ) + offset_covariance = { + name: max(0.0, float(covariance[6 + index, 6 + index])) + for index, name in enumerate(profile.direct_zero_joints) + } + + return ObservabilityEvidence( + observability_rank=observability_rank, + observability_parameter_count=observability_parameter_count, + observability_condition_number=observability_condition_number, + offset_covariance=offset_covariance, + ) + + +def estimate_cycle_evidence( + measurements: Sequence[JointAxisMeasurement], + training_cycles: Sequence[int], + *, + limits: OffsetLimits, + fit: TrainingFit, + options: ZeroSolveOptions, + problem: TrainingProblem, + geometry: ObservationGeometry, +) -> CycleEvidence: + base_rotation = fit.base_rotation + base_translation = fit.base_translation + fixed_offsets = problem.fixed_offsets + maximum_confidence_half_width_rad = options.maximum_confidence_half_width_rad + maximum_cycle_difference_rad = options.maximum_cycle_difference_rad + minimum_applied_offset_rad = options.minimum_applied_offset_rad + profile = problem.profile + significance_sigma = options.significance_sigma + training_offsets = fit.training_offsets + + fit_offsets = partial(optimise_offsets, geometry=geometry, problem=problem, limits=limits) + cycle_values: dict[str, list[float]] = { + name: [] for name in profile.direct_zero_joints + } + cycle_models: dict[int, tuple[Rotation, np.ndarray]] = {} + training_cycle_ids = tuple(sorted({int(value) for value in training_cycles})) + for cycle in training_cycle_ids: + selected = [item for item in measurements if item.cycle == cycle] + # Keep one palm pose while comparing cycles. Refitting a base pose from + # only two nearly parallel root axes per cycle makes harmless root-line + # noise appear as a large encoder-zero change. + cycle_rotation, cycle_translation = base_rotation, base_translation + cycle_offsets = fit_offsets( + selected, + cycle_rotation, + cycle_translation, + initial=training_offsets, + ) + cycle_models[cycle] = (cycle_rotation, cycle_translation) + for name, value in cycle_offsets.items(): + cycle_values[name].append(value) + uncertainties: dict[str, float] = {} + confidence_half_widths: dict[str, float] = {} + cycle_consistent = True + inconsistent_cycles: list[str] = [] + for name, values in cycle_values.items(): + array = np.asarray(values, dtype=float) + spread = float(np.max(array) - np.min(array)) + if spread > maximum_cycle_difference_rad: + cycle_consistent = False + inconsistent_cycles.append(name) + uncertainties[name] = ( + 0.0 + if array.size < 2 + else float(np.std(array, ddof=1) / math.sqrt(array.size)) + ) + confidence_half_widths[name] = ( + float("inf") + if array.size < 2 + else float( + student_t.ppf(0.975, df=array.size - 1) + * uncertainties[name] + ) + ) + + insignificant_large: list[str] = [] + applied_training: dict[str, float] = {} + for name, value in training_offsets.items(): + if name in fixed_offsets: + applied_training[name] = fixed_offsets[name] + continue + uncertainty = uncertainties[name] + confidence_half_width = confidence_half_widths[name] + if ( + abs(value) < minimum_applied_offset_rad + or abs(value) + <= max(significance_sigma * uncertainty, confidence_half_width) + ): + applied_training[name] = 0.0 + 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 + + return CycleEvidence( + cycle_values=cycle_values, + cycle_models=cycle_models, + training_cycle_ids=training_cycle_ids, + uncertainties=uncertainties, + confidence_half_widths=confidence_half_widths, + cycle_consistent=cycle_consistent, + inconsistent_cycles=inconsistent_cycles, + insignificant_large=insignificant_large, + applied_training=applied_training, + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py new file mode 100644 index 0000000..140da8c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/types.py @@ -0,0 +1,175 @@ +"""Types.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Mapping, Protocol, Sequence + + +class SpatialHandGeometry(Protocol): + side: str + layout_id: str + active_joints: Sequence[str] + reference_finger: str + palm_orientation_sources: Mapping[str, str] + minimum_palm_orientation_sources: int + stable_cross_view_cone_bias: bool + + +@dataclass(frozen=True) +class ZeroCalibrationProfile: + hand: SpatialHandGeometry + direct_zero_joints: tuple[str, ...] + axis_joints: tuple[str, ...] + # Dynamic command-angle curves may be shared by unobserved fingers. This + # mapping is intentionally *not* an authorization to copy an absolute + # encoder/URDF zero between independent motors. + inherited_zero_joints: Mapping[str, str] + # Absolute static-zero inheritance requires an independent mechanical + # guarantee. With the current 11-Tag layout only the reference finger is + # observed, so the conservative mapping is empty and unobserved active + # finger origins retain their source-CAD zero. + inherited_static_zero_joints: Mapping[str, str] + constrained_circle_joints: frozenset[str] + root_anchor_joints: frozenset[str] + axis_parent_joint: Mapping[str, str] + phase_parent_joint: Mapping[str, str] + offset_observer_joint: Mapping[str, str] + # Some serial offsets can be observed from the angle between two axes + # captured by the same camera. The mapping is geometric topology only; + # it never contains a model- or serial-specific zero value. + same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]] + fixed_direct_zero_offsets_rad: Mapping[str, float] + static_output_zero_offsets_rad: Mapping[str, float] + # Product full-hand fitting uses the five root-axis line pattern. The + # thumb kernel instead uses only the serial thumb chain, so its nuisance + # palm pose cannot be influenced by finger observations. + base_pose_strategy: str = "full_hand" + # A serial-chain model may use a separate palm-root joint axis to fix the + # otherwise free rotation about its primary root axis. G20 retains its + # historical defaults; other model profiles can name the physical anchor + # explicitly without introducing model-specific branches in the solver. + orientation_anchor_joint: str | None = None + # A model whose feedback direction is mechanically reviewed may use the + # 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 + + parallel_root_pattern: bool = False + common_mode_zero_joints: tuple[str, ...] = () + small_offset_joints: frozenset[str] = frozenset() + + @property + def reference_finger(self) -> str: + return self.hand.reference_finger + + +@dataclass(frozen=True) +class JointAxisMeasurement: + joint: str + cycle: int + axis_common_xyz: tuple[float, float, float] + point_common_xyz_m: tuple[float, float, float] + condition_state_u8: tuple[float, ...] + plane_rms_m: float + radial_rms_m: float + rotation_circle_axis_difference_rad: float + # Optical-axis direction expressed in the shared calibration frame. It + # lets the phase solver discard the least reliable monocular-PnP depth + # component when the joint axis is viewed approximately end-on. Older + # recordings and synthetic callers may omit it and retain the 3-D path. + view_normal_common_xyz: tuple[float, float, float] | None = None + # Logical commands that produced the condition. Firmware feedback may + # saturate a few u8 short of an endpoint (for example right motor 10 reads + # 250 for command 255); kinematics must use the command-indexed curve while + # retaining condition_state_u8 for diagnostics and safety. + condition_command_u8: tuple[float, ...] | None = None + axis_direction_source: str = "unspecified" + 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 + # clear the actual quality source instead of blindly rescanning ``joint``. + pose_axis_line_source_joints: tuple[str, ...] = () + # Camera centre that observed ``point_common_xyz_m``. When populated, + # the zero solver can use only the ray from this centre to the fitted axis + # point. That ray is the depth-free interpretation-plane observation of + # the physical axis; translating a monocular planar-PnP solution along + # its optical ray therefore cannot rotate the recovered palm frame. + axis_point_camera_center_common_xyz_m: ( + tuple[float, float, float] | None + ) = None + # Normal of the source camera's interpretation plane for this axis line. + # Unlike a 3-D PnP line point, the plane is unchanged by optical-depth + # error. Several named parallel root lines jointly recover their common + # physical direction as the null direction of these plane normals. + axis_point_interpretation_plane_normal_common_xyz: ( + tuple[float, float, float] | None + ) = None + + +@dataclass(frozen=True) +class PalmOrientationMeasurement: + """Direction-only joint observation from a partially visible sweep. + + The moving Tag may disappear before the motor reaches its far endpoint. + Only the relative SO(3) trajectory is retained, so fixed Tag translation + and mounting rotation cannot define the palm phase. + """ + + source_joint: str + model_joint: str + cycle: int + axis_common_xyz: tuple[float, float, float] + condition_state_u8: tuple[float, ...] + observed_arc_rad: float + rotation_orthogonal_rms_rad: float + axis_estimator: str = "baseline_relative_so3" + incremental_pair_count: int = 0 + + +@dataclass(frozen=True) +class ZeroSolveResult: + direct_offsets_rad: Mapping[str, float] + all_active_offsets_rad: Mapping[str, float] + base_translation_xyz_m: tuple[float, float, float] + base_quaternion_xyzw: tuple[float, float, float, float] + validation_errors_rad: tuple[float, ...] + validation_error_by_joint_rad: Mapping[str, float] + validation_line_error_by_joint_m: Mapping[str, float] + axis_line_rms_m: float + passed: bool + cycle_offsets_rad: Mapping[str, tuple[float, ...]] + offset_uncertainty_rad: Mapping[str, float] + offset_confidence_half_width_rad: Mapping[str, float] + training_cycles: tuple[int, ...] + validation_cycle: int + validation_original_error_by_joint_rad: Mapping[str, float] + validation_improvement_by_joint_rad: Mapping[str, float] + validation_improvement_confidence_lower_rad: Mapping[str, float] + observability_rank: int + observability_parameter_count: int + observability_condition_number: float + offset_covariance_rad2: Mapping[str, float] + 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 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py new file mode 100644 index 0000000..42b0bcb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/spatial_solver/validation.py @@ -0,0 +1,403 @@ +"""Independent holdout and geometric consistency checks of the frozen fit.""" + +from __future__ import annotations + +from typing import Mapping, Sequence +import math + +from scipy.spatial.transform import Rotation +from scipy.stats import t as student_t +import numpy as np + +from .geometry_helpers import _angles_from_state, _axis_cone_mismatch_rad +from .residuals import ObservationGeometry +from .stages import ( + CycleEvidence, + GeometryEvidence, + HoldoutEvidence, + LineEvidence, + TrainingFit, + ZeroProblem, + ZeroSolveOptions, +) +from .types import JointAxisMeasurement + + +def check_observation_geometry( + selected: Sequence[JointAxisMeasurement], + offsets: Mapping[str, float], + rotation: Rotation, + *, + options: ZeroSolveOptions, + problem: ZeroProblem, + geometry: ObservationGeometry, +): + fixed_offsets = problem.fixed_offsets + maximum_axis_cone_mismatch_rad = options.maximum_axis_cone_mismatch_rad + maximum_confidence_half_width_rad = options.maximum_confidence_half_width_rad + maximum_cycle_difference_rad = options.maximum_cycle_difference_rad + maximum_pose_axis_line_rms_m = options.maximum_pose_axis_line_rms_m + maximum_systematic_axis_cone_bias_rad = options.maximum_systematic_axis_cone_bias_rad + model = problem.model + profile = problem.profile + curves = geometry.curves + motor_by_joint = geometry.motor_by_joint + + axis_cone_mismatch_by_joint: dict[str, float] = {} + axis_cone_bias_classification_by_joint: dict[str, str] = {} + """Reject repeatable but geometrically inadmissible zero observers.""" + by_key = {(item.joint, item.cycle): item for item in selected} + failures: dict[str, str] = {} + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if offset_joint in fixed_offsets: + continue + observer_items = [ + item for item in selected if item.joint == observer_joint + ] + if observer_joint in profile.axis_parent_joint: + parent_joint = profile.axis_parent_joint[observer_joint] + cone_mismatches: list[float] = [] + for item in observer_items: + state = ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ) + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=( + profile.inherited_zero_joints + ), + ) + predicted_axis, _ = model.axis_line( + observer_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis, _ = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + cone_mismatches.append( + _axis_cone_mismatch_rad( + rotation.apply(predicted_axis), + item.axis_common_xyz, + rotation.apply(parent_axis), + ) + ) + maximum_cone_mismatch = max(cone_mismatches) + axis_cone_mismatch_by_joint[offset_joint] = ( + maximum_cone_mismatch + ) + if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: + cone_range = float(np.ptp(cone_mismatches)) + stable_product_bias = bool( + profile.hand.stable_cross_view_cone_bias + and maximum_systematic_axis_cone_bias_rad is not None + and len(cone_mismatches) >= 4 + and maximum_cone_mismatch + <= maximum_systematic_axis_cone_bias_rad + and cone_range <= maximum_cycle_difference_rad + ) + if stable_product_bias: + # A parent zero rotates the downstream direction + # around the parent axis and cannot change their cone + # angle. The zero-sensitive residual above already + # projects both directions onto the parent-normal + # plane, so a repeatable cross-camera/planar-PnP cone + # bias cannot corrupt the written encoder zero. Keep + # it visible in the result while retaining the gross + # gate for a wrong axis, loose Tag, or moved camera. + axis_cone_bias_classification_by_joint[offset_joint] = ( + "stable_cross_view_or_planar_pnp_bias" + ) + else: + failures[offset_joint] = ( + "zero_axis_cone_mismatch_too_large" + ) + else: + parent_joint = profile.phase_parent_joint[observer_joint] + phase_items: list[JointAxisMeasurement] = [] + propagated_angle_uncertainties: list[float] = [] + for item in observer_items: + phase_items.append(item) + parent_item = by_key.get((parent_joint, item.cycle)) + if parent_item is not None: + phase_items.append(parent_item) + parent_axis = np.asarray( + parent_item.axis_common_xyz, dtype=float + ) + parent_axis /= np.linalg.norm(parent_axis) + separation = ( + np.asarray(item.point_common_xyz_m, dtype=float) + - np.asarray( + parent_item.point_common_xyz_m, dtype=float + ) + ) + radial = separation - parent_axis * float( + separation @ parent_axis + ) + effective_distance = float(np.linalg.norm(radial)) + if effective_distance <= 1.0e-6: + propagated_angle_uncertainties.append(float("inf")) + else: + line_uncertainty = math.hypot( + item.pose_axis_line_rms_m, + parent_item.pose_axis_line_rms_m, + ) + propagated_angle_uncertainties.append( + math.atan2( + line_uncertainty, effective_distance + ) + ) + if profile.parallel_root_pattern: + angle_limit = ( + maximum_confidence_half_width_rad + if maximum_confidence_half_width_rad is not None + else math.radians(0.5) + ) + if ( + not propagated_angle_uncertainties + or max(propagated_angle_uncertainties) > angle_limit + ): + failures[offset_joint] = ( + "zero_phase_axis_line_angle_uncertainty_too_large" + ) + elif ( + not profile.parallel_root_pattern + and any( + item.pose_axis_line_rms_m + > maximum_pose_axis_line_rms_m + for item in phase_items + ) + ): + failures[offset_joint] = ( + "zero_phase_axis_line_residual_too_large" + ) + return GeometryEvidence(failures, axis_cone_mismatch_by_joint, axis_cone_bias_classification_by_joint) + + +def validate_holdout( + measurements: Sequence[JointAxisMeasurement], + *, + cycles: CycleEvidence, + fit: TrainingFit, + options: ZeroSolveOptions, + problem: ZeroProblem, + geometry: ObservationGeometry, +) -> HoldoutEvidence: + applied_training = cycles.applied_training + base_rotation = fit.base_rotation + base_translation = fit.base_translation + configured_palm_sources = problem.configured_palm_sources + cycle_models = cycles.cycle_models + fixed_offsets = problem.fixed_offsets + maximum_axis_cone_mismatch_rad = options.maximum_axis_cone_mismatch_rad + maximum_validation_error_rad = options.maximum_validation_error_rad + orientation_validation = problem.orientation_validation + profile = problem.profile + training_cycle_ids = cycles.training_cycle_ids + validation = problem.validation + zero_offsets = problem.zero_offsets + angular_errors = geometry.angular_errors + predicted_palm_orientation_local = geometry.predicted_palm_orientation_local + same_view_axis_pair_errors = geometry.same_view_axis_pair_errors + + candidate_errors = angular_errors( + applied_training, validation, base_rotation, base_translation + ) + original_errors = angular_errors( + zero_offsets, validation, base_rotation, base_translation + ) + validation_error_by_joint: dict[str, float] = {} + original_error_by_joint: dict[str, float] = {} + improvement_by_joint: dict[str, float] = {} + improvement_confidence_lower: dict[str, float] = {} + improvement_passed = True + if profile.same_view_axis_pair_by_offset: + # Validate the same camera/Tag-mount-invariant axis-pair angle used by + # the configured offset. This keeps the side channel out of every + # other joint, including the already stable thumb CMC roll solve. + palm_orientation_validation_errors = tuple( + abs(float(error)) + for offset_joint in profile.same_view_axis_pair_by_offset + for error in same_view_axis_pair_errors( + offset_joint, validation, applied_training + ) + ) + else: + palm_orientation_validation_errors = tuple( + math.acos( + abs( + float( + np.clip( + base_rotation.apply( + predicted_palm_orientation_local( + item, zero_offsets + ) + ) + @ np.asarray(item.axis_common_xyz, dtype=float), + -1.0, + 1.0, + ) + ) + ) + ) + for item in orientation_validation + ) + palm_orientation_validation_limit = ( + maximum_validation_error_rad + if maximum_validation_error_rad is not None + else maximum_axis_cone_mismatch_rad + ) + palm_orientation_validation_passed = bool( + not configured_palm_sources + or ( + palm_orientation_validation_errors + and float(np.median(palm_orientation_validation_errors)) + <= palm_orientation_validation_limit + ) + ) + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if offset_joint in fixed_offsets: + improvement_by_joint[offset_joint] = 0.0 + improvement_confidence_lower[offset_joint] = 0.0 + continue + candidate = abs(candidate_errors[offset_joint]) + original = abs(original_errors[offset_joint]) + validation_error_by_joint[observer_joint] = candidate + original_error_by_joint[observer_joint] = original + improvement_by_joint[offset_joint] = original - candidate + if applied_training[offset_joint] != 0.0 and not candidate < original: + improvement_passed = False + for offset_joint in profile.direct_zero_joints: + if offset_joint in fixed_offsets: + improvement_confidence_lower[offset_joint] = 0.0 + continue + if applied_training[offset_joint] == 0.0: + improvement_confidence_lower[offset_joint] = 0.0 + continue + cycle_improvements: list[float] = [] + for cycle in training_cycle_ids: + selected = [item for item in measurements if item.cycle == cycle] + cycle_rotation, cycle_translation = cycle_models[cycle] + candidate = abs( + angular_errors( + applied_training, + selected, + cycle_rotation, + cycle_translation, + )[offset_joint] + ) + original = abs( + angular_errors( + zero_offsets, + selected, + cycle_rotation, + cycle_translation, + )[offset_joint] + ) + cycle_improvements.append(original - candidate) + improvement_array = np.asarray(cycle_improvements, dtype=float) + improvement_se = ( + float("inf") + if improvement_array.size < 2 + else float( + np.std(improvement_array, ddof=1) + / math.sqrt(improvement_array.size) + ) + ) + lower = ( + float("-inf") + if not math.isfinite(improvement_se) + else float( + np.mean(improvement_array) + - student_t.ppf( + 0.975, df=improvement_array.size - 1 + ) + * improvement_se + ) + ) + improvement_confidence_lower[offset_joint] = lower + if lower <= 0.0: + improvement_passed = False + + validation_errors = np.asarray( + list(validation_error_by_joint.values()), dtype=float + ) + + return HoldoutEvidence( + validation_error_by_joint=validation_error_by_joint, + original_error_by_joint=original_error_by_joint, + improvement_by_joint=improvement_by_joint, + improvement_confidence_lower=improvement_confidence_lower, + improvement_passed=improvement_passed, + palm_orientation_validation_passed=palm_orientation_validation_passed, + validation_errors=validation_errors, + ) + + +def validate_axis_lines( + *, + cycles: CycleEvidence, + fit: TrainingFit, + problem: ZeroProblem, + geometry: ObservationGeometry, +) -> LineEvidence: + applied_training = cycles.applied_training + base_rotation = fit.base_rotation + base_translation = fit.base_translation + validation = problem.validation + predicted_local = geometry.predicted_local + + validation_line_samples: dict[str, list[float]] = {} + for item in validation: + predicted_axis, predicted_point = predicted_local( + item, applied_training + ) + predicted_axis = base_rotation.apply(predicted_axis) + predicted_axis /= np.linalg.norm(predicted_axis) + predicted_point = ( + base_rotation.apply(predicted_point) + base_translation + ) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + observed_axis /= np.linalg.norm(observed_axis) + observed_point = np.asarray(item.point_common_xyz_m, dtype=float) + cross = np.cross(predicted_axis, observed_axis) + cross_norm = float(np.linalg.norm(cross)) + separation = observed_point - predicted_point + line_error = ( + abs(float(separation @ cross)) / cross_norm + if cross_norm > 1.0e-6 + else float( + np.linalg.norm( + separation + - predicted_axis * float(separation @ predicted_axis) + ) + ) + ) + validation_line_samples.setdefault(item.joint, []).append(line_error) + validation_line_error_by_joint = { + name: float(np.sqrt(np.mean(np.square(values)))) + for name, values in validation_line_samples.items() + } + all_validation_line_errors = np.asarray( + [ + value + for values in validation_line_samples.values() + for value in values + ], + dtype=float, + ) + axis_line_rms = ( + float("inf") + if all_validation_line_errors.size == 0 + else float( + np.sqrt(np.mean(np.square(all_validation_line_errors))) + ) + ) + + return LineEvidence(validation_line_error_by_joint=validation_line_error_by_joint, axis_line_rms=axis_line_rms) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py new file mode 100644 index 0000000..2f4f3ab --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/tag_installation.py @@ -0,0 +1,158 @@ +"""One training-frozen Tag-to-link installation per physical Tag. + +This is not a zero solver. The CAD zero/base registration must already have +independent observability evidence. No holdout observation participates in +installation fitting, and no installation is fitted again for another task. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Mapping, Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..geometry.rotation import robust_rotation_summary +from ..urdf.kinematics import UrdfKinematicModel + + +def rigid_matrix(value) -> np.ndarray: + result = np.asarray(value, dtype=float) + if result.shape != (4, 4) or not np.all(np.isfinite(result)): + raise ValueError("Tag/base pose must be a finite rigid transform") + r = result[:3, :3] + if not np.allclose(result[3], (0, 0, 0, 1), atol=1e-9) or not np.allclose(r.T @ r, np.eye(3), atol=1e-6) or not np.isclose(np.linalg.det(r), 1, atol=1e-6): + raise ValueError("Tag/base pose must be a rigid transform") + return result + + +def matrix_tuple(value) -> tuple[tuple[float, ...], ...]: + return tuple(tuple(float(v) for v in row) for row in rigid_matrix(value)) + + +def link_pose(model: UrdfKinematicModel, link: str, angles: Mapping[str, float]) -> np.ndarray: + if link == model.root_link: + return np.eye(4) + if link not in model.parent_joint_by_child: + raise ValueError(f"Tag installation references a missing link:{link}") + return model.link_transform(model.parent_joint_by_child[link], + zero_offsets={}, joint_angles=angles, independent_mimic_angles=True) + + +@dataclass(frozen=True) +class TagTrainingPose: + sample_id: str + role: str + cycle: int + common_from_tag: tuple[tuple[float, ...], ...] + # Explicit CAD coordinates, prepared from the fitted measurement result, + # NOT obtained by reading a serializer's just-written JSON/URDF. + cad_angles: Mapping[str, float] + + +@dataclass(frozen=True) +class FrozenTagInstallation: + role: str + link: str + link_from_tag: tuple[tuple[float, ...], ...] + training_sample_ids: tuple[str, ...] + training_rotation_p95_deg: float + training_translation_p95_m: float + + +def fit_tag_installations( + *, source_model: UrdfKinematicModel, common_from_base, + link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose], + minimum_samples: int = 40, +) -> dict[str, FrozenTagInstallation]: + """Fit mounting transforms once across all tasks and three cycles. + + Repeated records of one image/Tag are deduplicated only when their pose + and joint conditions agree. Conflicting duplicates are invalid evidence. + Training residuals check rigid installation, not a per-frame PnP gate. + """ + base_from_common = np.linalg.inv(rigid_matrix(common_from_base)) + grouped: dict[str, dict[str, tuple[int, np.ndarray]]] = {role: {} for role in link_by_role} + for row in observations: + if row.cycle not in {0, 1, 2} or not row.sample_id: + raise ValueError("only identified training images may fit Tag installations") + if row.role not in link_by_role: + raise ValueError(f"undeclared Tag installation:{row.role}") + candidate = np.linalg.inv(link_pose(source_model, link_by_role[row.role], row.cad_angles)) @ base_from_common @ rigid_matrix(row.common_from_tag) + previous = grouped[row.role].get(row.sample_id) + if previous is not None and (previous[0] != row.cycle or not np.allclose(previous[1], candidate, atol=1e-8, rtol=0)): + raise ValueError(f"conflicting duplicate Tag image:{row.role}:{row.sample_id}") + grouped[row.role][row.sample_id] = (row.cycle, candidate) + output = {} + for role, unique in grouped.items(): + if len(unique) < minimum_samples or {value[0] for value in unique.values()} != {0, 1, 2}: + raise ValueError(f"Tag installation lacks three-cycle training evidence:{role}") + matrices = np.asarray([value[1] for value in unique.values()]) + rotation = Rotation.from_quat(robust_rotation_summary( + Rotation.from_matrix(matrices[:, :3, :3]).as_quat().tolist())[0]) + translation = np.median(matrices[:, :3, 3], axis=0) + errors = (rotation.inv() * Rotation.from_matrix(matrices[:, :3, :3])).magnitude() + distances = np.linalg.norm(matrices[:, :3, 3] - translation, axis=1) + rotation_p95 = float(np.rad2deg(np.percentile(errors, 95))) + translation_p95 = float(np.percentile(distances, 95)) + if rotation_p95 > 2.0 or translation_p95 > 0.003: + raise ValueError(f"Tag installation is not rigid across training tasks:{role}:rotation_p95_deg={rotation_p95}:translation_p95_m={translation_p95}") + transform = np.eye(4) + transform[:3, :3], transform[:3, 3] = rotation.as_matrix(), translation + output[role] = FrozenTagInstallation(role, link_by_role[role], matrix_tuple(transform), + tuple(sorted(unique)), rotation_p95, translation_p95) + return output + + +def register_base_translation( + *, source_model: UrdfKinematicModel, common_from_base, + link_by_role: Mapping[str, str], observations: Sequence[TagTrainingPose], +) -> tuple[tuple[float, ...], ...]: + """Register translation jointly with constant mounts, training only. + + The axis/zero solver owns orientation and observable joint zeros. Its + monocular line-point translation can contain a depth gauge. Full rigid + trajectories remove that gauge through p_tag = p_base + R_base*p_link + + R_base*R_link*p_mount. No extra joint zero or CAD geometry is estimated. + """ + base = rigid_matrix(common_from_base).copy() + roles = tuple(sorted(link_by_role)) + slots = {role: 3 + 3*index for index, role in enumerate(roles)} + count = 3 + 3*len(roles) + matrices, rhs = [], [] + seen = set() + for row in observations: + if row.cycle not in {0, 1, 2} or not row.sample_id or row.role not in slots: + raise ValueError("base registration requires identified training Tag poses") + identity = (row.role, row.sample_id) + if identity in seen: + raise ValueError("duplicate base-registration image") + seen.add(identity) + link = link_pose(source_model, link_by_role[row.role], row.cad_angles) + matrix = np.zeros((3, count)) + matrix[:, :3] = np.eye(3) + index = slots[row.role] + matrix[:, index:index+3] = base[:3, :3] @ link[:3, :3] + matrices.append(matrix) + rhs.append(rigid_matrix(row.common_from_tag)[:3, 3] - base[:3, :3] @ link[:3, 3]) + if not matrices: + raise ValueError("base registration has no training poses") + a, b = np.vstack(matrices), np.concatenate(rhs) + estimate, _, rank, singular = np.linalg.lstsq(a, b, rcond=None) + if rank != count or singular[-1] / singular[0] < 1e-8: + raise ValueError("base translation/mounts are not jointly observable") + # Robust weights act on entire 3-D observations; no axis is selectively + # discarded to hide a bad CAD/mount fit. + for _ in range(8): + residual = (a @ estimate - b).reshape(-1, 3) + distance = np.linalg.norm(residual, axis=1) + scale = max(0.0005, 1.4826*float(np.median(distance))) + weights = np.repeat(np.sqrt(np.minimum(1.0, scale / np.maximum(distance, 1e-12))), 3) + updated = np.linalg.lstsq(a*weights[:, None], b*weights, rcond=None)[0] + if np.linalg.norm(updated - estimate) < 1e-10: + estimate = updated + break + estimate = updated + base[:3, 3] = estimate[:3] + return matrix_tuple(base) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/trajectory_geometry.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/trajectory_geometry.py new file mode 100644 index 0000000..a02bbca --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/trajectory_geometry.py @@ -0,0 +1,356 @@ +"""Shared point-trajectory and legacy byte-domain numerical primitives.""" +from __future__ import annotations +import math +from typing import Any, Mapping, Sequence +import numpy as np +from scipy.optimize import least_squares +from ..domain.task import DIRECTION_DECREASING, DIRECTION_INCREASING, DIRECTIONS +from .curve import isotonic_nonincreasing + +def _vector3(value: Sequence[float], *, name: str) -> np.ndarray: + vector = np.asarray(value, dtype=float) + if vector.shape != (3,) or not np.all(np.isfinite(vector)): + raise ValueError(f"{name} must contain three finite values") + return vector + + +def _plane_basis(axis: Sequence[float]) -> tuple[np.ndarray, np.ndarray]: + normal = _vector3(axis, name="axis") + normal /= np.linalg.norm(normal) + candidates = np.eye(3) + seed = candidates[int(np.argmin(np.abs(candidates @ normal)))] + first = np.cross(normal, seed) + first /= np.linalg.norm(first) + second = np.cross(normal, first) + second /= np.linalg.norm(second) + return first, second + + +def _fit_plane_axis(point_sets: Sequence[np.ndarray]) -> tuple[np.ndarray, float]: + centred: list[np.ndarray] = [] + for points in point_sets: + array = np.asarray(points, dtype=float) + if array.ndim != 2 or array.shape[1] != 3 or len(array) < 3: + raise ValueError("each trajectory must contain at least three 3-D points") + # The arithmetic centroid of points in a plane remains in that plane + # and transforms correctly under a camera rotation. A component-wise + # median generally does neither. + centred.append(array - np.mean(array, axis=0)) + matrix = np.concatenate(centred, axis=0) + _, _, vh = np.linalg.svd(matrix, full_matrices=False) + axis = vh[-1] + axis /= np.linalg.norm(axis) + residuals = matrix @ axis + plane_rms = float(np.sqrt(np.mean(np.square(residuals)))) + return axis, plane_rms + + +def _fit_circle_with_axis( + points_xyz: Sequence[Sequence[float]], + axis_xyz: Sequence[float], +) -> dict[str, Any]: + points = np.asarray(points_xyz, dtype=float) + if ( + points.ndim != 2 + or points.shape[1] != 3 + or len(points) < 6 + or not np.all(np.isfinite(points)) + ): + raise ValueError("a circle requires at least six finite 3-D points") + axis = _vector3(axis_xyz, name="circle axis") + axis /= np.linalg.norm(axis) + basis_x, basis_y = _plane_basis(axis) + origin = np.mean(points, axis=0) + local = points - origin + xy = np.column_stack((local @ basis_x, local @ basis_y)) + + design = np.column_stack((2.0 * xy[:, 0], 2.0 * xy[:, 1], np.ones(len(xy)))) + target = np.sum(np.square(xy), axis=1) + initial_x, initial_y, constant = np.linalg.lstsq( + design, target, rcond=None + )[0] + initial_radius = math.sqrt( + max( + float(constant + initial_x * initial_x + initial_y * initial_y), + np.finfo(float).eps, + ) + ) + + def residual(parameters: np.ndarray) -> np.ndarray: + centre = parameters[:2] + radius = float(parameters[2]) + return np.linalg.norm(xy - centre, axis=1) - radius + + fitted = least_squares( + residual, + np.asarray([initial_x, initial_y, initial_radius], dtype=float), + loss="soft_l1", + f_scale=0.0005, + max_nfev=2000, + ) + centre_xy = fitted.x[:2] + radius = abs(float(fitted.x[2])) + radial_residuals = residual( + np.asarray([centre_xy[0], centre_xy[1], radius], dtype=float) + ) + centre_xyz = origin + centre_xy[0] * basis_x + centre_xy[1] * basis_y + plane_offsets = (points - centre_xyz) @ axis + centre_xyz += float(np.median(plane_offsets)) * axis + plane_residuals = (points - centre_xyz) @ axis + return { + "axis_xyz": [float(value) for value in axis], + "center_xyz_m": [float(value) for value in centre_xyz], + "radius_m": radius, + "radial_rms_m": float( + np.sqrt(np.mean(np.square(radial_residuals))) + ), + "plane_rms_m": float( + np.sqrt(np.mean(np.square(plane_residuals))) + ), + } + + +def _project_radial( + point_xyz: Sequence[float], + circle: Mapping[str, Any], +) -> np.ndarray: + point = _vector3(point_xyz, name="trajectory point") + centre = _vector3(circle["center_xyz_m"], name="circle centre") + axis = _vector3(circle["axis_xyz"], name="circle axis") + axis /= np.linalg.norm(axis) + radial = point - centre + radial -= float(radial @ axis) * axis + if float(np.linalg.norm(radial)) < 1.0e-9: + raise ValueError("trajectory point lies on the fitted rotation axis") + return radial + + +def _signed_angle( + reference_radial_xyz: Sequence[float], + observed_radial_xyz: Sequence[float], + axis_xyz: Sequence[float], +) -> float: + reference = _vector3(reference_radial_xyz, name="reference radial") + observed = _vector3(observed_radial_xyz, name="observed radial") + axis = _vector3(axis_xyz, name="angle axis") + axis /= np.linalg.norm(axis) + reference -= float(reference @ axis) * axis + observed -= float(observed @ axis) * axis + reference /= np.linalg.norm(reference) + observed /= np.linalg.norm(observed) + return math.atan2( + float(axis @ np.cross(reference, observed)), + float(np.clip(reference @ observed, -1.0, 1.0)), + ) + + +def _reference_radial( + records: Sequence[Mapping[str, Any]], + points: Sequence[np.ndarray], + circle: Mapping[str, Any], +) -> np.ndarray: + references = [ + _project_radial(point, circle) + for record, point in zip(records, points) + if int(record.get("command_u8", -1)) == 255 + ] + if not references: + raise ValueError("trajectory is missing command-255 reference points") + radial = np.median(np.asarray(references, dtype=float), axis=0) + axis = _vector3(circle["axis_xyz"], name="circle axis") + axis /= np.linalg.norm(axis) + radial -= float(radial @ axis) * axis + if float(np.linalg.norm(radial)) < 1.0e-9: + raise ValueError("command-255 reference is degenerate") + return radial + + +def _angle_for_circle( + point_xyz: Sequence[float], + circle: Mapping[str, Any], +) -> float: + return _signed_angle( + circle["reference_radial_xyz_m"], + _project_radial(point_xyz, circle), + circle["axis_xyz"], + ) + + +def _low_command_median( + records: Sequence[Mapping[str, Any]], + values: Sequence[float], +) -> float: + selected = [ + float(value) + for record, value in zip(records, values) + if int(record.get("command_u8", 255)) <= 16 + ] + if not selected: + selected = [ + float(value) + for _, value in sorted( + zip(records, values), + key=lambda item: int(item[0].get("command_u8", 255)), + )[: max(3, len(records) // 20)] + ] + return float(np.median(selected)) + + +def _orient_circle_positive( + circle: dict[str, Any], + records: Sequence[Mapping[str, Any]], + points: Sequence[np.ndarray], +) -> dict[str, Any]: + reference = _reference_radial(records, points, circle) + circle["reference_radial_xyz_m"] = [ + float(value) for value in reference + ] + values = [_angle_for_circle(point, circle) for point in points] + if _low_command_median(records, values) < 0.0: + axis = -_vector3(circle["axis_xyz"], name="circle axis") + circle["axis_xyz"] = [float(value) for value in axis] + values = [_angle_for_circle(point, circle) for point in points] + circle["observed_arc_rad"] = float(max(values) - min(values)) + return circle + + +def _fit_joint_curve( + records: Sequence[Mapping[str, Any]], + values: Sequence[float], + *, + 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 + } + for record, value in zip(records, values): + direction = str(record["direction"]) + command = int(record["command_u8"]) + by_direction[direction][command].append(float(value)) + + fitted: dict[str, list[float]] = {} + maximum_correction = 0.0 + for direction in DIRECTIONS: + commands = np.asarray( + [ + command + for command, samples in enumerate(by_direction[direction]) + if samples + ], + dtype=int, + ) + 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" + ) + raw = np.asarray( + [ + float(np.median(by_direction[direction][command])) + for command in commands + ], + dtype=float, + ) + if not preserve_direction_offset: + raw -= raw[-1] + projected_samples = isotonic_nonincreasing(raw) + if not preserve_direction_offset: + projected_samples -= projected_samples[-1] + maximum_correction = max( + maximum_correction, + float(np.max(np.abs(projected_samples - raw))), + ) + curve = np.interp( + np.arange(256, dtype=float), + commands.astype(float), + projected_samples, + ) + if not preserve_direction_offset: + curve -= curve[255] + if endpoint_reference is not None: + curve = _regularize_coupled_zero_tail( + curve, + endpoint_reference[f"{direction}_rad"], + ) + fitted[direction] = [ + round(float(value), 8) for value in curve + ] + + decreasing = np.asarray(fitted[DIRECTION_DECREASING], dtype=float) + increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float) + hysteresis = float(np.max(np.abs(decreasing - increasing))) + combined = 0.5 * (decreasing + increasing) + if not preserve_direction_offset: + combined -= combined[255] + return ( + { + "angle_rad": [round(float(value), 8) for value in combined], + "decreasing_rad": fitted[DIRECTION_DECREASING], + "increasing_rad": fitted[DIRECTION_INCREASING], + }, + maximum_correction, + hysteresis, + ) + + +def _regularize_coupled_zero_tail( + values: Sequence[float], + reference_values: Sequence[float], + *, + maximum_tail_commands: int = 16, + zero_tolerance_rad: float = 1.0e-10, +) -> np.ndarray: + """Replace a short noise-created zero tail using a coupled joint shape. + + The passive IP and active MCP share motor 15. Close to command 255 the + IP centre trajectory is small enough that measurement noise can become + negative. Isotonic projection correctly prevents a negative angle, but + otherwise turns all remaining commands into an artificial zero plateau. + Continue the last resolved IP/MCP ratio over a short tail instead. This + changes neither the resolved part of the curve nor the exact 255 zero. + """ + curve = np.asarray(values, dtype=float).copy() + reference = np.asarray(reference_values, dtype=float) + if curve.shape != (256,) or reference.shape != (256,): + raise ValueError("endpoint curves must each contain 256 values") + if not np.all(np.isfinite(curve)) or not np.all(np.isfinite(reference)): + raise ValueError("endpoint curves must be finite") + maximum_tail = int(maximum_tail_commands) + tolerance = float(zero_tolerance_rad) + if maximum_tail < 2 or tolerance < 0.0: + raise ValueError("invalid endpoint regularization settings") + + anchor = 254 + while anchor >= 0 and abs(float(curve[anchor])) <= tolerance: + anchor -= 1 + tail_commands = 255 - anchor + if ( + anchor < 0 + or tail_commands < 2 + or tail_commands > maximum_tail + or curve[anchor] <= tolerance + or reference[anchor] <= tolerance + or abs(float(curve[255])) > tolerance + or abs(float(reference[255])) > tolerance + ): + curve[255] = 0.0 + return curve + + ratio = float(curve[anchor] / reference[anchor]) + continuation = np.maximum( + 0.0, + ratio * reference[anchor + 1 :], + ) + continuation = np.minimum.accumulate(continuation) + continuation = np.minimum(continuation, float(curve[anchor])) + continuation[-1] = 0.0 + curve[anchor + 1 :] = continuation + return curve + diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py similarity index 76% rename from src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py rename to src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py index 35d6c7e..7b01aec 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/observations.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/candidate_selection.py @@ -1,4 +1,4 @@ -"""Offline thumb candidate resolution using training data only. +"""Articulated Tag 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. @@ -9,10 +9,9 @@ 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 +from .pnp import solve_square_tag_ippe -POLICY = 'o12_thumb_training_candidates_v1' +POLICY = 'training_frozen_articulated_candidates_v1' MATRIX_SOURCE = 'CameraInfo.P[:3,:3]' @@ -42,7 +41,8 @@ def _trajectory_score(parent, child): 'score': rms/.0015 + off_axis/math.radians(1)} -def resolve_thumb_observations(records, *, projection_override=None): +def resolve_chain_observations(records, *, task_key, role_pairs, + projection_override=None, solve_pose=solve_square_tag_ippe): """Return copied observations and auditable candidate-selection evidence. Older captures did not save P. They remain on the legacy path; K is never @@ -50,11 +50,14 @@ def resolve_thumb_observations(records, *, projection_override=None): 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'] + roles = tuple(dict.fromkeys(role for pair in role_pairs.values() for role in pair)) + if not role_pairs or len(roles) > 8 or any(len(pair) != 2 or pair[0] == pair[1] for pair in role_pairs.values()): + raise ValueError('candidate selection requires a bounded articulated observation graph') + joint_rows = [r for r in rows if r.get('joint') in role_pairs + and r.get('task_name') == task_key] 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'} + if ('roles' in r or str(r.get('kind', '')).endswith('pnp_candidate_frame')) + and r.get('task_name') == task_key} report = {'policy': POLICY, 'training_cycles': [0, 1, 2], 'holdout_cycle': 3, 'projection_override_used': projection_override is not None, 'is_accuracy_certificate': False} @@ -72,23 +75,23 @@ def resolve_thumb_observations(records, *, projection_override=None): 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') + raise ValueError('corner replay lacks evidence for accepted observations') training = np.asarray([int(evidence[s]['cycle']) in (0, 1, 2) for s in stamps]) if sum(training) < 40 or not any(int(evidence[s]['cycle']) == 3 for s in stamps): - raise ValueError('O12 corner replay requires training and independent holdout') + raise ValueError('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} + raise ValueError('holdout must follow the complete training trajectory') + paths = {role: [[], []] for role in 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: + for role in roles: data = frame['roles'][role] - poses = solve_square_tag_ippe(data['corners_xy'], tag_size_m=frame['tag_size_m'], camera_matrix=matrix) + poses = solve_pose(data['corners_xy'], tag_size_m=data.get('tag_size_m', frame['tag_size_m']), camera_matrix=matrix) limit = float(data.get('maximum_reprojection_error_px', 1.5)) poses = [p for p in poses if p.reprojection_error_px <= limit] if not poses: - raise ValueError(f'O12 corner replay has no eligible pose:{stamp}:{role}') + raise ValueError(f'corner replay has no eligible pose:{stamp}:{role}') cs = [dict(quaternion_xyzw=list(p.quaternion_xyzw), translation_xyz_m=list(p.translation_xyz_m), reprojection_error_px=p.reprojection_error_px) for p in poses[:2]] if len(cs) == 1: @@ -102,20 +105,20 @@ def resolve_thumb_observations(records, *, projection_override=None): 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])) + for branches in product(range(2), repeat=len(roles)): + train = {role: [p for p, keep in zip(paths[role][branch], training) if keep] + for role, branch in zip(roles, branches)} + metrics = [_trajectory_score(train[parent], train[child]) for parent, child in role_pairs.values()] + image_penalty = float(np.mean([p['reprojection_error_px'] for ps in train.values() 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'])} + lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(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_role, child_role = role_pairs[row['joint']] 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'] @@ -133,7 +136,7 @@ def resolve_thumb_observations(records, *, projection_override=None): 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' + row['projection_reprocessing_scope'] = 'partial_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/core/geometry/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py index 9ee5c91..6986ab3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py @@ -1,1751 +1,18 @@ -"""Square AprilTag pose estimation with planar ambiguity tracking. - -The AprilTag detections contain accurately refined image corners. This module -uses OpenCV's IPPE square solver directly so the calibration node can inspect -both planar PnP solutions instead of accepting an occasionally flipped TF -pose. -""" - -from __future__ import annotations - -from dataclasses import dataclass, replace -from itertools import product -import math -from typing import Mapping, Sequence - -import cv2 -import numpy as np -from scipy.spatial.transform import Rotation - - -@dataclass(frozen=True) -class SquareTagPose: - """One tag-to-camera pose candidate returned by IPPE.""" - - quaternion_xyzw: tuple[float, float, float, float] - translation_xyz_m: tuple[float, float, float] - reprojection_error_px: float - - -def _relative_pose( - parent: SquareTagPose, - child: SquareTagPose, -) -> tuple[Rotation, np.ndarray]: - parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) - child_rotation = Rotation.from_quat(child.quaternion_xyzw) - relative_rotation = parent_rotation.inv() * child_rotation - relative_translation = parent_rotation.inv().apply( - np.asarray(child.translation_xyz_m, dtype=float) - - np.asarray(parent.translation_xyz_m, dtype=float) - ) - return relative_rotation, relative_translation - - -def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float: - """Return the undirected angle between two observed Tag face normals.""" - first_normal = Rotation.from_quat(first.quaternion_xyzw).apply( - [0.0, 0.0, 1.0] - ) - second_normal = Rotation.from_quat(second.quaternion_xyzw).apply( - [0.0, 0.0, 1.0] - ) - return math.acos( - abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0))) - ) - - -def select_rigid_group_trajectory( - frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], - *, - roles: Sequence[str], - fixed_pairs: Sequence[tuple[str, str]], - reprojection_scale_px: float, - rotation_scale_rad: float, - translation_scale_m: float, - pair_geometry: str = "pose", - normal_alignment_pairs: Sequence[tuple[str, str]] = (), - normal_alignment_scale_rad: float | None = None, -) -> tuple[ - list[dict[str, SquareTagPose]], - dict[str, float | str], -]: - """Resolve planar branches using geometry that should stay rigid. - - Every possible branch combination in the first frame is treated as a - candidate rigid reference. For each such reference, every later frame - independently chooses the combination with the lowest reprojection plus - geometric-drift cost. ``pose`` compares relative rotation and translation; - ``distance`` compares only Euclidean centre distances and therefore does - not allow planar-PnP orientation jitter into centre-trajectory angles. - The globally cheapest reference and path win. - """ - role_names = tuple(str(role) for role in roles) - pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) - normal_pair_names = tuple( - (str(first), str(second)) - for first, second in normal_alignment_pairs - ) - if not frames: - raise ValueError("at least one PnP frame is required") - if len(set(role_names)) != len(role_names) or not role_names: - raise ValueError("roles must be non-empty and unique") - if any( - parent not in role_names or child not in role_names - for parent, child in (*pair_names, *normal_pair_names) - ): - raise ValueError("geometry pairs must reference roles") - reprojection_scale = float(reprojection_scale_px) - rotation_scale = float(rotation_scale_rad) - translation_scale = float(translation_scale_m) - geometry_mode = str(pair_geometry) - normal_scale = ( - rotation_scale - if normal_alignment_scale_rad is None - else float(normal_alignment_scale_rad) - ) - if min( - reprojection_scale, - rotation_scale, - translation_scale, - normal_scale, - ) <= 0.0: - raise ValueError("trajectory selection scales must be positive") - if geometry_mode not in {"pose", "distance"}: - raise ValueError("pair_geometry must be pose or distance") - - combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] - for frame in frames: - candidate_lists = [tuple(frame.get(role, ())) for role in role_names] - if any(not candidates for candidates in candidate_lists): - raise ValueError("every frame must contain every requested role") - combinations_by_frame.append( - [ - dict(zip(role_names, combination)) - for combination in product(*candidate_lists) - ] - ) - - best_total = float("inf") - best_path: list[dict[str, SquareTagPose]] | None = None - - def emission( - combination: Mapping[str, SquareTagPose], - reference_pairs: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray] - ], - reference_distances: Mapping[tuple[str, str], float], - ) -> tuple[float, float, float]: - reprojection_cost = sum( - pose.reprojection_error_px - for pose in combination.values() - ) / reprojection_scale - normal_alignment_cost = sum( - _normal_alignment_rad( - combination[first], combination[second] - ) - for first, second in normal_pair_names - ) / normal_scale - rotation_drifts: list[float] = [] - translation_drifts: list[float] = [] - distance_drifts: list[float] = [] - for pair, ( - reference_rotation, - reference_translation, - ) in reference_pairs.items(): - rotation, translation = _relative_pose( - combination[pair[0]], - combination[pair[1]], - ) - rotation_drifts.append( - float( - (reference_rotation.inv() * rotation).magnitude() - ) - ) - translation_drifts.append( - float( - np.linalg.norm( - translation - reference_translation - ) - ) - ) - current_distance = float( - np.linalg.norm( - np.asarray( - combination[pair[1]].translation_xyz_m, - dtype=float, - ) - - np.asarray( - combination[pair[0]].translation_xyz_m, - dtype=float, - ) - ) - ) - distance_drifts.append( - abs(current_distance - reference_distances[pair]) - ) - if geometry_mode == "distance": - geometry_cost = sum(distance_drifts) / translation_scale - else: - geometry_cost = ( - sum(rotation_drifts) / rotation_scale - + sum(translation_drifts) / translation_scale - ) - return ( - reprojection_cost + geometry_cost + normal_alignment_cost, - max(rotation_drifts, default=0.0), - ( - max(distance_drifts, default=0.0) - if geometry_mode == "distance" - else max(translation_drifts, default=0.0) - ), - ) - - def transition_cost( - previous: Mapping[str, SquareTagPose], - current: Mapping[str, SquareTagPose], - ) -> float: - rotation_motion = sum( - rotation_distance_rad( - previous[role].quaternion_xyzw, - current[role].quaternion_xyzw, - ) - for role in role_names - ) - translation_motion = sum( - float( - np.linalg.norm( - np.asarray(current[role].translation_xyz_m) - - np.asarray(previous[role].translation_xyz_m) - ) - ) - for role in role_names - ) - if geometry_mode == "distance": - return translation_motion / translation_scale - return ( - rotation_motion / rotation_scale - + translation_motion / translation_scale - ) - - for reference_index, reference_combination in enumerate( - combinations_by_frame[0] - ): - reference_pairs = { - pair: _relative_pose( - reference_combination[pair[0]], - reference_combination[pair[1]], - ) - for pair in pair_names - } - reference_distances = { - pair: float( - np.linalg.norm( - np.asarray( - reference_combination[pair[1]].translation_xyz_m, - dtype=float, - ) - - np.asarray( - reference_combination[pair[0]].translation_xyz_m, - dtype=float, - ) - ) - ) - for pair in pair_names - } - first_emission = emission( - reference_combination, - reference_pairs, - reference_distances, - ) - previous_costs = np.full( - len(combinations_by_frame[0]), - np.inf, - dtype=float, - ) - previous_costs[reference_index] = first_emission[0] - back_pointers: list[list[int]] = [] - for frame_index in range(1, len(combinations_by_frame)): - previous_combinations = combinations_by_frame[frame_index - 1] - combinations = combinations_by_frame[frame_index] - frame_emissions = [ - emission( - combination, - reference_pairs, - reference_distances, - ) - for combination in combinations - ] - current_costs = np.full(len(combinations), np.inf, dtype=float) - frame_back_pointers: list[int] = [] - for current_index, combination in enumerate(combinations): - transition_costs = [ - previous_costs[previous_index] - + transition_cost( - previous_combination, - combination, - ) - for previous_index, previous_combination in enumerate( - previous_combinations - ) - ] - best_previous = int(np.argmin(transition_costs)) - frame_back_pointers.append(best_previous) - current_costs[current_index] = ( - transition_costs[best_previous] - + frame_emissions[current_index][0] - ) - back_pointers.append(frame_back_pointers) - previous_costs = current_costs - - final_index = int(np.argmin(previous_costs)) - total = float(previous_costs[final_index]) - path_indices = [final_index] - for frame_back_pointers in reversed(back_pointers): - path_indices.append( - frame_back_pointers[path_indices[-1]] - ) - path_indices.reverse() - path = [ - combinations[index] - for combinations, index in zip( - combinations_by_frame, - path_indices, - ) - ] - if total < best_total: - best_total = total - best_path = path - - if best_path is None: - raise RuntimeError("trajectory branch selection produced no path") - - # The marker-to-marker mounting transforms are unknown, so the rigid - # reference must be estimated from the complete sweep. Using frame zero - # as both the optimisation seed and the reported quality reference made - # one noisy endpoint frame look like drift in every other frame. A - # rotation medoid and component-wise translation median are insensitive - # to that endpoint noise while still exposing a persistent mirror branch. - robust_reference_pairs: dict[ - tuple[str, str], tuple[Rotation, np.ndarray] - ] = {} - for pair in pair_names: - pair_poses = [ - _relative_pose(frame[pair[0]], frame[pair[1]]) - for frame in best_path - ] - pair_rotations = [pose[0] for pose in pair_poses] - angular_costs = np.asarray( - [ - sum( - float((candidate.inv() * other).magnitude()) - for other in pair_rotations - ) - for candidate in pair_rotations - ], - dtype=float, - ) - rotation_medoid = pair_rotations[int(np.argmin(angular_costs))] - translation_median = np.median( - np.asarray([pose[1] for pose in pair_poses], dtype=float), - axis=0, - ) - robust_reference_pairs[pair] = ( - rotation_medoid, - translation_median, - ) - - rotation_drifts_by_frame: list[float] = [] - translation_drifts_by_frame: list[float] = [] - pair_distances_by_pair = { - pair: np.asarray( - [ - np.linalg.norm( - np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) - - np.asarray( - frame[pair[0]].translation_xyz_m, dtype=float - ) - ) - for frame in best_path - ], - dtype=float, - ) - for pair in pair_names - } - robust_pair_distances = { - pair: float(np.median(distances)) - for pair, distances in pair_distances_by_pair.items() - } - distance_drifts_by_frame: list[float] = [] - for frame in best_path: - frame_rotation_drifts: list[float] = [] - frame_translation_drifts: list[float] = [] - frame_distance_drifts: list[float] = [] - for pair, ( - reference_rotation, - reference_translation, - ) in robust_reference_pairs.items(): - rotation, translation = _relative_pose( - frame[pair[0]], frame[pair[1]] - ) - frame_rotation_drifts.append( - float((reference_rotation.inv() * rotation).magnitude()) - ) - frame_translation_drifts.append( - float(np.linalg.norm(translation - reference_translation)) - ) - distance = float( - np.linalg.norm( - np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) - - np.asarray( - frame[pair[0]].translation_xyz_m, dtype=float - ) - ) - ) - frame_distance_drifts.append( - abs(distance - robust_pair_distances[pair]) - ) - rotation_drifts_by_frame.append( - max(frame_rotation_drifts, default=0.0) - ) - translation_drifts_by_frame.append( - max(frame_translation_drifts, default=0.0) - ) - distance_drifts_by_frame.append( - max(frame_distance_drifts, default=0.0) - ) - - rotation_drifts = np.asarray(rotation_drifts_by_frame, dtype=float) - translation_drifts = np.asarray( - translation_drifts_by_frame, dtype=float - ) - distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float) - normal_alignments = np.asarray( - [ - max( - ( - _normal_alignment_rad(frame[first], frame[second]) - for first, second in normal_pair_names - ), - default=0.0, - ) - for frame in best_path - ], - dtype=float, - ) - return best_path, { - "total_cost": float(best_total), - "pair_geometry": geometry_mode, - "maximum_normal_alignment_rad": float( - np.max(normal_alignments, initial=0.0) - ), - "maximum_pair_rotation_drift_rad": float( - np.max(rotation_drifts, initial=0.0) - ), - "p95_pair_rotation_drift_rad": float( - np.percentile(rotation_drifts, 95.0) - ), - "median_pair_rotation_drift_rad": float( - np.median(rotation_drifts) - ), - "maximum_pair_translation_drift_m": float( - np.max(translation_drifts, initial=0.0) - ), - "p95_pair_translation_drift_m": float( - np.percentile(translation_drifts, 95.0) - ), - "maximum_pair_distance_drift_m": float( - np.max(distance_drifts, initial=0.0) - ), - "p95_pair_distance_drift_m": float( - np.percentile(distance_drifts, 95.0) - ), - "median_pair_distance_drift_m": float( - np.median(distance_drifts) - ), - } - - -def select_static_rigid_group_initialization( - frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], - *, - roles: Sequence[str], - fixed_pairs: Sequence[tuple[str, str]], - reprojection_scale_px: float, - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - relative_rotation_scale_rad: float, - relative_translation_scale_m: float, - normal_alignment_pairs: Sequence[tuple[str, str]] = (), - normal_alignment_scale_rad: float = math.radians(5.0), - task_reference_pairs: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray] - ] | None = None, - task_reference_rotation_scale_rad: float = math.radians(1.0), - task_reference_translation_scale_m: float = 0.01, -) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]: - """Select a static multi-Tag IPPE branch path in bounded time. - - Group initialization is performed while the hand is held at an endpoint, - so every frame should describe the same camera and relative Tag poses. - Enumerating a full Viterbi transition matrix for every possible first-frame - branch is therefore unnecessary: with four two-branch Tags and eight - frames it performs more than one hundred thousand scipy rotations and can - block the live ROS callback for several seconds. - - Instead, treat every first-frame combination as a possible static - reference and independently select the closest combination in each later - frame. This preserves the multi-frame rigidity and normal-alignment - evidence while changing the search from O(F*C^2*C0) to O(F*C*C0). - """ - role_names = tuple(str(role) for role in roles) - pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) - normal_pair_names = tuple( - (str(first), str(second)) for first, second in normal_alignment_pairs - ) - task_references = dict(task_reference_pairs or {}) - if not frames: - raise ValueError("at least one PnP frame is required") - if not role_names or len(set(role_names)) != len(role_names): - raise ValueError("roles must be non-empty and unique") - if any( - parent not in role_names or child not in role_names - for parent, child in ( - *pair_names, - *normal_pair_names, - *task_references, - ) - ): - raise ValueError("geometry pairs must reference roles") - scales = ( - float(reprojection_scale_px), - float(maximum_pose_jump_rad), - float(maximum_translation_jump_m), - float(relative_rotation_scale_rad), - float(relative_translation_scale_m), - float(normal_alignment_scale_rad), - float(task_reference_rotation_scale_rad), - float(task_reference_translation_scale_m), - ) - if min(scales) <= 0.0: - raise ValueError("static initialization scales must be positive") - ( - reprojection_scale, - pose_scale, - translation_scale, - relative_rotation_scale, - relative_translation_scale, - normal_scale, - task_reference_rotation_scale, - task_reference_translation_scale, - ) = scales - - combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] - for frame in frames: - candidate_lists = [tuple(frame.get(role, ())) for role in role_names] - if any(not candidates for candidates in candidate_lists): - raise ValueError("every frame must contain every requested role") - combinations_by_frame.append( - [ - dict(zip(role_names, combination)) - for combination in product(*candidate_lists) - ] - ) - - def score_against_reference( - reference: Mapping[str, SquareTagPose], - reference_pairs: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray] - ], - combination: Mapping[str, SquareTagPose], - ) -> float: - score = sum( - pose.reprojection_error_px for pose in combination.values() - ) / reprojection_scale - score += sum( - _normal_alignment_rad(combination[first], combination[second]) - for first, second in normal_pair_names - ) / normal_scale - score += sum( - rotation_distance_rad( - reference[role].quaternion_xyzw, - combination[role].quaternion_xyzw, - ) - / pose_scale - + float( - np.linalg.norm( - np.asarray(combination[role].translation_xyz_m, dtype=float) - - np.asarray(reference[role].translation_xyz_m, dtype=float) - ) - ) - / translation_scale - for role in role_names - ) - for pair, (reference_rotation, reference_translation) in ( - reference_pairs.items() - ): - rotation, translation = _relative_pose( - combination[pair[0]], combination[pair[1]] - ) - score += ( - float((reference_rotation.inv() * rotation).magnitude()) - / relative_rotation_scale - + float(np.linalg.norm(translation - reference_translation)) - / relative_translation_scale - ) - for pair, (task_rotation, task_translation) in task_references.items(): - rotation, translation = _relative_pose( - combination[pair[0]], combination[pair[1]] - ) - score += ( - float((task_rotation.inv() * rotation).magnitude()) - / task_reference_rotation_scale - + float(np.linalg.norm(translation - task_translation)) - / task_reference_translation_scale - ) - return float(score) - - best_total = float("inf") - best_path: list[dict[str, SquareTagPose]] | None = None - for reference in combinations_by_frame[0]: - reference_pairs = { - pair: _relative_pose(reference[pair[0]], reference[pair[1]]) - for pair in pair_names - } - path = [reference] - total = score_against_reference(reference, reference_pairs, reference) - for combinations in combinations_by_frame[1:]: - scored = [ - ( - score_against_reference( - reference, reference_pairs, combination - ), - combination, - ) - for combination in combinations - ] - cost, selected = min(scored, key=lambda item: item[0]) - total += cost - path.append(selected) - if total < best_total: - best_total = total - best_path = path - - if best_path is None: - raise RuntimeError("static group initialization produced no path") - - # Reuse the complete quality calculation with exactly one chosen branch - # per role and frame. This retains all existing quality fields without - # reintroducing the combinatorial branch search. - reduced_frames = [ - {role: (frame[role],) for role in role_names} for frame in best_path - ] - selected_path, quality = select_rigid_group_trajectory( - reduced_frames, - roles=role_names, - fixed_pairs=pair_names, - reprojection_scale_px=reprojection_scale, - rotation_scale_rad=relative_rotation_scale, - translation_scale_m=relative_translation_scale, - normal_alignment_pairs=normal_pair_names, - normal_alignment_scale_rad=normal_scale, - ) - quality = dict(quality) - quality["total_cost"] = float(best_total) - quality["initialization_search"] = "static_reference" - quality["task_reference_used"] = ( - "true" if task_references else "false" - ) - return selected_path, quality - - -def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray: - matrix = np.asarray(camera_matrix, dtype=np.float64) - if matrix.shape != (3, 3): - raise ValueError("camera_matrix must have shape (3, 3)") - if not np.all(np.isfinite(matrix)): - raise ValueError("camera_matrix must be finite") - if matrix[0, 0] <= 0.0 or matrix[1, 1] <= 0.0: - raise ValueError("camera focal lengths must be positive") - return matrix - - -def square_object_points(tag_size_m: float) -> np.ndarray: - """Return IPPE-square points matching apriltag_msgs corner order. - - ``apriltag_ros`` reports bottom-left, bottom-right, top-right, top-left. - OpenCV's ``SOLVEPNP_IPPE_SQUARE`` requires the same physical corners in - the order below. - """ - size = float(tag_size_m) - if not math.isfinite(size) or size <= 0.0: - raise ValueError("tag_size_m must be finite and positive") - half = size / 2.0 - return np.asarray( - [ - [-half, half, 0.0], - [half, half, 0.0], - [half, -half, 0.0], - [-half, -half, 0.0], - ], - dtype=np.float64, - ) - - -def solve_square_tag_ippe( - corners_xy: Sequence[Sequence[float]], - *, - tag_size_m: float, - camera_matrix: Sequence[Sequence[float]], -) -> list[SquareTagPose]: - """Return every finite, positive-depth IPPE pose for one square tag.""" - image_points = np.asarray(corners_xy, dtype=np.float64) - if image_points.shape != (4, 2): - raise ValueError("corners_xy must have shape (4, 2)") - if not np.all(np.isfinite(image_points)): - raise ValueError("corners_xy must be finite") - intrinsic = _as_camera_matrix(camera_matrix) - object_points = square_object_points(tag_size_m) - distortion = np.zeros((4, 1), dtype=np.float64) - - solved, rotation_vectors, translations, _ = cv2.solvePnPGeneric( - object_points, - image_points, - intrinsic, - distortion, - flags=cv2.SOLVEPNP_IPPE_SQUARE, - ) - if not solved: - return [] - - candidates: list[SquareTagPose] = [] - for rotation_vector, translation in zip(rotation_vectors, translations): - rotation_matrix, _ = cv2.Rodrigues(rotation_vector) - translation_vector = np.asarray(translation, dtype=float).reshape(3) - camera_points = ( - rotation_matrix @ object_points.T - + translation_vector.reshape(3, 1) - ).T - if np.min(camera_points[:, 2]) <= 0.0: - continue - projected, _ = cv2.projectPoints( - object_points, - rotation_vector, - translation_vector, - intrinsic, - distortion, - ) - residual = projected.reshape(4, 2) - image_points - reprojection_error = float( - np.sqrt(np.mean(np.sum(residual * residual, axis=1))) - ) - quaternion = Rotation.from_matrix(rotation_matrix).as_quat() - if not ( - np.all(np.isfinite(quaternion)) - and np.all(np.isfinite(translation_vector)) - and math.isfinite(reprojection_error) - ): - continue - candidates.append( - SquareTagPose( - quaternion_xyzw=tuple(float(value) for value in quaternion), - translation_xyz_m=tuple( - float(value) for value in translation_vector - ), - reprojection_error_px=reprojection_error, - ) - ) - return candidates - - -def rotation_distance_rad( - first_xyzw: Sequence[float], - second_xyzw: Sequence[float], -) -> float: - first = np.asarray(first_xyzw, dtype=float) - second = np.asarray(second_xyzw, dtype=float) - if first.shape != (4,) or second.shape != (4,): - raise ValueError("quaternions must contain four values") - first_norm = float(np.linalg.norm(first)) - second_norm = float(np.linalg.norm(second)) - if ( - not np.all(np.isfinite(first)) - or not np.all(np.isfinite(second)) - or first_norm <= 0.0 - or second_norm <= 0.0 - ): - raise ValueError("quaternions must be finite and non-zero") - # q and -q represent the same rotation. The absolute dot-product gives - # the geodesic SO(3) distance without constructing two scipy Rotation - # objects for every branch comparison in the live tracker. - cosine_half_angle = abs( - float(np.dot(first / first_norm, second / second_norm)) - ) - return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0))) - - -def select_continuous_pose( - candidates: Sequence[SquareTagPose], - *, - previous: SquareTagPose | None, - maximum_reprojection_error_px: float, - reprojection_tie_px: float, - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - maximum_tag_tilt_rad: float, -) -> tuple[SquareTagPose | None, str]: - """Select the best IPPE branch using image fit and temporal continuity.""" - maximum_error = float(maximum_reprojection_error_px) - tie_error = float(reprojection_tie_px) - maximum_rotation = float(maximum_pose_jump_rad) - maximum_translation = float(maximum_translation_jump_m) - maximum_tilt = float(maximum_tag_tilt_rad) - if min( - maximum_error, - maximum_rotation, - maximum_translation, - maximum_tilt, - ) <= 0.0: - raise ValueError("PnP selection thresholds must be positive") - if tie_error < 0.0: - raise ValueError("reprojection_tie_px must be non-negative") - - eligible: list[SquareTagPose] = [] - for candidate in candidates: - if candidate.reprojection_error_px > maximum_error: - continue - normal = Rotation.from_quat(candidate.quaternion_xyzw).as_matrix()[:, 2] - tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0))) - if tilt > maximum_tilt: - continue - eligible.append(candidate) - if not eligible: - return None, "no_pose_within_reprojection_or_tilt_limit" - - eligible.sort(key=lambda item: item.reprojection_error_px) - best = eligible[0] - if previous is None: - return best, "" - - # Temporal continuity must only break a genuine planar-PnP tie. The old - # implementation normalised reprojection error by the permissive 1.5 px - # rejection limit, which allowed a stale mirror branch at 0.25 px to beat - # the true branch at e.g. 0.05 px merely because it was closer to the - # preceding (already wrong) pose. Once one IPPE solution has a meaningful - # image-fit advantage, trust it and allow the tracker to leave the stale - # branch even if that correction is a large pose jump. - competitive = [ - candidate - for candidate in eligible - if candidate.reprojection_error_px - <= best.reprojection_error_px + tie_error - ] - if len(competitive) == 1: - return best, "" - - previous_translation = np.asarray(previous.translation_xyz_m, dtype=float) - scored: list[tuple[float, SquareTagPose]] = [] - for candidate in competitive: - rotation_jump = rotation_distance_rad( - previous.quaternion_xyzw, - candidate.quaternion_xyzw, - ) - translation_jump = float( - np.linalg.norm( - np.asarray(candidate.translation_xyz_m, dtype=float) - - previous_translation - ) - ) - if ( - rotation_jump > maximum_rotation - or translation_jump > maximum_translation - ): - continue - score = ( - ( - candidate.reprojection_error_px - - best.reprojection_error_px - ) - / max(tie_error, np.finfo(float).eps) - + rotation_jump / maximum_rotation - + translation_jump / maximum_translation - ) - scored.append((float(score), candidate)) - if not scored: - return None, "pose_jump" - - selected = min(scored, key=lambda item: item[0])[1] - previous_quaternion = np.asarray(previous.quaternion_xyzw, dtype=float) - selected_quaternion = np.asarray(selected.quaternion_xyzw, dtype=float) - if float(np.dot(previous_quaternion, selected_quaternion)) < 0.0: - selected = replace( - selected, - quaternion_xyzw=tuple( - float(value) for value in -selected_quaternion - ), - ) - return selected, "" - - -class SquareTagPoseTracker: - """Maintain the selected planar-PnP branch independently for each tag.""" - - def __init__( - self, - *, - maximum_reprojection_error_px: float, - reprojection_tie_px: float, - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - maximum_tag_tilt_rad: float, - reset_after_seconds: float, - ) -> None: - self.maximum_reprojection_error_px = float( - maximum_reprojection_error_px - ) - self.reprojection_tie_px = float(reprojection_tie_px) - self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) - self.maximum_translation_jump_m = float(maximum_translation_jump_m) - self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad) - self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000) - if self.reset_after_ns <= 0: - raise ValueError("reset_after_seconds must be positive") - self._previous: dict[str, tuple[int, SquareTagPose]] = {} - self.last_candidates_by_role: dict[ - str, tuple[SquareTagPose, ...] - ] = {} - self.last_candidate_diagnostics_by_role: dict[ - str, dict[str, float | int] - ] = {} - self.branch_correction_counts: dict[str, int] = {} - - def reset(self) -> None: - self._previous.clear() - self.last_candidates_by_role.clear() - self.last_candidate_diagnostics_by_role.clear() - self.branch_correction_counts.clear() - - def estimate( - self, - role: str, - corners_xy: Sequence[Sequence[float]], - *, - tag_size_m: float, - camera_matrix: Sequence[Sequence[float]], - stamp_ns: int, - reprojection_tie_px: float | None = None, - ) -> tuple[SquareTagPose | None, str]: - try: - candidates = solve_square_tag_ippe( - corners_xy, - tag_size_m=tag_size_m, - camera_matrix=camera_matrix, - ) - except (ValueError, cv2.error): - self.last_candidates_by_role[str(role)] = () - self.last_candidate_diagnostics_by_role[str(role)] = { - "solved_candidate_count": 0, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - return None, "pnp_solve_failed" - if not candidates: - self.last_candidates_by_role[str(role)] = () - self.last_candidate_diagnostics_by_role[str(role)] = { - "solved_candidate_count": 0, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - return None, "pnp_solve_failed" - reprojection_candidates = [ - candidate - for candidate in candidates - if candidate.reprojection_error_px - <= self.maximum_reprojection_error_px - ] - candidate_tilts_rad: list[float] = [] - independent_candidates: list[SquareTagPose] = [] - for candidate in reprojection_candidates: - normal = Rotation.from_quat( - candidate.quaternion_xyzw - ).as_matrix()[:, 2] - tilt = math.acos( - float(np.clip(abs(normal[2]), 0.0, 1.0)) - ) - candidate_tilts_rad.append(float(tilt)) - if tilt <= self.maximum_tag_tilt_rad: - independent_candidates.append(candidate) - - # Candidate generation and candidate selection have different - # contracts. The per-Tag tilt limit protects a pose used without any - # other geometry, but it must not erase a finite, low-reprojection - # IPPE solution before SquareTagGroupPoseTracker can evaluate it - # against the fixed palm reference, the articulated chain and the - # preceding group pose. At a strongly oblique view the planar - # ambiguity is usually smaller, and rejecting both branches at a - # fixed angle caused deterministic mid-sweep holes despite continuous - # image detections. Group tracking therefore receives every - # reprojection-valid candidate; independent tracking below retains the - # original tilt safety gate. - self.last_candidates_by_role[str(role)] = tuple( - reprojection_candidates - ) - diagnostics: dict[str, float | int] = { - "solved_candidate_count": len(candidates), - "reprojection_candidate_count": len(reprojection_candidates), - "independent_tilt_candidate_count": len(independent_candidates), - "minimum_reprojection_error_px": float( - min( - candidate.reprojection_error_px - for candidate in candidates - ) - ), - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - if candidate_tilts_rad: - diagnostics["minimum_candidate_tilt_deg"] = math.degrees( - min(candidate_tilts_rad) - ) - diagnostics["maximum_candidate_tilt_deg"] = math.degrees( - max(candidate_tilts_rad) - ) - self.last_candidate_diagnostics_by_role[str(role)] = diagnostics - if not independent_candidates: - return None, "no_pose_within_reprojection_or_tilt_limit" - - previous_record = self._previous.get(str(role)) - previous: SquareTagPose | None = None - if previous_record is not None: - previous_stamp, previous_pose = previous_record - elapsed = int(stamp_ns) - previous_stamp - if 0 <= elapsed <= self.reset_after_ns: - previous = previous_pose - - selected, reason = select_continuous_pose( - independent_candidates, - previous=previous, - maximum_reprojection_error_px=( - self.maximum_reprojection_error_px - ), - reprojection_tie_px=( - self.reprojection_tie_px - if reprojection_tie_px is None - else float(reprojection_tie_px) - ), - maximum_pose_jump_rad=self.maximum_pose_jump_rad, - maximum_translation_jump_m=self.maximum_translation_jump_m, - maximum_tag_tilt_rad=self.maximum_tag_tilt_rad, - ) - if selected is not None: - if previous is not None: - rotation_jump = rotation_distance_rad( - previous.quaternion_xyzw, - selected.quaternion_xyzw, - ) - translation_jump = float( - np.linalg.norm( - np.asarray(selected.translation_xyz_m, dtype=float) - - np.asarray( - previous.translation_xyz_m, - dtype=float, - ) - ) - ) - if ( - rotation_jump > self.maximum_pose_jump_rad - or translation_jump - > self.maximum_translation_jump_m - ): - key = str(role) - self.branch_correction_counts[key] = ( - self.branch_correction_counts.get(key, 0) + 1 - ) - self._previous[str(role)] = (int(stamp_ns), selected) - return selected, reason - - -class SquareTagGroupPoseTracker: - """Choose all tag branches together using thumb-chain continuity. - - A 30 px planar tag has two IPPE solutions whose reprojection errors can - exchange order from one frame to the next. Tracking each tag - independently can therefore choose an incompatible pair for a relative - joint such as T4->T5. This tracker enumerates the small Cartesian product - (at most 2**4 combinations) and favours the combination that keeps both - the camera poses and all adjacent relative poses continuous. - """ - - def __init__( - self, - *, - roles: Sequence[str], - adjacent_pairs: Sequence[tuple[str, str]], - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - relative_rotation_scale_rad: float, - relative_translation_scale_m: float, - reprojection_scale_px: float, - reprojection_weight: float, - reset_after_seconds: float, - initialization_frames: int = 1, - normal_alignment_pairs: Sequence[tuple[str, str]] = (), - normal_alignment_scale_rad: float = math.radians(5.0), - maximum_normal_alignment_rad: float | None = None, - return_reference_rotation_scale_rad: float = math.radians(1.0), - return_reference_maximum_command_gap_u8: int = 8, - coupled_rotation_pairs: Sequence[ - tuple[str, str, str, str, float] - ] = (), - coupled_rotation_scale_rad: float = math.radians(3.0), - maximum_coupled_rotation_residual_rad: float | None = None, - ) -> None: - self.roles = tuple(str(role) for role in roles) - self.adjacent_pairs = tuple( - (str(parent), str(child)) - for parent, child in adjacent_pairs - ) - self.normal_alignment_pairs = tuple( - (str(first), str(second)) - for first, second in normal_alignment_pairs - ) - self.coupled_rotation_pairs = tuple( - ( - str(driver_parent), - str(driver_child), - str(follower_parent), - str(follower_child), - float(multiplier), - ) - for ( - driver_parent, - driver_child, - follower_parent, - follower_child, - multiplier, - ) in coupled_rotation_pairs - ) - if not self.roles or len(set(self.roles)) != len(self.roles): - raise ValueError("roles must be non-empty and unique") - if any( - parent not in self.roles or child not in self.roles - for parent, child in ( - *self.adjacent_pairs, - *self.normal_alignment_pairs, - ) - ): - raise ValueError("group geometry pairs must reference roles") - self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) - self.maximum_translation_jump_m = float( - maximum_translation_jump_m - ) - self.relative_rotation_scale_rad = float( - relative_rotation_scale_rad - ) - self.relative_translation_scale_m = float( - relative_translation_scale_m - ) - self.reprojection_scale_px = float(reprojection_scale_px) - self.reprojection_weight = float(reprojection_weight) - self.initialization_frames = int(initialization_frames) - self.normal_alignment_scale_rad = float( - normal_alignment_scale_rad - ) - self.maximum_normal_alignment_rad = ( - None - if maximum_normal_alignment_rad is None - else float(maximum_normal_alignment_rad) - ) - self.return_reference_rotation_scale_rad = float( - return_reference_rotation_scale_rad - ) - self.return_reference_maximum_command_gap_u8 = int( - return_reference_maximum_command_gap_u8 - ) - self.coupled_rotation_scale_rad = float( - coupled_rotation_scale_rad - ) - self.maximum_coupled_rotation_residual_rad = ( - None - if maximum_coupled_rotation_residual_rad is None - else float(maximum_coupled_rotation_residual_rad) - ) - reset_seconds = float(reset_after_seconds) - if min( - self.maximum_pose_jump_rad, - self.maximum_translation_jump_m, - self.relative_rotation_scale_rad, - self.relative_translation_scale_m, - self.reprojection_scale_px, - self.normal_alignment_scale_rad, - self.return_reference_rotation_scale_rad, - self.coupled_rotation_scale_rad, - reset_seconds, - ) <= 0.0: - raise ValueError("group tracking scales must be positive") - if self.reprojection_weight < 0.0: - raise ValueError("reprojection_weight must be non-negative") - if self.initialization_frames < 1: - raise ValueError("initialization_frames must be positive") - if self.return_reference_maximum_command_gap_u8 < 0: - raise ValueError( - "return reference maximum command gap must be non-negative" - ) - if ( - self.maximum_normal_alignment_rad is not None - and self.maximum_normal_alignment_rad <= 0.0 - ): - raise ValueError("maximum normal alignment must be positive") - if any( - role not in self.roles - for coupling in self.coupled_rotation_pairs - for role in coupling[:4] - ): - raise ValueError("coupled rotation pairs must reference roles") - if any( - multiplier <= 0.0 - for *_, multiplier in self.coupled_rotation_pairs - ): - raise ValueError("coupled rotation multipliers must be positive") - if ( - self.maximum_coupled_rotation_residual_rad is not None - and self.maximum_coupled_rotation_residual_rad <= 0.0 - ): - raise ValueError( - "maximum coupled rotation residual must be positive" - ) - self.reset_after_ns = int(reset_seconds * 1_000_000_000) - self._previous: dict[str, SquareTagPose] = {} - self._previous_stamp_ns: int | None = None - self._initial_candidates: list[ - dict[str, tuple[SquareTagPose, ...]] - ] = [] - self._initial_stamps_ns: list[int] = [] - self.last_initialization_quality: dict[str, float | str] = {} - self.branch_correction_counts: dict[str, int] = {} - self._decreasing_relative_rotations: dict[ - int, dict[tuple[str, str], Rotation] - ] = {} - self._coupled_reference_rotations: dict[ - tuple[str, str], Rotation - ] = {} - self._task_reference_relative_poses: dict[ - tuple[str, str], tuple[Rotation, np.ndarray] - ] = {} - self.last_missing_roles: tuple[str, ...] = () - - def reset(self, *, preserve_task_reference: bool = False) -> None: - self._previous.clear() - self._previous_stamp_ns = None - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - self.last_initialization_quality.clear() - self.branch_correction_counts.clear() - self._decreasing_relative_rotations.clear() - self._coupled_reference_rotations.clear() - self.last_missing_roles = () - if not preserve_task_reference: - self._task_reference_relative_poses.clear() - - def _task_reference_cost( - self, combination: Mapping[str, SquareTagPose] - ) -> float: - residual = 0.0 - for pair, (expected_rotation, expected_translation) in ( - self._task_reference_relative_poses.items() - ): - rotation, translation = _relative_pose( - combination[pair[0]], combination[pair[1]] - ) - residual += ( - float((expected_rotation.inv() * rotation).magnitude()) - / self.return_reference_rotation_scale_rad - + float(np.linalg.norm(translation - expected_translation)) - / self.relative_translation_scale_m - ) - return residual - - def _coupled_rotation_residuals( - self, combination: Mapping[str, SquareTagPose] - ) -> tuple[float, ...]: - if not self.coupled_rotation_pairs: - return () - residuals: list[float] = [] - for ( - driver_parent, - driver_child, - follower_parent, - follower_child, - multiplier, - ) in self.coupled_rotation_pairs: - driver_pair = (driver_parent, driver_child) - follower_pair = (follower_parent, follower_child) - if ( - driver_pair not in self._coupled_reference_rotations - or follower_pair not in self._coupled_reference_rotations - ): - return () - driver_rotation = _relative_pose( - combination[driver_parent], combination[driver_child] - )[0] - follower_rotation = _relative_pose( - combination[follower_parent], combination[follower_child] - )[0] - driver_travel = ( - self._coupled_reference_rotations[driver_pair].inv() - * driver_rotation - ).magnitude() - follower_travel = ( - self._coupled_reference_rotations[follower_pair].inv() - * follower_rotation - ).magnitude() - residuals.append( - abs(float(follower_travel) - multiplier * float(driver_travel)) - ) - return tuple(residuals) - - def _informative_coupled_rotation_costs( - self, - combinations: Sequence[Mapping[str, SquareTagPose]], - ) -> tuple[float, ...]: - """Return branch costs only while the weak coupling prior is credible. - - The URDF mimic ratio is useful for distinguishing two planar-IPPE - branches, but it is not measurement truth for a passive joint. Once - every otherwise viable combination disagrees with that ratio, using - it would bias the measured curve (and previously rejected every - frame). In that case fall back to visual continuity for this frame. - """ - residuals = tuple( - self._coupled_rotation_residuals(combination) - for combination in combinations - ) - if not residuals or not any(residuals): - return tuple(0.0 for _ in combinations) - if ( - self.maximum_coupled_rotation_residual_rad is not None - and not any( - values - and max(values) - <= self.maximum_coupled_rotation_residual_rad - for values in residuals - ) - ): - return tuple(0.0 for _ in combinations) - return tuple( - sum(values) / self.coupled_rotation_scale_rad - for values in residuals - ) - - def _return_reference( - self, command_u8: int | None - ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]: - if command_u8 is None or not self._decreasing_relative_rotations: - return {} - command = int(command_u8) - nearest = min( - self._decreasing_relative_rotations, - key=lambda candidate: abs(candidate - command), - ) - if ( - abs(nearest - command) - > self.return_reference_maximum_command_gap_u8 - ): - return {} - references = self._decreasing_relative_rotations[nearest] - commands = sorted(self._decreasing_relative_rotations) - axes: dict[tuple[str, str], np.ndarray | None] = {} - for pair in self.adjacent_pairs: - endpoint_delta = ( - self._decreasing_relative_rotations[commands[-1]][pair].inv() - * self._decreasing_relative_rotations[commands[0]][pair] - ).as_rotvec() - norm = float(np.linalg.norm(endpoint_delta)) - axes[pair] = ( - None - if norm < math.radians(5.0) - else endpoint_delta / norm - ) - return { - pair: (rotation, axes[pair]) - for pair, rotation in references.items() - } - - def _return_reference_cost( - self, - combination: Mapping[str, SquareTagPose], - reference: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray | None] - ], - ) -> float: - residual = 0.0 - for pair, (expected, motion_axis) in reference.items(): - vector = ( - expected.inv() - * _relative_pose( - combination[pair[0]], combination[pair[1]] - )[0] - ).as_rotvec() - if motion_axis is not None: - # The outbound trajectory identifies the physical one-DOF - # motion axis. Do not penalize return travel along that axis: - # it may contain real mechanical hysteresis that calibration - # must measure. A planar-IPPE mirror branch appears primarily - # as a large orthogonal tilt and is rejected by this residual. - vector = vector - motion_axis * float(vector @ motion_axis) - residual += float(np.linalg.norm(vector)) - return residual / self.return_reference_rotation_scale_rad - - def select( - self, - candidates_by_role: Mapping[str, Sequence[SquareTagPose]], - *, - stamp_ns: int, - trajectory_command_u8: int | None = None, - trajectory_direction: str | None = None, - ) -> tuple[dict[str, SquareTagPose] | None, str]: - """Return one mutually consistent pose for every configured role.""" - direction = ( - None - if trajectory_direction is None - else str(trajectory_direction) - ) - if direction not in {None, "decreasing", "increasing"}: - raise ValueError( - "trajectory_direction must be decreasing or increasing" - ) - return_reference = ( - self._return_reference(trajectory_command_u8) - if direction == "increasing" - else {} - ) - candidate_lists = [ - tuple(candidates_by_role.get(role, ())) - for role in self.roles - ] - self.last_missing_roles = tuple( - role - for role, candidates in zip(self.roles, candidate_lists) - if not candidates - ) - if self.last_missing_roles: - return None, "group_missing_pose_candidates" - self.last_missing_roles = () - - combinations = [ - dict(zip(self.roles, combination)) - for combination in product(*candidate_lists) - ] - minimum_errors = { - role: min( - candidate.reprojection_error_px - for candidate in candidates - ) - for role, candidates in zip(self.roles, candidate_lists) - } - stamp = int(stamp_ns) - previous_is_fresh = ( - self._previous_stamp_ns is not None - and 0 <= stamp - self._previous_stamp_ns - <= self.reset_after_ns - and set(self._previous) == set(self.roles) - ) - - if not previous_is_fresh: - if self._previous_stamp_ns is not None: - self._previous.clear() - self._previous_stamp_ns = None - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - self.last_initialization_quality.clear() - if self.initialization_frames > 1: - if self._initial_stamps_ns and not ( - 0 <= stamp - self._initial_stamps_ns[-1] - <= self.reset_after_ns - ): - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - self._initial_candidates.append( - { - role: tuple(candidates_by_role.get(role, ())) - for role in self.roles - } - ) - self._initial_stamps_ns.append(stamp) - if len(self._initial_candidates) < self.initialization_frames: - return ( - None, - "group_initializing:" - f"{len(self._initial_candidates)}/" - f"{self.initialization_frames}", - ) - selected_path, initialization_quality = ( - select_static_rigid_group_initialization( - self._initial_candidates, - roles=self.roles, - fixed_pairs=self.adjacent_pairs, - reprojection_scale_px=self.reprojection_scale_px, - maximum_pose_jump_rad=self.maximum_pose_jump_rad, - maximum_translation_jump_m=( - self.maximum_translation_jump_m - ), - relative_rotation_scale_rad=( - self.relative_rotation_scale_rad - ), - relative_translation_scale_m=( - self.relative_translation_scale_m - ), - normal_alignment_pairs=( - self.normal_alignment_pairs - ), - normal_alignment_scale_rad=( - self.normal_alignment_scale_rad - ), - task_reference_pairs=( - self._task_reference_relative_poses - ), - task_reference_rotation_scale_rad=( - self.return_reference_rotation_scale_rad - ), - task_reference_translation_scale_m=( - self.relative_translation_scale_m - ), - ) - ) - selected = selected_path[-1] - stamp = self._initial_stamps_ns[-1] - self.last_initialization_quality = dict( - initialization_quality - ) - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - if ( - self.maximum_normal_alignment_rad is not None - and float( - initialization_quality[ - "maximum_normal_alignment_rad" - ] - ) - > self.maximum_normal_alignment_rad - ): - return None, "group_normal_alignment" - else: - coupling_costs = self._informative_coupled_rotation_costs( - combinations - ) - selected = min( - zip(combinations, coupling_costs), - key=lambda item: ( - sum( - pose.reprojection_error_px - for pose in item[0].values() - ) - / self.reprojection_scale_px - + sum( - _normal_alignment_rad( - item[0][first], item[0][second] - ) - for first, second in self.normal_alignment_pairs - ) - / self.normal_alignment_scale_rad - + self._return_reference_cost( - item[0], return_reference - ) - + item[1] - + self._task_reference_cost(item[0]) - ), - )[0] - maximum_alignment = max( - ( - _normal_alignment_rad( - selected[first], selected[second] - ) - for first, second in self.normal_alignment_pairs - ), - default=0.0, - ) - self.last_initialization_quality = { - "maximum_normal_alignment_rad": maximum_alignment - } - if ( - self.maximum_normal_alignment_rad is not None - and maximum_alignment - > self.maximum_normal_alignment_rad - ): - return None, "group_normal_alignment" - else: - previous_relative = { - pair: _relative_pose( - self._previous[pair[0]], - self._previous[pair[1]], - ) - for pair in self.adjacent_pairs - } - base_scored: list[tuple[float, dict[str, SquareTagPose]]] = [] - for combination in combinations: - absolute_rotation_motion = 0.0 - absolute_translation_motion = 0.0 - rejected = False - for role in self.roles: - rotation_motion = rotation_distance_rad( - self._previous[role].quaternion_xyzw, - combination[role].quaternion_xyzw, - ) - translation_motion = float( - np.linalg.norm( - np.asarray( - combination[role].translation_xyz_m, - dtype=float, - ) - - np.asarray( - self._previous[role].translation_xyz_m, - dtype=float, - ) - ) - ) - if ( - rotation_motion > self.maximum_pose_jump_rad - or translation_motion - > self.maximum_translation_jump_m - ): - rejected = True - break - absolute_rotation_motion += rotation_motion - absolute_translation_motion += translation_motion - if rejected: - continue - - relative_rotation_motion = 0.0 - relative_translation_motion = 0.0 - for pair in self.adjacent_pairs: - rotation, translation = _relative_pose( - combination[pair[0]], - combination[pair[1]], - ) - old_rotation, old_translation = previous_relative[pair] - relative_rotation_motion += float( - (old_rotation.inv() * rotation).magnitude() - ) - relative_translation_motion += float( - np.linalg.norm(translation - old_translation) - ) - - reprojection_penalty = sum( - max( - 0.0, - combination[role].reprojection_error_px - - minimum_errors[role], - ) - for role in self.roles - ) / self.reprojection_scale_px - score = ( - absolute_rotation_motion - / self.maximum_pose_jump_rad - + absolute_translation_motion - / self.maximum_translation_jump_m - + relative_rotation_motion - / self.relative_rotation_scale_rad - + relative_translation_motion - / self.relative_translation_scale_m - + self.reprojection_weight * reprojection_penalty - + self._return_reference_cost( - combination, return_reference - ) - ) - base_scored.append((float(score), combination)) - - if not base_scored: - return None, "group_pose_jump" - coupling_costs = self._informative_coupled_rotation_costs( - [combination for _, combination in base_scored] - ) - scored = [ - (base_score + coupling_cost, combination) - for (base_score, combination), coupling_cost in zip( - base_scored, coupling_costs - ) - ] - selected = min(scored, key=lambda item: item[0])[1] - - aligned: dict[str, SquareTagPose] = {} - for role in self.roles: - pose = selected[role] - if previous_is_fresh: - old_quaternion = np.asarray( - self._previous[role].quaternion_xyzw, - dtype=float, - ) - quaternion = np.asarray( - pose.quaternion_xyzw, - dtype=float, - ) - if float(np.dot(old_quaternion, quaternion)) < 0.0: - pose = replace( - pose, - quaternion_xyzw=tuple( - float(value) for value in -quaternion - ), - ) - best_reprojection = min( - candidate_lists[self.roles.index(role)], - key=lambda candidate: candidate.reprojection_error_px, - ) - if pose != best_reprojection: - self.branch_correction_counts[role] = ( - self.branch_correction_counts.get(role, 0) + 1 - ) - aligned[role] = pose - - self._previous = aligned - self._previous_stamp_ns = stamp - if ( - direction == "decreasing" - and trajectory_command_u8 is not None - and not self._coupled_reference_rotations - ): - for ( - driver_parent, - driver_child, - follower_parent, - follower_child, - _multiplier, - ) in self.coupled_rotation_pairs: - for pair in ( - (driver_parent, driver_child), - (follower_parent, follower_child), - ): - self._coupled_reference_rotations[pair] = _relative_pose( - aligned[pair[0]], aligned[pair[1]] - )[0] - if direction == "decreasing" and trajectory_command_u8 is not None: - self._decreasing_relative_rotations[ - int(trajectory_command_u8) - ] = { - pair: _relative_pose( - aligned[pair[0]], aligned[pair[1]] - )[0] - for pair in self.adjacent_pairs - } - if not self._task_reference_relative_poses: - self._task_reference_relative_poses = { - pair: _relative_pose( - aligned[pair[0]], aligned[pair[1]] - ) - for pair in self.adjacent_pairs - } - return dict(aligned), "" +"""Stable PnP API; implementations are separated by pose-estimation responsibility.""" + +from .tag_pose.types import SquareTagPose as SquareTagPose +from .tag_pose.relative import _relative_pose as _relative_pose +from .tag_pose.relative import _normal_alignment_rad as _normal_alignment_rad +from .tag_pose.trajectory import select_rigid_group_trajectory as select_rigid_group_trajectory +from .tag_pose.rigid_group import select_static_rigid_group_initialization as select_static_rigid_group_initialization +from .tag_pose.ippe import _as_camera_matrix as _as_camera_matrix +from .tag_pose.ippe import square_object_points as square_object_points +from .tag_pose.ippe import solve_square_tag_ippe as solve_square_tag_ippe +from .tag_pose.ippe import rotation_distance_rad as rotation_distance_rad +from .tag_pose.tracking import _validate_reprojection_thresholds as _validate_reprojection_thresholds +from .tag_pose.tracking import select_continuous_pose as select_continuous_pose +from .tag_pose.tracking import SquareTagPoseTracker as SquareTagPoseTracker +from .tag_pose.rigid_group import SquareTagGroupPoseTracker as SquareTagGroupPoseTracker +from .tag_pose.parameters import PoseTrackingParameters as PoseTrackingParameters +from .tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS as DEFAULT_POSE_TRACKING_PARAMETERS +from .tag_pose.parameters import DEFAULT_REPROJECTION_TIE_PX as DEFAULT_REPROJECTION_TIE_PX diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/__init__.py new file mode 100644 index 0000000..b85ebd0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/__init__.py @@ -0,0 +1 @@ +"""Shared pose estimation primitives; public API is core.geometry.pnp.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py new file mode 100644 index 0000000..f7da3a9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/ippe.py @@ -0,0 +1,137 @@ +"""Ippe for shared square Tag pose estimation.""" + +from __future__ import annotations + +from typing import Sequence +import math + +from scipy.spatial.transform import Rotation +import cv2 +import numpy as np + +from .types import SquareTagPose + + +def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray: + matrix = np.asarray(camera_matrix, dtype=np.float64) + if matrix.shape != (3, 3): + raise ValueError("camera_matrix must have shape (3, 3)") + if not np.all(np.isfinite(matrix)): + raise ValueError("camera_matrix must be finite") + if matrix[0, 0] <= 0.0 or matrix[1, 1] <= 0.0: + raise ValueError("camera focal lengths must be positive") + return matrix + + +def square_object_points(tag_size_m: float) -> np.ndarray: + """Return IPPE-square points matching apriltag_msgs corner order. + + ``apriltag_ros`` reports bottom-left, bottom-right, top-right, top-left. + OpenCV's ``SOLVEPNP_IPPE_SQUARE`` requires the same physical corners in + the order below. + """ + size = float(tag_size_m) + if not math.isfinite(size) or size <= 0.0: + raise ValueError("tag_size_m must be finite and positive") + half = size / 2.0 + return np.asarray( + [ + [-half, half, 0.0], + [half, half, 0.0], + [half, -half, 0.0], + [-half, -half, 0.0], + ], + dtype=np.float64, + ) + + +def solve_square_tag_ippe( + corners_xy: Sequence[Sequence[float]], + *, + tag_size_m: float, + camera_matrix: Sequence[Sequence[float]], +) -> list[SquareTagPose]: + """Return every finite, positive-depth IPPE pose for one square tag.""" + image_points = np.asarray(corners_xy, dtype=np.float64) + if image_points.shape != (4, 2): + raise ValueError("corners_xy must have shape (4, 2)") + if not np.all(np.isfinite(image_points)): + raise ValueError("corners_xy must be finite") + intrinsic = _as_camera_matrix(camera_matrix) + object_points = square_object_points(tag_size_m) + distortion = np.zeros((4, 1), dtype=np.float64) + + solved, rotation_vectors, translations, _ = cv2.solvePnPGeneric( + object_points, + image_points, + intrinsic, + distortion, + flags=cv2.SOLVEPNP_IPPE_SQUARE, + ) + if not solved: + return [] + + candidates: list[SquareTagPose] = [] + for rotation_vector, translation in zip(rotation_vectors, translations): + rotation_matrix, _ = cv2.Rodrigues(rotation_vector) + translation_vector = np.asarray(translation, dtype=float).reshape(3) + camera_points = ( + rotation_matrix @ object_points.T + + translation_vector.reshape(3, 1) + ).T + if np.min(camera_points[:, 2]) <= 0.0: + continue + projected, _ = cv2.projectPoints( + object_points, + rotation_vector, + translation_vector, + intrinsic, + distortion, + ) + residual = projected.reshape(4, 2) - image_points + reprojection_error = float( + np.sqrt(np.mean(np.sum(residual * residual, axis=1))) + ) + quaternion = Rotation.from_matrix(rotation_matrix).as_quat() + if not ( + np.all(np.isfinite(quaternion)) + and np.all(np.isfinite(translation_vector)) + and math.isfinite(reprojection_error) + ): + continue + candidates.append( + SquareTagPose( + quaternion_xyzw=tuple(float(value) for value in quaternion), + translation_xyz_m=tuple( + float(value) for value in translation_vector + ), + reprojection_error_px=reprojection_error, + ) + ) + return candidates + + +def rotation_distance_rad( + first_xyzw: Sequence[float], + second_xyzw: Sequence[float], +) -> float: + first = np.asarray(first_xyzw, dtype=float) + second = np.asarray(second_xyzw, dtype=float) + if first.shape != (4,) or second.shape != (4,): + raise ValueError("quaternions must contain four values") + first_norm = float(np.linalg.norm(first)) + second_norm = float(np.linalg.norm(second)) + if ( + not np.all(np.isfinite(first)) + or not np.all(np.isfinite(second)) + or first_norm <= 0.0 + or second_norm <= 0.0 + ): + raise ValueError("quaternions must be finite and non-zero") + # q and -q represent the same rotation. The absolute dot-product gives + # the geodesic SO(3) distance without constructing two scipy Rotation + # objects for every branch comparison in the live tracker. + cosine_half_angle = abs( + float(np.dot(first / first_norm, second / second_norm)) + ) + return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py new file mode 100644 index 0000000..37177e1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/parameters.py @@ -0,0 +1,21 @@ +"""Parameters for shared square Tag pose estimation.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +@dataclass(frozen=True) +class PoseTrackingParameters: + """One shared set of defaults, with angles stored in radians.""" + + maximum_reprojection_error_px: float = 1.5 + reprojection_tie_px: float = 0.03 + maximum_pose_jump_rad: float = math.radians(35.0) + maximum_translation_jump_m: float = 0.04 + maximum_tag_tilt_rad: float = math.radians(75.0) + reset_after_seconds: float = 5.0 + + +DEFAULT_POSE_TRACKING_PARAMETERS = PoseTrackingParameters() +DEFAULT_REPROJECTION_TIE_PX = DEFAULT_POSE_TRACKING_PARAMETERS.reprojection_tie_px diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/relative.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/relative.py new file mode 100644 index 0000000..0fc3324 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/relative.py @@ -0,0 +1,37 @@ +"""Relative for shared square Tag pose estimation.""" + +from __future__ import annotations + +import math + +from scipy.spatial.transform import Rotation +import numpy as np + +from .types import SquareTagPose + + +def _relative_pose( + parent: SquareTagPose, + child: SquareTagPose, +) -> tuple[Rotation, np.ndarray]: + parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) + child_rotation = Rotation.from_quat(child.quaternion_xyzw) + relative_rotation = parent_rotation.inv() * child_rotation + relative_translation = parent_rotation.inv().apply( + np.asarray(child.translation_xyz_m, dtype=float) + - np.asarray(parent.translation_xyz_m, dtype=float) + ) + return relative_rotation, relative_translation + + +def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float: + """Return the undirected angle between two observed Tag face normals.""" + first_normal = Rotation.from_quat(first.quaternion_xyzw).apply( + [0.0, 0.0, 1.0] + ) + second_normal = Rotation.from_quat(second.quaternion_xyzw).apply( + [0.0, 0.0, 1.0] + ) + return math.acos( + abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0))) + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/rigid_group.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/rigid_group.py new file mode 100644 index 0000000..e5d9b81 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/rigid_group.py @@ -0,0 +1,880 @@ +"""Rigid group for shared square Tag pose estimation.""" + +from __future__ import annotations + +from dataclasses import replace +from itertools import product +from typing import Mapping, Sequence +import math + +from scipy.spatial.transform import Rotation +import numpy as np + +from .ippe import rotation_distance_rad +from .relative import _normal_alignment_rad, _relative_pose +from .trajectory import select_rigid_group_trajectory +from .types import SquareTagPose + + +def select_static_rigid_group_initialization( + frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], + *, + roles: Sequence[str], + fixed_pairs: Sequence[tuple[str, str]], + reprojection_scale_px: float, + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + relative_rotation_scale_rad: float, + relative_translation_scale_m: float, + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float = math.radians(5.0), + task_reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] | None = None, + task_reference_rotation_scale_rad: float = math.radians(1.0), + task_reference_translation_scale_m: float = 0.01, +) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]: + """Select a static multi-Tag IPPE branch path in bounded time. + + Group initialization is performed while the hand is held at an endpoint, + so every frame should describe the same camera and relative Tag poses. + Enumerating a full Viterbi transition matrix for every possible first-frame + branch is therefore unnecessary: with four two-branch Tags and eight + frames it performs more than one hundred thousand scipy rotations and can + block the live ROS callback for several seconds. + + Instead, treat every first-frame combination as a possible static + reference and independently select the closest combination in each later + frame. This preserves the multi-frame rigidity and normal-alignment + evidence while changing the search from O(F*C^2*C0) to O(F*C*C0). + """ + role_names = tuple(str(role) for role in roles) + pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) + normal_pair_names = tuple( + (str(first), str(second)) for first, second in normal_alignment_pairs + ) + task_references = dict(task_reference_pairs or {}) + if not frames: + raise ValueError("at least one PnP frame is required") + if not role_names or len(set(role_names)) != len(role_names): + raise ValueError("roles must be non-empty and unique") + if any( + parent not in role_names or child not in role_names + for parent, child in ( + *pair_names, + *normal_pair_names, + *task_references, + ) + ): + raise ValueError("geometry pairs must reference roles") + scales = ( + float(reprojection_scale_px), + float(maximum_pose_jump_rad), + float(maximum_translation_jump_m), + float(relative_rotation_scale_rad), + float(relative_translation_scale_m), + float(normal_alignment_scale_rad), + float(task_reference_rotation_scale_rad), + float(task_reference_translation_scale_m), + ) + if min(scales) <= 0.0: + raise ValueError("static initialization scales must be positive") + ( + reprojection_scale, + pose_scale, + translation_scale, + relative_rotation_scale, + relative_translation_scale, + normal_scale, + task_reference_rotation_scale, + task_reference_translation_scale, + ) = scales + + combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] + for frame in frames: + candidate_lists = [tuple(frame.get(role, ())) for role in role_names] + if any(not candidates for candidates in candidate_lists): + raise ValueError("every frame must contain every requested role") + combinations_by_frame.append( + [ + dict(zip(role_names, combination)) + for combination in product(*candidate_lists) + ] + ) + + def score_against_reference( + reference: Mapping[str, SquareTagPose], + reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ], + combination: Mapping[str, SquareTagPose], + ) -> float: + score = sum( + pose.reprojection_error_px for pose in combination.values() + ) / reprojection_scale + score += sum( + _normal_alignment_rad(combination[first], combination[second]) + for first, second in normal_pair_names + ) / normal_scale + score += sum( + rotation_distance_rad( + reference[role].quaternion_xyzw, + combination[role].quaternion_xyzw, + ) + / pose_scale + + float( + np.linalg.norm( + np.asarray(combination[role].translation_xyz_m, dtype=float) + - np.asarray(reference[role].translation_xyz_m, dtype=float) + ) + ) + / translation_scale + for role in role_names + ) + for pair, (reference_rotation, reference_translation) in ( + reference_pairs.items() + ): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + score += ( + float((reference_rotation.inv() * rotation).magnitude()) + / relative_rotation_scale + + float(np.linalg.norm(translation - reference_translation)) + / relative_translation_scale + ) + for pair, (task_rotation, task_translation) in task_references.items(): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + score += ( + float((task_rotation.inv() * rotation).magnitude()) + / task_reference_rotation_scale + + float(np.linalg.norm(translation - task_translation)) + / task_reference_translation_scale + ) + return float(score) + + best_total = float("inf") + best_path: list[dict[str, SquareTagPose]] | None = None + for reference in combinations_by_frame[0]: + reference_pairs = { + pair: _relative_pose(reference[pair[0]], reference[pair[1]]) + for pair in pair_names + } + path = [reference] + total = score_against_reference(reference, reference_pairs, reference) + for combinations in combinations_by_frame[1:]: + scored = [ + ( + score_against_reference( + reference, reference_pairs, combination + ), + combination, + ) + for combination in combinations + ] + cost, selected = min(scored, key=lambda item: item[0]) + total += cost + path.append(selected) + if total < best_total: + best_total = total + best_path = path + + if best_path is None: + raise RuntimeError("static group initialization produced no path") + + # Reuse the complete quality calculation with exactly one chosen branch + # per role and frame. This retains all existing quality fields without + # reintroducing the combinatorial branch search. + reduced_frames = [ + {role: (frame[role],) for role in role_names} for frame in best_path + ] + selected_path, quality = select_rigid_group_trajectory( + reduced_frames, + roles=role_names, + fixed_pairs=pair_names, + reprojection_scale_px=reprojection_scale, + rotation_scale_rad=relative_rotation_scale, + translation_scale_m=relative_translation_scale, + normal_alignment_pairs=normal_pair_names, + normal_alignment_scale_rad=normal_scale, + ) + quality = dict(quality) + quality["total_cost"] = float(best_total) + quality["initialization_search"] = "static_reference" + quality["task_reference_used"] = ( + "true" if task_references else "false" + ) + return selected_path, quality + + +class SquareTagGroupPoseTracker: + """Choose all tag branches together using thumb-chain continuity. + + A 30 px planar tag has two IPPE solutions whose reprojection errors can + exchange order from one frame to the next. Tracking each tag + independently can therefore choose an incompatible pair for a relative + joint such as T4->T5. This tracker enumerates the small Cartesian product + (at most 2**4 combinations) and favours the combination that keeps both + the camera poses and all adjacent relative poses continuous. + """ + + def __init__( + self, + *, + roles: Sequence[str], + adjacent_pairs: Sequence[tuple[str, str]], + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + relative_rotation_scale_rad: float, + relative_translation_scale_m: float, + reprojection_scale_px: float, + reprojection_weight: float, + reset_after_seconds: float, + initialization_frames: int = 1, + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float = math.radians(5.0), + maximum_normal_alignment_rad: float | None = None, + return_reference_rotation_scale_rad: float = math.radians(1.0), + return_reference_maximum_command_gap_u8: int = 8, + coupled_rotation_pairs: Sequence[ + tuple[str, str, str, str, float] + ] = (), + coupled_rotation_scale_rad: float = math.radians(3.0), + maximum_coupled_rotation_residual_rad: float | None = None, + ) -> None: + self.roles = tuple(str(role) for role in roles) + self.adjacent_pairs = tuple( + (str(parent), str(child)) + for parent, child in adjacent_pairs + ) + self.normal_alignment_pairs = tuple( + (str(first), str(second)) + for first, second in normal_alignment_pairs + ) + self.coupled_rotation_pairs = tuple( + ( + str(driver_parent), + str(driver_child), + str(follower_parent), + str(follower_child), + float(multiplier), + ) + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + multiplier, + ) in coupled_rotation_pairs + ) + if not self.roles or len(set(self.roles)) != len(self.roles): + raise ValueError("roles must be non-empty and unique") + if any( + parent not in self.roles or child not in self.roles + for parent, child in ( + *self.adjacent_pairs, + *self.normal_alignment_pairs, + ) + ): + raise ValueError("group geometry pairs must reference roles") + self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) + self.maximum_translation_jump_m = float( + maximum_translation_jump_m + ) + self.relative_rotation_scale_rad = float( + relative_rotation_scale_rad + ) + self.relative_translation_scale_m = float( + relative_translation_scale_m + ) + self.reprojection_scale_px = float(reprojection_scale_px) + self.reprojection_weight = float(reprojection_weight) + self.initialization_frames = int(initialization_frames) + self.normal_alignment_scale_rad = float( + normal_alignment_scale_rad + ) + self.maximum_normal_alignment_rad = ( + None + if maximum_normal_alignment_rad is None + else float(maximum_normal_alignment_rad) + ) + self.return_reference_rotation_scale_rad = float( + return_reference_rotation_scale_rad + ) + self.return_reference_maximum_command_gap_u8 = int( + return_reference_maximum_command_gap_u8 + ) + self.coupled_rotation_scale_rad = float( + coupled_rotation_scale_rad + ) + self.maximum_coupled_rotation_residual_rad = ( + None + if maximum_coupled_rotation_residual_rad is None + else float(maximum_coupled_rotation_residual_rad) + ) + reset_seconds = float(reset_after_seconds) + if min( + self.maximum_pose_jump_rad, + self.maximum_translation_jump_m, + self.relative_rotation_scale_rad, + self.relative_translation_scale_m, + self.reprojection_scale_px, + self.normal_alignment_scale_rad, + self.return_reference_rotation_scale_rad, + self.coupled_rotation_scale_rad, + reset_seconds, + ) <= 0.0: + raise ValueError("group tracking scales must be positive") + if self.reprojection_weight < 0.0: + raise ValueError("reprojection_weight must be non-negative") + if self.initialization_frames < 1: + raise ValueError("initialization_frames must be positive") + if self.return_reference_maximum_command_gap_u8 < 0: + raise ValueError( + "return reference maximum command gap must be non-negative" + ) + if ( + self.maximum_normal_alignment_rad is not None + and self.maximum_normal_alignment_rad <= 0.0 + ): + raise ValueError("maximum normal alignment must be positive") + if any( + role not in self.roles + for coupling in self.coupled_rotation_pairs + for role in coupling[:4] + ): + raise ValueError("coupled rotation pairs must reference roles") + if any( + multiplier <= 0.0 + for *_, multiplier in self.coupled_rotation_pairs + ): + raise ValueError("coupled rotation multipliers must be positive") + if ( + self.maximum_coupled_rotation_residual_rad is not None + and self.maximum_coupled_rotation_residual_rad <= 0.0 + ): + raise ValueError( + "maximum coupled rotation residual must be positive" + ) + self.reset_after_ns = int(reset_seconds * 1_000_000_000) + self._previous: dict[str, SquareTagPose] = {} + self._previous_stamp_ns: int | None = None + self._initial_candidates: list[ + dict[str, tuple[SquareTagPose, ...]] + ] = [] + self._initial_stamps_ns: list[int] = [] + self.last_initialization_quality: dict[str, float | str] = {} + self.branch_correction_counts: dict[str, int] = {} + self._decreasing_relative_rotations: dict[ + int, dict[tuple[str, str], Rotation] + ] = {} + self._coupled_reference_rotations: dict[ + tuple[str, str], Rotation + ] = {} + self._task_reference_relative_poses: dict[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] = {} + self.last_missing_roles: tuple[str, ...] = () + + def reset(self, *, preserve_task_reference: bool = False) -> None: + self._previous.clear() + self._previous_stamp_ns = None + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self.last_initialization_quality.clear() + self.branch_correction_counts.clear() + self._decreasing_relative_rotations.clear() + self._coupled_reference_rotations.clear() + self.last_missing_roles = () + if not preserve_task_reference: + self._task_reference_relative_poses.clear() + + def _task_reference_cost( + self, combination: Mapping[str, SquareTagPose] + ) -> float: + residual = 0.0 + for pair, (expected_rotation, expected_translation) in ( + self._task_reference_relative_poses.items() + ): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + residual += ( + float((expected_rotation.inv() * rotation).magnitude()) + / self.return_reference_rotation_scale_rad + + float(np.linalg.norm(translation - expected_translation)) + / self.relative_translation_scale_m + ) + return residual + + def _coupled_rotation_residuals( + self, combination: Mapping[str, SquareTagPose] + ) -> tuple[float, ...]: + if not self.coupled_rotation_pairs: + return () + residuals: list[float] = [] + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + multiplier, + ) in self.coupled_rotation_pairs: + driver_pair = (driver_parent, driver_child) + follower_pair = (follower_parent, follower_child) + if ( + driver_pair not in self._coupled_reference_rotations + or follower_pair not in self._coupled_reference_rotations + ): + return () + driver_rotation = _relative_pose( + combination[driver_parent], combination[driver_child] + )[0] + follower_rotation = _relative_pose( + combination[follower_parent], combination[follower_child] + )[0] + driver_travel = ( + self._coupled_reference_rotations[driver_pair].inv() + * driver_rotation + ).magnitude() + follower_travel = ( + self._coupled_reference_rotations[follower_pair].inv() + * follower_rotation + ).magnitude() + residuals.append( + abs(float(follower_travel) - multiplier * float(driver_travel)) + ) + return tuple(residuals) + + def _informative_coupled_rotation_costs( + self, + combinations: Sequence[Mapping[str, SquareTagPose]], + ) -> tuple[float, ...]: + """Return branch costs only while the weak coupling prior is credible. + + The URDF mimic ratio is useful for distinguishing two planar-IPPE + branches, but it is not measurement truth for a passive joint. Once + every otherwise viable combination disagrees with that ratio, using + it would bias the measured curve (and previously rejected every + frame). In that case fall back to visual continuity for this frame. + """ + residuals = tuple( + self._coupled_rotation_residuals(combination) + for combination in combinations + ) + if not residuals or not any(residuals): + return tuple(0.0 for _ in combinations) + if ( + self.maximum_coupled_rotation_residual_rad is not None + and not any( + values + and max(values) + <= self.maximum_coupled_rotation_residual_rad + for values in residuals + ) + ): + return tuple(0.0 for _ in combinations) + return tuple( + sum(values) / self.coupled_rotation_scale_rad + for values in residuals + ) + + def _return_reference( + self, command_u8: int | None + ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]: + if command_u8 is None or not self._decreasing_relative_rotations: + return {} + command = int(command_u8) + nearest = min( + self._decreasing_relative_rotations, + key=lambda candidate: abs(candidate - command), + ) + if ( + abs(nearest - command) + > self.return_reference_maximum_command_gap_u8 + ): + return {} + references = self._decreasing_relative_rotations[nearest] + commands = sorted(self._decreasing_relative_rotations) + axes: dict[tuple[str, str], np.ndarray | None] = {} + for pair in self.adjacent_pairs: + endpoint_delta = ( + self._decreasing_relative_rotations[commands[-1]][pair].inv() + * self._decreasing_relative_rotations[commands[0]][pair] + ).as_rotvec() + norm = float(np.linalg.norm(endpoint_delta)) + axes[pair] = ( + None + if norm < math.radians(5.0) + else endpoint_delta / norm + ) + return { + pair: (rotation, axes[pair]) + for pair, rotation in references.items() + } + + def _return_reference_cost( + self, + combination: Mapping[str, SquareTagPose], + reference: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray | None] + ], + ) -> float: + residual = 0.0 + for pair, (expected, motion_axis) in reference.items(): + vector = ( + expected.inv() + * _relative_pose( + combination[pair[0]], combination[pair[1]] + )[0] + ).as_rotvec() + if motion_axis is not None: + # The outbound trajectory identifies the physical one-DOF + # motion axis. Do not penalize return travel along that axis: + # it may contain real mechanical hysteresis that calibration + # must measure. A planar-IPPE mirror branch appears primarily + # as a large orthogonal tilt and is rejected by this residual. + vector = vector - motion_axis * float(vector @ motion_axis) + residual += float(np.linalg.norm(vector)) + return residual / self.return_reference_rotation_scale_rad + + def select( + self, + candidates_by_role: Mapping[str, Sequence[SquareTagPose]], + *, + stamp_ns: int, + trajectory_command_u8: int | None = None, + trajectory_direction: str | None = None, + ) -> tuple[dict[str, SquareTagPose] | None, str]: + """Return one mutually consistent pose for every configured role.""" + direction = ( + None + if trajectory_direction is None + else str(trajectory_direction) + ) + if direction not in {None, "decreasing", "increasing"}: + raise ValueError( + "trajectory_direction must be decreasing or increasing" + ) + return_reference = ( + self._return_reference(trajectory_command_u8) + if direction == "increasing" + else {} + ) + candidate_lists = [ + tuple(candidates_by_role.get(role, ())) + for role in self.roles + ] + self.last_missing_roles = tuple( + role + for role, candidates in zip(self.roles, candidate_lists) + if not candidates + ) + if self.last_missing_roles: + return None, "group_missing_pose_candidates" + self.last_missing_roles = () + + combinations = [ + dict(zip(self.roles, combination)) + for combination in product(*candidate_lists) + ] + minimum_errors = { + role: min( + candidate.reprojection_error_px + for candidate in candidates + ) + for role, candidates in zip(self.roles, candidate_lists) + } + stamp = int(stamp_ns) + previous_is_fresh = ( + self._previous_stamp_ns is not None + and 0 <= stamp - self._previous_stamp_ns + <= self.reset_after_ns + and set(self._previous) == set(self.roles) + ) + + if not previous_is_fresh: + if self._previous_stamp_ns is not None: + self._previous.clear() + self._previous_stamp_ns = None + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self.last_initialization_quality.clear() + if self.initialization_frames > 1: + if self._initial_stamps_ns and not ( + 0 <= stamp - self._initial_stamps_ns[-1] + <= self.reset_after_ns + ): + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self._initial_candidates.append( + { + role: tuple(candidates_by_role.get(role, ())) + for role in self.roles + } + ) + self._initial_stamps_ns.append(stamp) + if len(self._initial_candidates) < self.initialization_frames: + return ( + None, + "group_initializing:" + f"{len(self._initial_candidates)}/" + f"{self.initialization_frames}", + ) + selected_path, initialization_quality = ( + select_static_rigid_group_initialization( + self._initial_candidates, + roles=self.roles, + fixed_pairs=self.adjacent_pairs, + reprojection_scale_px=self.reprojection_scale_px, + maximum_pose_jump_rad=self.maximum_pose_jump_rad, + maximum_translation_jump_m=( + self.maximum_translation_jump_m + ), + relative_rotation_scale_rad=( + self.relative_rotation_scale_rad + ), + relative_translation_scale_m=( + self.relative_translation_scale_m + ), + normal_alignment_pairs=( + self.normal_alignment_pairs + ), + normal_alignment_scale_rad=( + self.normal_alignment_scale_rad + ), + task_reference_pairs=( + self._task_reference_relative_poses + ), + task_reference_rotation_scale_rad=( + self.return_reference_rotation_scale_rad + ), + task_reference_translation_scale_m=( + self.relative_translation_scale_m + ), + ) + ) + selected = selected_path[-1] + stamp = self._initial_stamps_ns[-1] + self.last_initialization_quality = dict( + initialization_quality + ) + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + if ( + self.maximum_normal_alignment_rad is not None + and float( + initialization_quality[ + "maximum_normal_alignment_rad" + ] + ) + > self.maximum_normal_alignment_rad + ): + return None, "group_normal_alignment" + else: + coupling_costs = self._informative_coupled_rotation_costs( + combinations + ) + selected = min( + zip(combinations, coupling_costs), + key=lambda item: ( + sum( + pose.reprojection_error_px + for pose in item[0].values() + ) + / self.reprojection_scale_px + + sum( + _normal_alignment_rad( + item[0][first], item[0][second] + ) + for first, second in self.normal_alignment_pairs + ) + / self.normal_alignment_scale_rad + + self._return_reference_cost( + item[0], return_reference + ) + + item[1] + + self._task_reference_cost(item[0]) + ), + )[0] + maximum_alignment = max( + ( + _normal_alignment_rad( + selected[first], selected[second] + ) + for first, second in self.normal_alignment_pairs + ), + default=0.0, + ) + self.last_initialization_quality = { + "maximum_normal_alignment_rad": maximum_alignment + } + if ( + self.maximum_normal_alignment_rad is not None + and maximum_alignment + > self.maximum_normal_alignment_rad + ): + return None, "group_normal_alignment" + else: + previous_relative = { + pair: _relative_pose( + self._previous[pair[0]], + self._previous[pair[1]], + ) + for pair in self.adjacent_pairs + } + base_scored: list[tuple[float, dict[str, SquareTagPose]]] = [] + for combination in combinations: + absolute_rotation_motion = 0.0 + absolute_translation_motion = 0.0 + rejected = False + for role in self.roles: + rotation_motion = rotation_distance_rad( + self._previous[role].quaternion_xyzw, + combination[role].quaternion_xyzw, + ) + translation_motion = float( + np.linalg.norm( + np.asarray( + combination[role].translation_xyz_m, + dtype=float, + ) + - np.asarray( + self._previous[role].translation_xyz_m, + dtype=float, + ) + ) + ) + if ( + rotation_motion > self.maximum_pose_jump_rad + or translation_motion + > self.maximum_translation_jump_m + ): + rejected = True + break + absolute_rotation_motion += rotation_motion + absolute_translation_motion += translation_motion + if rejected: + continue + + relative_rotation_motion = 0.0 + relative_translation_motion = 0.0 + for pair in self.adjacent_pairs: + rotation, translation = _relative_pose( + combination[pair[0]], + combination[pair[1]], + ) + old_rotation, old_translation = previous_relative[pair] + relative_rotation_motion += float( + (old_rotation.inv() * rotation).magnitude() + ) + relative_translation_motion += float( + np.linalg.norm(translation - old_translation) + ) + + reprojection_penalty = sum( + max( + 0.0, + combination[role].reprojection_error_px + - minimum_errors[role], + ) + for role in self.roles + ) / self.reprojection_scale_px + score = ( + absolute_rotation_motion + / self.maximum_pose_jump_rad + + absolute_translation_motion + / self.maximum_translation_jump_m + + relative_rotation_motion + / self.relative_rotation_scale_rad + + relative_translation_motion + / self.relative_translation_scale_m + + self.reprojection_weight * reprojection_penalty + + self._return_reference_cost( + combination, return_reference + ) + ) + base_scored.append((float(score), combination)) + + if not base_scored: + return None, "group_pose_jump" + coupling_costs = self._informative_coupled_rotation_costs( + [combination for _, combination in base_scored] + ) + scored = [ + (base_score + coupling_cost, combination) + for (base_score, combination), coupling_cost in zip( + base_scored, coupling_costs + ) + ] + selected = min(scored, key=lambda item: item[0])[1] + + aligned: dict[str, SquareTagPose] = {} + for role in self.roles: + pose = selected[role] + if previous_is_fresh: + old_quaternion = np.asarray( + self._previous[role].quaternion_xyzw, + dtype=float, + ) + quaternion = np.asarray( + pose.quaternion_xyzw, + dtype=float, + ) + if float(np.dot(old_quaternion, quaternion)) < 0.0: + pose = replace( + pose, + quaternion_xyzw=tuple( + float(value) for value in -quaternion + ), + ) + best_reprojection = min( + candidate_lists[self.roles.index(role)], + key=lambda candidate: candidate.reprojection_error_px, + ) + if pose != best_reprojection: + self.branch_correction_counts[role] = ( + self.branch_correction_counts.get(role, 0) + 1 + ) + aligned[role] = pose + + self._previous = aligned + self._previous_stamp_ns = stamp + if ( + direction == "decreasing" + and trajectory_command_u8 is not None + and not self._coupled_reference_rotations + ): + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + _multiplier, + ) in self.coupled_rotation_pairs: + for pair in ( + (driver_parent, driver_child), + (follower_parent, follower_child), + ): + self._coupled_reference_rotations[pair] = _relative_pose( + aligned[pair[0]], aligned[pair[1]] + )[0] + if direction == "decreasing" and trajectory_command_u8 is not None: + self._decreasing_relative_rotations[ + int(trajectory_command_u8) + ] = { + pair: _relative_pose( + aligned[pair[0]], aligned[pair[1]] + )[0] + for pair in self.adjacent_pairs + } + if not self._task_reference_relative_poses: + self._task_reference_relative_poses = { + pair: _relative_pose( + aligned[pair[0]], aligned[pair[1]] + ) + for pair in self.adjacent_pairs + } + return dict(aligned), "" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py new file mode 100644 index 0000000..238463c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/tracking.py @@ -0,0 +1,324 @@ +"""Tracking for shared square Tag pose estimation.""" + +from __future__ import annotations + +from dataclasses import replace +from typing import Sequence +import math + +from scipy.spatial.transform import Rotation +import cv2 +import numpy as np + +from .ippe import rotation_distance_rad, solve_square_tag_ippe +from .parameters import DEFAULT_POSE_TRACKING_PARAMETERS, DEFAULT_REPROJECTION_TIE_PX +from .types import SquareTagPose + + +def _validate_reprojection_thresholds(maximum_error: float, tie_error: float) -> None: + if not math.isfinite(maximum_error) or maximum_error <= 0: + raise ValueError("maximum_reprojection_error_px must be finite and positive") + if not math.isfinite(tie_error) or not 0 <= tie_error < maximum_error: + raise ValueError( + "reprojection_tie_px must be finite, non-negative and smaller than " + "maximum_reprojection_error_px; image acceptance is not a branch tie" + ) + + +def select_continuous_pose( + candidates: Sequence[SquareTagPose], + *, + previous: SquareTagPose | None, + maximum_reprojection_error_px: float, + reprojection_tie_px: float, + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + maximum_tag_tilt_rad: float, +) -> tuple[SquareTagPose | None, str]: + """Select the best IPPE branch using image fit and temporal continuity.""" + maximum_error = float(maximum_reprojection_error_px) + tie_error = float(reprojection_tie_px) + _validate_reprojection_thresholds(maximum_error, tie_error) + maximum_rotation = float(maximum_pose_jump_rad) + maximum_translation = float(maximum_translation_jump_m) + maximum_tilt = float(maximum_tag_tilt_rad) + if min( + maximum_error, + maximum_rotation, + maximum_translation, + maximum_tilt, + ) <= 0.0: + raise ValueError("PnP selection thresholds must be positive") + + eligible: list[SquareTagPose] = [] + for candidate in candidates: + if candidate.reprojection_error_px > maximum_error: + continue + normal = Rotation.from_quat(candidate.quaternion_xyzw).as_matrix()[:, 2] + tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0))) + if tilt > maximum_tilt: + continue + eligible.append(candidate) + if not eligible: + return None, "no_pose_within_reprojection_or_tilt_limit" + + eligible.sort(key=lambda item: item.reprojection_error_px) + best = eligible[0] + if previous is None: + return best, "" + + # Temporal continuity must only break a genuine planar-PnP tie. The old + # implementation normalised reprojection error by the permissive 1.5 px + # rejection limit, which allowed a stale mirror branch at 0.25 px to beat + # the true branch at e.g. 0.05 px merely because it was closer to the + # preceding (already wrong) pose. Once one IPPE solution has a meaningful + # image-fit advantage, trust it and allow the tracker to leave the stale + # branch even if that correction is a large pose jump. + competitive = [ + candidate + for candidate in eligible + if candidate.reprojection_error_px + <= best.reprojection_error_px + tie_error + ] + if len(competitive) == 1: + return best, "" + + previous_translation = np.asarray(previous.translation_xyz_m, dtype=float) + scored: list[tuple[float, SquareTagPose]] = [] + for candidate in competitive: + rotation_jump = rotation_distance_rad( + previous.quaternion_xyzw, + candidate.quaternion_xyzw, + ) + translation_jump = float( + np.linalg.norm( + np.asarray(candidate.translation_xyz_m, dtype=float) + - previous_translation + ) + ) + if ( + rotation_jump > maximum_rotation + or translation_jump > maximum_translation + ): + continue + score = ( + ( + candidate.reprojection_error_px + - best.reprojection_error_px + ) + / max(tie_error, np.finfo(float).eps) + + rotation_jump / maximum_rotation + + translation_jump / maximum_translation + ) + scored.append((float(score), candidate)) + if not scored: + return None, "pose_jump" + + selected = min(scored, key=lambda item: item[0])[1] + previous_quaternion = np.asarray(previous.quaternion_xyzw, dtype=float) + selected_quaternion = np.asarray(selected.quaternion_xyzw, dtype=float) + if float(np.dot(previous_quaternion, selected_quaternion)) < 0.0: + selected = replace( + selected, + quaternion_xyzw=tuple( + float(value) for value in -selected_quaternion + ), + ) + return selected, "" + + +class SquareTagPoseTracker: + """Maintain the selected planar-PnP branch independently for each tag.""" + + def __init__( + self, + *, + maximum_reprojection_error_px: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px, + reprojection_tie_px: float = DEFAULT_REPROJECTION_TIE_PX, + maximum_pose_jump_rad: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_pose_jump_rad, + maximum_translation_jump_m: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_translation_jump_m, + maximum_tag_tilt_rad: float = DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad, + reset_after_seconds: float = DEFAULT_POSE_TRACKING_PARAMETERS.reset_after_seconds, + ) -> None: + self.maximum_reprojection_error_px = float( + maximum_reprojection_error_px + ) + self.reprojection_tie_px = float(reprojection_tie_px) + _validate_reprojection_thresholds( + self.maximum_reprojection_error_px, self.reprojection_tie_px + ) + self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) + self.maximum_translation_jump_m = float(maximum_translation_jump_m) + self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad) + self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000) + if self.reset_after_ns <= 0: + raise ValueError("reset_after_seconds must be positive") + self._previous: dict[str, tuple[int, SquareTagPose]] = {} + self.last_candidates_by_role: dict[ + str, tuple[SquareTagPose, ...] + ] = {} + self.last_candidate_diagnostics_by_role: dict[ + str, dict[str, float | int] + ] = {} + self.branch_correction_counts: dict[str, int] = {} + + def reset(self) -> None: + self._previous.clear() + self.last_candidates_by_role.clear() + self.last_candidate_diagnostics_by_role.clear() + self.branch_correction_counts.clear() + + def estimate( + self, + role: str, + corners_xy: Sequence[Sequence[float]], + *, + tag_size_m: float, + camera_matrix: Sequence[Sequence[float]], + stamp_ns: int, + reprojection_tie_px: float | None = None, + ) -> tuple[SquareTagPose | None, str]: + try: + candidates = solve_square_tag_ippe( + corners_xy, + tag_size_m=tag_size_m, + camera_matrix=camera_matrix, + ) + except (ValueError, cv2.error): + self.last_candidates_by_role[str(role)] = () + self.last_candidate_diagnostics_by_role[str(role)] = { + "solved_candidate_count": 0, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + return None, "pnp_solve_failed" + if not candidates: + self.last_candidates_by_role[str(role)] = () + self.last_candidate_diagnostics_by_role[str(role)] = { + "solved_candidate_count": 0, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + return None, "pnp_solve_failed" + reprojection_candidates = [ + candidate + for candidate in candidates + if candidate.reprojection_error_px + <= self.maximum_reprojection_error_px + ] + candidate_tilts_rad: list[float] = [] + independent_candidates: list[SquareTagPose] = [] + for candidate in reprojection_candidates: + normal = Rotation.from_quat( + candidate.quaternion_xyzw + ).as_matrix()[:, 2] + tilt = math.acos( + float(np.clip(abs(normal[2]), 0.0, 1.0)) + ) + candidate_tilts_rad.append(float(tilt)) + if tilt <= self.maximum_tag_tilt_rad: + independent_candidates.append(candidate) + + # Candidate generation and candidate selection have different + # contracts. The per-Tag tilt limit protects a pose used without any + # other geometry, but it must not erase a finite, low-reprojection + # IPPE solution before SquareTagGroupPoseTracker can evaluate it + # against the fixed palm reference, the articulated chain and the + # preceding group pose. At a strongly oblique view the planar + # ambiguity is usually smaller, and rejecting both branches at a + # fixed angle caused deterministic mid-sweep holes despite continuous + # image detections. Group tracking therefore receives every + # reprojection-valid candidate; independent tracking below retains the + # original tilt safety gate. + self.last_candidates_by_role[str(role)] = tuple( + reprojection_candidates + ) + diagnostics: dict[str, float | int] = { + "solved_candidate_count": len(candidates), + "reprojection_candidate_count": len(reprojection_candidates), + "independent_tilt_candidate_count": len(independent_candidates), + "minimum_reprojection_error_px": float( + min( + candidate.reprojection_error_px + for candidate in candidates + ) + ), + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + if candidate_tilts_rad: + diagnostics["minimum_candidate_tilt_deg"] = math.degrees( + min(candidate_tilts_rad) + ) + diagnostics["maximum_candidate_tilt_deg"] = math.degrees( + max(candidate_tilts_rad) + ) + self.last_candidate_diagnostics_by_role[str(role)] = diagnostics + if not independent_candidates: + return None, "no_pose_within_reprojection_or_tilt_limit" + + previous_record = self._previous.get(str(role)) + previous: SquareTagPose | None = None + if previous_record is not None: + previous_stamp, previous_pose = previous_record + elapsed = int(stamp_ns) - previous_stamp + if 0 <= elapsed <= self.reset_after_ns: + previous = previous_pose + + selected, reason = select_continuous_pose( + independent_candidates, + previous=previous, + maximum_reprojection_error_px=( + self.maximum_reprojection_error_px + ), + reprojection_tie_px=( + self.reprojection_tie_px + if reprojection_tie_px is None + else float(reprojection_tie_px) + ), + maximum_pose_jump_rad=self.maximum_pose_jump_rad, + maximum_translation_jump_m=self.maximum_translation_jump_m, + maximum_tag_tilt_rad=self.maximum_tag_tilt_rad, + ) + if selected is not None: + if previous is not None: + rotation_jump = rotation_distance_rad( + previous.quaternion_xyzw, + selected.quaternion_xyzw, + ) + translation_jump = float( + np.linalg.norm( + np.asarray(selected.translation_xyz_m, dtype=float) + - np.asarray( + previous.translation_xyz_m, + dtype=float, + ) + ) + ) + if ( + rotation_jump > self.maximum_pose_jump_rad + or translation_jump + > self.maximum_translation_jump_m + ): + key = str(role) + self.branch_correction_counts[key] = ( + self.branch_correction_counts.get(key, 0) + 1 + ) + self._previous[str(role)] = (int(stamp_ns), selected) + return selected, reason diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/trajectory.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/trajectory.py new file mode 100644 index 0000000..a82be08 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/trajectory.py @@ -0,0 +1,424 @@ +"""Trajectory for shared square Tag pose estimation.""" + +from __future__ import annotations + +from itertools import product +from typing import Mapping, Sequence + +from scipy.spatial.transform import Rotation +import numpy as np + +from .ippe import rotation_distance_rad +from .relative import _normal_alignment_rad, _relative_pose +from .types import SquareTagPose + + +def select_rigid_group_trajectory( + frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], + *, + roles: Sequence[str], + fixed_pairs: Sequence[tuple[str, str]], + reprojection_scale_px: float, + rotation_scale_rad: float, + translation_scale_m: float, + pair_geometry: str = "pose", + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float | None = None, +) -> tuple[ + list[dict[str, SquareTagPose]], + dict[str, float | str], +]: + """Resolve planar branches using geometry that should stay rigid. + + Every possible branch combination in the first frame is treated as a + candidate rigid reference. For each such reference, every later frame + independently chooses the combination with the lowest reprojection plus + geometric-drift cost. ``pose`` compares relative rotation and translation; + ``distance`` compares only Euclidean centre distances and therefore does + not allow planar-PnP orientation jitter into centre-trajectory angles. + The globally cheapest reference and path win. + """ + role_names = tuple(str(role) for role in roles) + pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) + normal_pair_names = tuple( + (str(first), str(second)) + for first, second in normal_alignment_pairs + ) + if not frames: + raise ValueError("at least one PnP frame is required") + if len(set(role_names)) != len(role_names) or not role_names: + raise ValueError("roles must be non-empty and unique") + if any( + parent not in role_names or child not in role_names + for parent, child in (*pair_names, *normal_pair_names) + ): + raise ValueError("geometry pairs must reference roles") + reprojection_scale = float(reprojection_scale_px) + rotation_scale = float(rotation_scale_rad) + translation_scale = float(translation_scale_m) + geometry_mode = str(pair_geometry) + normal_scale = ( + rotation_scale + if normal_alignment_scale_rad is None + else float(normal_alignment_scale_rad) + ) + if min( + reprojection_scale, + rotation_scale, + translation_scale, + normal_scale, + ) <= 0.0: + raise ValueError("trajectory selection scales must be positive") + if geometry_mode not in {"pose", "distance"}: + raise ValueError("pair_geometry must be pose or distance") + + combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] + for frame in frames: + candidate_lists = [tuple(frame.get(role, ())) for role in role_names] + if any(not candidates for candidates in candidate_lists): + raise ValueError("every frame must contain every requested role") + combinations_by_frame.append( + [ + dict(zip(role_names, combination)) + for combination in product(*candidate_lists) + ] + ) + + best_total = float("inf") + best_path: list[dict[str, SquareTagPose]] | None = None + + def emission( + combination: Mapping[str, SquareTagPose], + reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ], + reference_distances: Mapping[tuple[str, str], float], + ) -> tuple[float, float, float]: + reprojection_cost = sum( + pose.reprojection_error_px + for pose in combination.values() + ) / reprojection_scale + normal_alignment_cost = sum( + _normal_alignment_rad( + combination[first], combination[second] + ) + for first, second in normal_pair_names + ) / normal_scale + rotation_drifts: list[float] = [] + translation_drifts: list[float] = [] + distance_drifts: list[float] = [] + for pair, ( + reference_rotation, + reference_translation, + ) in reference_pairs.items(): + rotation, translation = _relative_pose( + combination[pair[0]], + combination[pair[1]], + ) + rotation_drifts.append( + float( + (reference_rotation.inv() * rotation).magnitude() + ) + ) + translation_drifts.append( + float( + np.linalg.norm( + translation - reference_translation + ) + ) + ) + current_distance = float( + np.linalg.norm( + np.asarray( + combination[pair[1]].translation_xyz_m, + dtype=float, + ) + - np.asarray( + combination[pair[0]].translation_xyz_m, + dtype=float, + ) + ) + ) + distance_drifts.append( + abs(current_distance - reference_distances[pair]) + ) + if geometry_mode == "distance": + geometry_cost = sum(distance_drifts) / translation_scale + else: + geometry_cost = ( + sum(rotation_drifts) / rotation_scale + + sum(translation_drifts) / translation_scale + ) + return ( + reprojection_cost + geometry_cost + normal_alignment_cost, + max(rotation_drifts, default=0.0), + ( + max(distance_drifts, default=0.0) + if geometry_mode == "distance" + else max(translation_drifts, default=0.0) + ), + ) + + def transition_cost( + previous: Mapping[str, SquareTagPose], + current: Mapping[str, SquareTagPose], + ) -> float: + rotation_motion = sum( + rotation_distance_rad( + previous[role].quaternion_xyzw, + current[role].quaternion_xyzw, + ) + for role in role_names + ) + translation_motion = sum( + float( + np.linalg.norm( + np.asarray(current[role].translation_xyz_m) + - np.asarray(previous[role].translation_xyz_m) + ) + ) + for role in role_names + ) + if geometry_mode == "distance": + return translation_motion / translation_scale + return ( + rotation_motion / rotation_scale + + translation_motion / translation_scale + ) + + for reference_index, reference_combination in enumerate( + combinations_by_frame[0] + ): + reference_pairs = { + pair: _relative_pose( + reference_combination[pair[0]], + reference_combination[pair[1]], + ) + for pair in pair_names + } + reference_distances = { + pair: float( + np.linalg.norm( + np.asarray( + reference_combination[pair[1]].translation_xyz_m, + dtype=float, + ) + - np.asarray( + reference_combination[pair[0]].translation_xyz_m, + dtype=float, + ) + ) + ) + for pair in pair_names + } + first_emission = emission( + reference_combination, + reference_pairs, + reference_distances, + ) + previous_costs = np.full( + len(combinations_by_frame[0]), + np.inf, + dtype=float, + ) + previous_costs[reference_index] = first_emission[0] + back_pointers: list[list[int]] = [] + for frame_index in range(1, len(combinations_by_frame)): + previous_combinations = combinations_by_frame[frame_index - 1] + combinations = combinations_by_frame[frame_index] + frame_emissions = [ + emission( + combination, + reference_pairs, + reference_distances, + ) + for combination in combinations + ] + current_costs = np.full(len(combinations), np.inf, dtype=float) + frame_back_pointers: list[int] = [] + for current_index, combination in enumerate(combinations): + transition_costs = [ + previous_costs[previous_index] + + transition_cost( + previous_combination, + combination, + ) + for previous_index, previous_combination in enumerate( + previous_combinations + ) + ] + best_previous = int(np.argmin(transition_costs)) + frame_back_pointers.append(best_previous) + current_costs[current_index] = ( + transition_costs[best_previous] + + frame_emissions[current_index][0] + ) + back_pointers.append(frame_back_pointers) + previous_costs = current_costs + + final_index = int(np.argmin(previous_costs)) + total = float(previous_costs[final_index]) + path_indices = [final_index] + for frame_back_pointers in reversed(back_pointers): + path_indices.append( + frame_back_pointers[path_indices[-1]] + ) + path_indices.reverse() + path = [ + combinations[index] + for combinations, index in zip( + combinations_by_frame, + path_indices, + ) + ] + if total < best_total: + best_total = total + best_path = path + + if best_path is None: + raise RuntimeError("trajectory branch selection produced no path") + + # The marker-to-marker mounting transforms are unknown, so the rigid + # reference must be estimated from the complete sweep. Using frame zero + # as both the optimisation seed and the reported quality reference made + # one noisy endpoint frame look like drift in every other frame. A + # rotation medoid and component-wise translation median are insensitive + # to that endpoint noise while still exposing a persistent mirror branch. + robust_reference_pairs: dict[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] = {} + for pair in pair_names: + pair_poses = [ + _relative_pose(frame[pair[0]], frame[pair[1]]) + for frame in best_path + ] + pair_rotations = [pose[0] for pose in pair_poses] + angular_costs = np.asarray( + [ + sum( + float((candidate.inv() * other).magnitude()) + for other in pair_rotations + ) + for candidate in pair_rotations + ], + dtype=float, + ) + rotation_medoid = pair_rotations[int(np.argmin(angular_costs))] + translation_median = np.median( + np.asarray([pose[1] for pose in pair_poses], dtype=float), + axis=0, + ) + robust_reference_pairs[pair] = ( + rotation_medoid, + translation_median, + ) + + rotation_drifts_by_frame: list[float] = [] + translation_drifts_by_frame: list[float] = [] + pair_distances_by_pair = { + pair: np.asarray( + [ + np.linalg.norm( + np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) + - np.asarray( + frame[pair[0]].translation_xyz_m, dtype=float + ) + ) + for frame in best_path + ], + dtype=float, + ) + for pair in pair_names + } + robust_pair_distances = { + pair: float(np.median(distances)) + for pair, distances in pair_distances_by_pair.items() + } + distance_drifts_by_frame: list[float] = [] + for frame in best_path: + frame_rotation_drifts: list[float] = [] + frame_translation_drifts: list[float] = [] + frame_distance_drifts: list[float] = [] + for pair, ( + reference_rotation, + reference_translation, + ) in robust_reference_pairs.items(): + rotation, translation = _relative_pose( + frame[pair[0]], frame[pair[1]] + ) + frame_rotation_drifts.append( + float((reference_rotation.inv() * rotation).magnitude()) + ) + frame_translation_drifts.append( + float(np.linalg.norm(translation - reference_translation)) + ) + distance = float( + np.linalg.norm( + np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) + - np.asarray( + frame[pair[0]].translation_xyz_m, dtype=float + ) + ) + ) + frame_distance_drifts.append( + abs(distance - robust_pair_distances[pair]) + ) + rotation_drifts_by_frame.append( + max(frame_rotation_drifts, default=0.0) + ) + translation_drifts_by_frame.append( + max(frame_translation_drifts, default=0.0) + ) + distance_drifts_by_frame.append( + max(frame_distance_drifts, default=0.0) + ) + + rotation_drifts = np.asarray(rotation_drifts_by_frame, dtype=float) + translation_drifts = np.asarray( + translation_drifts_by_frame, dtype=float + ) + distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float) + normal_alignments = np.asarray( + [ + max( + ( + _normal_alignment_rad(frame[first], frame[second]) + for first, second in normal_pair_names + ), + default=0.0, + ) + for frame in best_path + ], + dtype=float, + ) + return best_path, { + "total_cost": float(best_total), + "pair_geometry": geometry_mode, + "maximum_normal_alignment_rad": float( + np.max(normal_alignments, initial=0.0) + ), + "maximum_pair_rotation_drift_rad": float( + np.max(rotation_drifts, initial=0.0) + ), + "p95_pair_rotation_drift_rad": float( + np.percentile(rotation_drifts, 95.0) + ), + "median_pair_rotation_drift_rad": float( + np.median(rotation_drifts) + ), + "maximum_pair_translation_drift_m": float( + np.max(translation_drifts, initial=0.0) + ), + "p95_pair_translation_drift_m": float( + np.percentile(translation_drifts, 95.0) + ), + "maximum_pair_distance_drift_m": float( + np.max(distance_drifts, initial=0.0) + ), + "p95_pair_distance_drift_m": float( + np.percentile(distance_drifts, 95.0) + ), + "median_pair_distance_drift_m": float( + np.median(distance_drifts) + ), + } diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/types.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/types.py new file mode 100644 index 0000000..8eb7a8d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/tag_pose/types.py @@ -0,0 +1,14 @@ +"""Types for shared square Tag pose estimation.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class SquareTagPose: + """One tag-to-camera pose candidate returned by IPPE.""" + + quaternion_xyzw: tuple[float, float, float, float] + translation_xyz_m: tuple[float, float, float] + reprojection_error_px: float diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py deleted file mode 100644 index ad28c51..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -"""Task acceptance and final-session solver contracts.""" - -from .interfaces import ( - SessionSolution, - SessionSolver, - TaskEvaluation, - TaskEvaluator, -) - -__all__ = [ - "SessionSolution", - "SessionSolver", - "TaskEvaluation", - "TaskEvaluator", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py b/src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py deleted file mode 100644 index 023baeb..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py +++ /dev/null @@ -1,42 +0,0 @@ -"""Shared evaluator and final-solver interfaces.""" - -from __future__ import annotations - -from dataclasses import dataclass, field -from typing import Any, Mapping, Protocol, Sequence - -from ..domain import CalibrationProfile, SampleRecord, TaskSpec - - -@dataclass(frozen=True) -class TaskEvaluation: - accepted: bool - failures: tuple[Mapping[str, Any], ...] = () - rescan_measurements: frozenset[str] = frozenset() - rescan_cycles: frozenset[int] = frozenset() - - -@dataclass(frozen=True) -class SessionSolution: - passed: bool - calibration: Mapping[str, Any] - zero_offsets_rad: Mapping[str, float] - failures: tuple[Mapping[str, Any], ...] = () - diagnostics: Mapping[str, Any] = field(default_factory=dict) - - -class TaskEvaluator(Protocol): - def evaluate_task( - self, - profile: CalibrationProfile, - task: TaskSpec, - samples: Sequence[SampleRecord], - ) -> TaskEvaluation: ... - - -class SessionSolver(Protocol): - def solve_session( - self, - profile: CalibrationProfile, - samples: Sequence[SampleRecord], - ) -> SessionSolution: ... diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py index 46c1413..6766385 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py @@ -1,12 +1,22 @@ """URDF correction authorization and validation types.""" -from .plan import UrdfCorrectionPlan, build_correction_plan +from .plan import ( + StandardUrdfPlan, UrdfCorrectionPlan, build_correction_plan, + build_standard_correction_plan, +) +from .validate import ( + URDF_FIELD_TO_XML, + validate_runtime_curve_limits, + validate_structural_urdf_diff, +) from .patch import ( MujocoEqualityPatch, UrdfJointPatch, UrdfPatchSet, apply_urdf_patch_text, + corrected_origin_rpy, materialize_relative_mesh_assets, + parse_vector3, validate_urdf_mimic_ranges, write_urdf_patches, ) @@ -14,11 +24,18 @@ from .patch import ( __all__ = [ "MujocoEqualityPatch", "UrdfCorrectionPlan", + "StandardUrdfPlan", "UrdfJointPatch", "UrdfPatchSet", + "URDF_FIELD_TO_XML", "apply_urdf_patch_text", "build_correction_plan", + "build_standard_correction_plan", + "corrected_origin_rpy", "materialize_relative_mesh_assets", + "parse_vector3", "validate_urdf_mimic_ranges", + "validate_runtime_curve_limits", + "validate_structural_urdf_diff", "write_urdf_patches", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py new file mode 100644 index 0000000..01be3ee --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/acceptance.py @@ -0,0 +1,312 @@ +"""Acceptance of serialized standard-URDF motion, not a substitute JSON model.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import math +from pathlib import Path +from typing import Any, Mapping, Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from .kinematics import UrdfKinematicModel +from .patch import validate_urdf_mimic_ranges +from .validate import validate_runtime_curve_limits, validate_structural_urdf_diff + + +@dataclass(frozen=True) +class JointHoldout: + """Independent observation in the corrected coordinate convention. + + Inputs retain their recorded SDK domain. No validation-time tag mounting, + zero, branch selection, curve fit or base registration is permitted. + """ + + sample_id: str + joint: str + sdk_values: tuple[float, ...] + observed_rad: float + direction: str + cycle: int = 3 + # Optional independent child-link pose in the *training-locked* base frame. + base_from_link: tuple[tuple[float, ...], ...] | None = None + # An active scan direction must not be assigned to every held axis. + sdk_directions_by_joint: Mapping[str, str] | None = None + + +@dataclass(frozen=True) +class ErrorMetrics: + count: int + mae_deg: float + p95_deg: float + maximum_deg: float + + @property + def passed(self) -> bool: + return self.count > 0 and self.mae_deg <= 1.0 and self.p95_deg <= 2.0 and self.maximum_deg <= 3.0 + + +def angular_metrics(errors_rad: Sequence[float]) -> ErrorMetrics: + errors = np.abs(np.asarray(errors_rad, dtype=float)) + if errors.ndim != 1 or not errors.size or not np.all(np.isfinite(errors)): + raise ValueError("angular validation requires finite independent errors") + degrees = np.rad2deg(errors) + return ErrorMetrics(len(errors), float(np.mean(degrees)), float(np.percentile(degrees, 95)), float(np.max(degrees))) + + +class SerializedJointMapping: + """Read the numeric legacy v4/v6/v7 curve shapes without model imports. + + Byte payloads preserve their deployed nearest-integer lookup; continuous + payloads interpolate explicit knots. Neither path silently clamps inputs. + """ + + def __init__(self, payload: Mapping[str, Any], *, input_kind=None) -> None: + self.payload = payload + self.compact = payload.get("format") == "unified_calibration_v2" + input_kind = input_kind or ("command" if self.compact else "feedback") + self.unified = payload.get("format") == "unified_calibration_v1" + if self.compact: + if input_kind != "command" or payload.get("schema_version") != 2: + raise ValueError("compact JSON is command-only; feedback mapping belongs to the report") + unit = payload.get("input_unit") + if unit not in {"rad", "u8"}: + raise ValueError("invalid compact input unit") + joints = {} + for name, row in payload.get("joints", {}).items(): + expected = {"sdk_channel", "angle_rad"} | ({"input_values"} if unit == "rad" else set()) + index = row.get("sdk_channel") + if set(row) != expected or type(index) is not int or index < 0: + raise ValueError(f"invalid compact joint/channel fields:{name}") + joints[name] = {"motor_index": index, "angle_rad": row["angle_rad"], + "curve_input_knots_rad": row.get("input_values", ())} + payload = {"joints": joints, "curve_input_domain": f"command_{unit}"} + elif self.unified: + if input_kind not in {"command", "feedback"} or payload.get("schema_version") != 1: + raise ValueError("unsupported unified mapping kind/version") + joints = {} + unit = payload["command_unit"] + count = len(payload["command_names"]) + if unit not in {"rad", "u8"} or count == 0: + raise ValueError("invalid unified SDK layout") + for name, row in payload["joints"].items(): + if row.get("urdf_joint") != name: + raise ValueError("unified keys must be original URDF joint names") + if row.get("passive"): + if "command_to_rad" in row or "feedback_to_rad" in row: + raise ValueError("passive runtime curves cannot bypass standard URDF mimic") + continue + data = row[f"{input_kind}_to_rad"] + if (data["input_domain"] != f"{input_kind}_{unit}" or data["input_unit"] != unit + or data["output_unit"] != "rad" or data["interpolation"] != "piecewise_linear" + or data["extrapolation"] != "reject" or not 0 <= data["channel_index"] < count + or data["valid_input_range"] != [data["knots"][0], data["knots"][-1]]): + raise ValueError("unified mapping coordinate contract changed") + joints[name] = {**data, "motor_index": data["channel_index"], + "curve_input_knots_rad": data["knots"], "urdf_joint": name} + payload = {**payload, "joints": joints, "curve_input_domain": f"{input_kind}_{unit}"} + self.domain = str(payload.get("curve_input_domain", "command_u8" if payload.get("schema_version") == 4 else "")) + if self.domain not in {"command_u8", "feedback_u8", "feedback_rad", "command_rad"}: + raise ValueError("serialized SDK input domain is missing or unsupported") + self.joints: dict[str, Mapping[str, Any]] = {} + for name, row in payload.get("joints", {}).items(): + target = str(row.get("urdf_joint", name)) + if target in self.joints: + raise ValueError(f"duplicate serialized joint: {target}") + self.joints[target] = row + if int(row["motor_index"]) < 0: + raise ValueError("negative serialized motor index") + for branch in ("angle_rad", "increasing_rad", "decreasing_rad"): + values = np.asarray(row.get(branch, row["angle_rad"]), dtype=float) + if values.ndim != 1 or len(values) < 2 or not np.all(np.isfinite(values)): + raise ValueError(f"invalid serialized curve: {target}:{branch}") + differences = np.diff(values) + if not (np.all(differences >= -1e-9) or np.all(differences <= 1e-9)): + raise ValueError(f"serialized curve is not monotonic: {target}:{branch}") + if self.domain.endswith("_u8") and not self.unified: + if len(values) != 256: + raise ValueError("byte mapping requires 256 entries") + else: + knots = np.asarray(row.get("curve_input_knots_rad", ()), dtype=float) + if knots.shape != values.shape or not np.all(np.isfinite(knots)) or np.any(np.diff(knots) <= 0): + raise ValueError(f"continuous curve requires strictly ordered finite knots: {target}") + if not self.joints: + raise ValueError("serialized calibration has no joint mappings") + + def evaluate(self, sdk_values: Sequence[float], direction: str, *, active_joints: Sequence[str], directions_by_joint: Mapping[str, str] | None = None) -> dict[str, float]: + if direction not in {"increasing", "decreasing", ""}: + raise ValueError("invalid recorded SDK direction") + output: dict[str, float] = {} + for name in active_joints: + row = self.joints.get(name) + if row is None: + raise ValueError(f"serialized mapping is missing active joint: {name}") + index = int(row["motor_index"]) + if index >= len(sdk_values): + raise ValueError("holdout SDK vector has missing channels") + value = float(sdk_values[index]) + if not math.isfinite(value): + raise ValueError("holdout SDK input is nonfinite") + selected = direction if directions_by_joint is None else directions_by_joint.get(name, "") + if selected not in {"increasing", "decreasing", ""}: + raise ValueError("invalid per-joint SDK direction") + if self.domain.endswith("_rad") and selected: + increasing_branch = str(row.get("raw_increasing_curve_branch", "increasing")) + if increasing_branch not in {"increasing", "decreasing"}: + raise ValueError("invalid serialized direction convention") + selected = increasing_branch if selected == "increasing" else ("decreasing" if increasing_branch == "increasing" else "increasing") + values = row.get(f"{selected}_rad", row["angle_rad"]) if selected else row["angle_rad"] + if self.domain.endswith("_u8") and not self.unified: + if not 0 <= value <= 255: + raise ValueError("holdout input is outside serialized byte domain") + angle = float(values[int(math.floor(value + 0.5))]) + else: + knots = row["curve_input_knots_rad"] + if self.domain.endswith("_u8"): + value = float(math.floor(value+0.5)) + if not knots[0] <= value <= knots[-1]: + raise ValueError(f"holdout input is outside serialized knot domain: {name}") + angle = float(np.interp(value, knots, values)) + output[name] = angle + return output + + +def validate_standard_urdf_holdout( + *, corrected_urdf: str | Path, payload: Mapping[str, Any], + observations: Sequence[JointHoldout], required_joints: Sequence[str], + required_pose_joints: Sequence[str] = (), holdout_cycle: int = 3, + minimum_samples: int = 40, +) -> dict[str, ErrorMetrics]: + """Resolve passive joints exclusively from the written URDF and validate. + + A perfect nonlinear passive JSON curve cannot make a wrong linear URDF + pass. Missing observations, duplicate-only samples and training rows fail. + """ + model = UrdfKinematicModel(corrected_urdf) + mapping = SerializedJointMapping(payload) + required = frozenset(required_joints) + pose_required = frozenset(required_pose_joints) + if not required or not pose_required <= required or not required <= model.joints.keys(): + raise ValueError("holdout requires declared, existing measured joints") + if minimum_samples < 1: + raise ValueError("minimum holdout samples must be positive") + active = tuple(name for name, joint in model.joints.items() if joint.kind != "fixed" and joint.mimic_joint is None) + errors: dict[str, list[float]] = {name: [] for name in required} + pose_errors: dict[str, list[tuple[float, float]]] = {name: [] for name in pose_required} + seen: set[tuple[str, str]] = set() + for row in observations: + if row.cycle != holdout_cycle: + raise ValueError("training data cannot enter serialized URDF holdout") + if row.joint not in required: + continue + identity = (row.joint, row.sample_id) + if not row.sample_id or identity in seen: + raise ValueError("holdout sample identity is empty or duplicated") + seen.add(identity) + if not math.isfinite(row.observed_rad): + raise ValueError("nonfinite holdout observation") + angles = mapping.evaluate(row.sdk_values, row.direction, active_joints=active, + directions_by_joint=row.sdk_directions_by_joint) + resolved = model.resolve_angles(angles) + for name, angle in resolved.items(): + joint = model.joints[name] + if joint.lower is not None and not joint.lower - 1e-9 <= angle <= joint.upper + 1e-9: + raise ValueError(f"serialized URDF motion exceeds joint limit: {name}") + errors[row.joint].append(resolved[row.joint] - row.observed_rad) + if row.joint in pose_required: + observed = np.asarray(row.base_from_link, dtype=float) + if observed.shape != (4, 4) or not np.all(np.isfinite(observed)): + raise ValueError(f"missing independent locked-base pose: {row.joint}") + rotation = observed[:3, :3] + if not np.allclose(observed[3], (0, 0, 0, 1), atol=1e-9) or not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-6) or not np.isclose(np.linalg.det(rotation), 1, atol=1e-6): + raise ValueError("holdout pose is not a rigid transform") + predicted = model.link_transform(row.joint, zero_offsets={}, joint_angles=angles) + angular = float(Rotation.from_matrix(predicted[:3, :3].T @ rotation).magnitude()) + translation = float(np.linalg.norm(predicted[:3, 3] - observed[:3, 3])) + pose_errors[row.joint].append((angular, translation)) + result: dict[str, ErrorMetrics] = {} + for name, values in errors.items(): + if len(values) < minimum_samples: + raise ValueError(f"missing independent URDF holdout samples: {name}:{len(values)}") + metrics = angular_metrics(values) + if not metrics.passed: + code = "standard_urdf_mimic_not_accurate" if model.joints[name].mimic_joint else "serialized_active_mapping_not_accurate" + raise ValueError(f"{code}:{name}:mae={metrics.mae_deg:.6f}:p95={metrics.p95_deg:.6f}:max={metrics.maximum_deg:.6f}") + result[name] = metrics + for name, values in pose_errors.items(): + metrics = angular_metrics([value[0] for value in values]) + # Existing independent combination-pose acceptance: 3 mm P95 / 2 deg P95. + if not metrics.passed or float(np.percentile([value[1] for value in values], 95)) > 0.003: + raise ValueError(f"serialized_urdf_spatial_holdout_failed:{name}") + return result + + +def validate_artifact_structure(*, source_urdf: str | Path, corrected_urdf: str | Path, + source_sha256: str, authorized_fields: Mapping[str, Sequence[str]], + payload: Mapping[str, Any], zero_offsets_rad: Mapping[str, float]) -> tuple[str, ...]: + """Validate physical ranges in CAD coordinates, including coordinate shifts.""" + if len(source_sha256) != 64 or hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() != source_sha256: + raise ValueError("protected source URDF SHA256 mismatch") + changes = validate_structural_urdf_diff(source_urdf, corrected_urdf, + authorized_fields=authorized_fields, expected_source_sha256=source_sha256) + before, after = UrdfKinematicModel(source_urdf), UrdfKinematicModel(corrected_urdf) + if not set(zero_offsets_rad) <= before.joints.keys(): + raise ValueError("declared coordinate zero targets missing joints") + # No model bypass, no inherited-CAD excess allowance at final acceptance. + validate_urdf_mimic_ranges(corrected_urdf) + for name, joint in after.joints.items(): + original = before.joints[name] + delta = float(zero_offsets_rad.get(name, 0.0)) + if not math.isfinite(delta): + raise ValueError("nonfinite coordinate zero") + if joint.lower is not None and original.lower is not None: + if joint.lower + delta < original.lower - 1e-8 or joint.upper + delta > original.upper + 1e-8: + raise ValueError(f"corrected range exceeds source mechanical range: {name}") + expected = original.origin @ before._motion(original, delta) + if not np.allclose(expected, joint.origin, atol=1e-8, rtol=0): + raise ValueError(f"URDF origin does not match declared coordinate zero: {name}") + mapping = SerializedJointMapping(payload) + if mapping.compact: + validate_compact_urdf_tables(payload, after) + for branch in ("angle_rad", "increasing_rad", "decreasing_rad"): + validate_runtime_curve_limits(corrected_urdf, + {name: row.get(branch, row["angle_rad"]) for name, row in mapping.joints.items()}) + if payload.get("format") == "unified_calibration_v1": + commands = SerializedJointMapping(payload, input_kind="command") + for branch in ("angle_rad", "increasing_rad", "decreasing_rad"): + validate_runtime_curve_limits(corrected_urdf, + {name: row[branch] for name, row in commands.joints.items()}) + for name, row in payload["joints"].items(): + if row.get("passive"): + joint = after.joints[name] + expected = {"joint": joint.mimic_joint, "multiplier": joint.mimic_multiplier, + "offset_rad": joint.mimic_offset} + actual = row.get("mimic", {}) + if (actual.get("joint") != expected["joint"] or any(not math.isclose( + float(actual.get(field, float("nan"))), expected[field], abs_tol=1e-8, rel_tol=0) + for field in ("multiplier", "offset_rad"))): + raise ValueError(f"JSON mimic differs from exported URDF:{name}") + return changes + + +def validate_compact_urdf_tables(payload, model): + """Redundant passive lookup entries must equal the written URDF, recursively.""" + mapping = SerializedJointMapping(payload, input_kind="command") + moving = {name for name, joint in model.joints.items() if joint.kind != "fixed"} + if set(mapping.joints) != moving: + raise ValueError("compact JSON joints differ from the exported URDF") + for name, row in mapping.joints.items(): + joint = model.joints[name] + values = np.asarray(row["angle_rad"]) + if joint.lower is not None and (np.min(values) < joint.lower-1e-9 or np.max(values) > joint.upper+1e-9): + raise ValueError(f"compact table exceeds exported URDF range:{name}") + if joint.mimic_joint: + parent = mapping.joints[joint.mimic_joint] + if (row["motor_index"] != parent["motor_index"] + or row["curve_input_knots_rad"] != parent["curve_input_knots_rad"] + or len(values) != len(parent["angle_rad"]) + or not np.allclose(values, joint.mimic_multiplier*np.asarray(parent["angle_rad"]) + + joint.mimic_offset, atol=1e-8, rtol=0)): + raise ValueError(f"compact passive table differs from exported URDF mimic:{name}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py new file mode 100644 index 0000000..208d26e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/kinematics.py @@ -0,0 +1,200 @@ +"""Standard URDF kinematics; independent of SDK, ROS and product profiles. + +Measured passive angles are useful to estimate geometry, but release validation +must resolve the *serialized* mimic graph, never those independent observations. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from pathlib import Path +from typing import Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + + +def _parse_triplet(value: str) -> np.ndarray: + result = np.asarray([float(v) for v in str(value).split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF vector: {value}") + return result + + +def _axis_rotation(axis: Sequence[float], angle: float) -> np.ndarray: + vector = np.asarray(axis, dtype=float) + if vector.shape != (3,) or not np.all(np.isfinite(vector)): + raise ValueError("joint axis must be a finite 3-vector") + norm = float(np.linalg.norm(vector)) + if norm <= 1e-12 or not math.isfinite(float(angle)): + raise ValueError("joint axis/angle is invalid") + transform = np.eye(4) + transform[:3, :3] = Rotation.from_rotvec(vector / norm * float(angle)).as_matrix() + return transform + + +@dataclass(frozen=True) +class UrdfJoint: + name: str + parent: str + child: str + origin: np.ndarray + axis: np.ndarray + mimic_joint: str | None + mimic_multiplier: float + mimic_offset: float + kind: str = "revolute" + lower: float | None = None + upper: float | None = None + + +class UrdfKinematicModel: + def __init__(self, source: str | Path) -> None: + self.source = Path(source).expanduser().resolve() + root = ET.parse(self.source).getroot() + if root.tag != "robot": + raise ValueError("URDF root must be robot") + names = [element.get("name", "") for element in root.findall("link")] + if not names or "" in names or len(set(names)) != len(names): + raise ValueError("URDF link names must be nonempty and unique") + self.links = frozenset(names) + self.joints: dict[str, UrdfJoint] = {} + self.parent_joint_by_child: dict[str, str] = {} + for element in root.findall("joint"): + name = element.get("name", "") + kind = element.get("type", "") + if not name or name in self.joints: + raise ValueError("URDF joint names must be nonempty and unique") + if kind not in {"fixed", "revolute", "continuous", "prismatic"}: + raise ValueError(f"unsupported calibration joint type: {name}:{kind}") + parent, child = element.find("parent"), element.find("child") + if parent is None or child is None: + raise ValueError(f"URDF joint is missing parent/child: {name}") + parent_name, child_name = parent.get("link", ""), child.get("link", "") + if parent_name not in self.links or child_name not in self.links: + raise ValueError(f"URDF joint references a missing link: {name}") + if child_name in self.parent_joint_by_child: + raise ValueError(f"URDF link has multiple parents: {child_name}") + origin_node, axis_node = element.find("origin"), element.find("axis") + xyz = _parse_triplet("0 0 0" if origin_node is None else origin_node.get("xyz", "0 0 0")) + rpy = _parse_triplet("0 0 0" if origin_node is None else origin_node.get("rpy", "0 0 0")) + axis = _parse_triplet("1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")) + if float(np.linalg.norm(axis)) <= 1e-12: + raise ValueError(f"URDF joint has a zero axis: {name}") + axis /= np.linalg.norm(axis) + origin = np.eye(4) + origin[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + origin[:3, 3] = xyz + mimic, limit = element.find("mimic"), element.find("limit") + multiplier = 1.0 if mimic is None else float(mimic.get("multiplier", "1")) + offset = 0.0 if mimic is None else float(mimic.get("offset", "0")) + if not math.isfinite(multiplier) or not math.isfinite(offset): + raise ValueError(f"URDF mimic is nonfinite: {name}") + lower = None if limit is None or kind in {"fixed", "continuous"} else float(limit.get("lower", "nan")) + upper = None if lower is None else float(limit.get("upper", "nan")) + if lower is not None and (not math.isfinite(lower) or not math.isfinite(upper) or lower > upper): + raise ValueError(f"URDF joint has invalid limits: {name}") + self.joints[name] = UrdfJoint(name, parent_name, child_name, origin, axis, + None if mimic is None else mimic.get("joint", ""), multiplier, offset, + kind, lower, upper) + self.parent_joint_by_child[child_name] = name + roots = self.links - self.parent_joint_by_child.keys() + if len(roots) != 1: + raise ValueError("URDF must have exactly one root link") + self.root_link = next(iter(roots)) + for name in self.joints: + self._chain(name) + self.resolve_angles({}) # Validate the complete mimic graph, including siblings. + + def _chain(self, target_joint: str) -> list[UrdfJoint]: + if target_joint not in self.joints: + raise ValueError(f"URDF is missing joint {target_joint}") + chain: list[UrdfJoint] = [] + seen: set[str] = set() + current = self.joints[target_joint] + while True: + if current.name in seen: + raise ValueError(f"URDF kinematic cycle: {current.name}") + seen.add(current.name) + chain.append(current) + parent = self.parent_joint_by_child.get(current.parent) + if parent is None: + if current.parent != self.root_link: + raise ValueError("URDF contains a disconnected joint") + return list(reversed(chain)) + current = self.joints[parent] + + def resolve_angles(self, joint_angles: Mapping[str, float], *, independent_mimic_angles: bool = False) -> dict[str, float]: + supplied = {str(k): float(v) for k, v in joint_angles.items()} + if not all(math.isfinite(v) for v in supplied.values()): + raise ValueError("joint angles must be finite") + resolved: dict[str, float] = {} + visiting: set[str] = set() + + def resolve(name: str) -> float: + if name in resolved: + return resolved[name] + if name in visiting or name not in self.joints: + raise ValueError(f"invalid URDF mimic dependency: {name}") + visiting.add(name) + joint = self.joints[name] + if joint.kind == "fixed": + angle = 0.0 + elif joint.mimic_joint is not None and not (independent_mimic_angles and name in supplied): + angle = joint.mimic_multiplier * resolve(joint.mimic_joint) + joint.mimic_offset + else: + angle = supplied.get(name, 0.0) + if not math.isfinite(angle): + raise ValueError(f"nonfinite resolved URDF angle: {name}") + visiting.remove(name) + resolved[name] = angle + return angle + + for name in self.joints: + resolve(name) + return resolved + + @staticmethod + def _motion(joint: UrdfJoint, angle: float) -> np.ndarray: + if joint.kind == "fixed": + return np.eye(4) + if joint.kind == "prismatic": + transform = np.eye(4) + transform[:3, 3] = joint.axis * angle + return transform + return _axis_rotation(joint.axis, angle) + + def link_transform(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float], independent_mimic_angles: bool = False) -> np.ndarray: + """Base-to-child-link pose. Independent passive input is measurement-only.""" + resolved = self.resolve_angles(joint_angles, independent_mimic_angles=independent_mimic_angles) + transform = np.eye(4) + for joint in self._chain(target_joint): + transform = transform @ joint.origin @ self._motion(joint, float(zero_offsets.get(joint.name, 0.0)) + resolved[joint.name]) + return transform + + def axis_line(self, target_joint: str, *, zero_offsets: Mapping[str, float], joint_angles: Mapping[str, float]) -> tuple[np.ndarray, np.ndarray]: + resolved = self.resolve_angles(joint_angles) + transform = np.eye(4) + for joint in self._chain(target_joint): + frame = transform @ joint.origin @ self._motion(joint, float(zero_offsets.get(joint.name, 0.0))) + if joint.name == target_joint: + return frame[:3, :3] @ joint.axis, frame[:3, 3].copy() + transform = frame @ self._motion(joint, resolved[joint.name]) + raise ValueError(f"cannot resolve joint axis: {target_joint}") + + +def corrected_origin_rpy(joint: UrdfJoint, offset_rad: float) -> tuple[float, float, float]: + """q_CAD = q_output + offset; compose about the local axis, not an RPY component.""" + if joint.kind not in {"revolute", "continuous"}: + raise ValueError("angular zero correction requires a revolute joint") + matrix = joint.origin @ _axis_rotation(joint.axis, offset_rad) + return tuple(float(v) for v in Rotation.from_matrix(matrix[:3, :3]).as_euler("xyz")) + + +def corrected_mimic_offset(multiplier: float, offset: float, parent_zero: float, child_zero: float) -> float: + values = (multiplier, offset, parent_zero, child_zero) + if not all(math.isfinite(float(v)) for v in values): + raise ValueError("mimic coordinate correction must be finite") + return float(offset + multiplier * parent_zero - child_zero) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py index 48b5e9a..6189b7b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py @@ -9,14 +9,19 @@ publishing a new file. from __future__ import annotations from dataclasses import dataclass, field +import hashlib import math import os from pathlib import Path import re import shutil +import tempfile from typing import Mapping, Sequence import xml.etree.ElementTree as ET +import numpy as np +from scipy.spatial.transform import Rotation + @dataclass(frozen=True) class UrdfJointPatch: @@ -73,6 +78,39 @@ class UrdfPatchSet: ) +def parse_vector3(value: str) -> np.ndarray: + """Parse and validate a finite three-component URDF vector.""" + result = np.asarray([float(item) for item in str(value).split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF vector: {value}") + return result + + +def corrected_origin_rpy(joint: ET.Element, offset_rad: float) -> str: + """Rotate a joint origin about its declared axis while preserving zero edits.""" + origin = joint.find("origin") + if origin is None or origin.get("rpy") is None: + raise ValueError(f"joint {joint.get('name')} has no origin.rpy") + if abs(float(offset_rad)) <= 1.0e-12: + return str(origin.get("rpy")) + axis_node = joint.find("axis") + axis = parse_vector3( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + norm = float(np.linalg.norm(axis)) + if norm <= 1.0e-12: + raise ValueError(f"joint {joint.get('name')} has a degenerate axis") + source = Rotation.from_euler( + "xyz", parse_vector3(origin.get("rpy", "0 0 0")) + ) + corrected = source * Rotation.from_rotvec( + axis / norm * float(offset_rad) + ) + return " ".join( + f"{float(value):.15g}" for value in corrected.as_euler("xyz") + ) + + def _replace_attribute( block: str, element: str, attribute: str, value: str ) -> str: @@ -83,7 +121,15 @@ def _replace_attribute( ) match = pattern.search(block) if match is None: - raise ValueError(f"{element} has no {attribute} attribute") + # URDF permits omitted mimic coefficients (defaults: 1 and 0). A + # coordinate change must be able to materialize an authorized offset. + if (element, attribute) not in {("mimic", "offset"), ("mimic", "multiplier")}: + raise ValueError(f"{element} has no {attribute} attribute") + opening = re.search(rf"<{element}\b[^>]*>", block) + if opening is None: + raise ValueError(f"{element} has no {attribute} attribute: joint has no {element} element") + position = opening.end() - (2 if opening.group(0).endswith("/>") else 1) + return block[:position] + f' {attribute}="{value}"' + block[position:] start, end = match.span("value") return block[:start] + str(value) + block[end:] @@ -362,37 +408,89 @@ def write_urdf_patches( patches: UrdfPatchSet, forbidden_source_stem_patterns: Sequence[str] = (), copy_complete_mesh_directory: bool = False, + authorized_fields: Mapping[str, Sequence[str]] | None = None, + validate_mimic_ranges: bool = True, + expected_source_sha256: str | None = None, ) -> Path: """Validate and atomically materialize one patched URDF.""" source = Path(source_urdf).expanduser().resolve() destination = Path(destination_urdf).expanduser().resolve() if not source.is_file(): raise ValueError(f"source URDF does not exist: {source}") + source_bytes = source.read_bytes() + source_digest = hashlib.sha256(source_bytes).hexdigest() + if expected_source_sha256 is not None and source_digest != expected_source_sha256: + raise ValueError("source URDF SHA256 differs from protected input") for pattern in forbidden_source_stem_patterns: if re.search(str(pattern), source.stem, re.IGNORECASE): raise ValueError( "source URDF must be the immutable original CAD URDF" ) - if destination == source or destination.exists(): + if destination == source or destination.exists() or destination.is_symlink(): raise ValueError(f"refusing to overwrite URDF: {destination}") destination.parent.mkdir(parents=True, exist_ok=True) - tree = ET.parse(source) - root = tree.getroot() + root = ET.fromstring(source_bytes) corrected = apply_urdf_patch_text( - source.read_text(encoding="utf-8"), root, patches + source_bytes.decode("utf-8"), root, patches ) - materialize_relative_mesh_assets( - source=source, output=destination.parent, urdf_root=root - ) - if copy_complete_mesh_directory: - _copy_complete_mesh_directory(source, destination.parent) - temporary = destination.with_suffix(destination.suffix + ".tmp") + descriptor, temporary_name = tempfile.mkstemp(prefix=".urdf-validation-", suffix=".urdf", dir=destination.parent) + temporary = Path(temporary_name) try: - with temporary.open("w", encoding="utf-8") as stream: + with os.fdopen(descriptor, "w", encoding="utf-8") as stream: stream.write(corrected) stream.flush() os.fsync(stream.fileno()) - os.replace(temporary, destination) + # Reparse both documents and prove that patching preserved topology, + # mesh/inertial/collision/visual data, and every non-requested field. + # The local import avoids a module cycle with ``validate``. + from .validate import validate_structural_urdf_diff + + requested_authorization = { + name: tuple( + f"{element}.{attribute}" + for element, attribute, _value in patch.replacements() + ) + for name, patch in patches.joints.items() + } + authorization = ( + requested_authorization + if authorized_fields is None + else { + str(name): tuple(str(field) for field in fields) + for name, fields in authorized_fields.items() + } + ) + for name, requested in requested_authorization.items(): + unauthorized = set(requested) - set(authorization.get(name, ())) + if unauthorized: + raise ValueError( + "URDF patch requested fields absent from Profile authorization: " + f"{name}:" + ",".join(sorted(unauthorized)) + ) + validate_structural_urdf_diff( + source, + temporary, + authorized_fields=authorization, + authorized_mujoco_equalities=tuple(patches.mujoco_equalities), + expected_source_sha256=source_digest, + ) + # This belongs to the common writer rather than individual model + # serializers: every emitted URDF must have valid finite joint limits, + # existing mimic sources, acyclic chains, and no newly enlarged mimic + # range violation. Pre-existing CAD rounding excess is tolerated only + # when calibration does not make it worse. + if validate_mimic_ranges: + validate_urdf_mimic_ranges(temporary, reference_urdf=source) + materialize_relative_mesh_assets( + source=source, output=destination.parent, urdf_root=root + ) + if copy_complete_mesh_directory: + _copy_complete_mesh_directory(source, destination.parent) + if hashlib.sha256(source.read_bytes()).hexdigest() != source_digest: + raise ValueError("source URDF changed during validation") + # Atomic no-clobber installation. A concurrent writer (or user file) + # must never be overwritten or removed by our failure cleanup. + os.link(temporary, destination) finally: if temporary.exists(): temporary.unlink() diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py index 58eb643..441fc13 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py @@ -5,9 +5,152 @@ from __future__ import annotations from dataclasses import dataclass, field import hashlib from pathlib import Path -from typing import Mapping +from typing import Mapping, Sequence +import math +import xml.etree.ElementTree as ET from ..domain import CalibrationProfile +from ..fitting.coupling import LinearMimicFit +from .kinematics import UrdfKinematicModel, corrected_mimic_offset +from .patch import UrdfJointPatch, UrdfPatchSet, corrected_origin_rpy, write_urdf_patches + + +@dataclass(frozen=True) +class StandardUrdfPlan: + """Frozen coordinate transform plus independently declared field rights. + + Limits are expressed in output coordinates. Mimic fits enter in CAD + coordinates and are transformed exactly once here, including chains whose + parent is a passive joint. Nothing is inferred from JSON runtime curves. + """ + + source_sha256: str + zero_offsets_rad: Mapping[str, float] + limits_output_rad: Mapping[str, tuple[float, float]] + mimic_output: Mapping[str, tuple[str, float, float]] + patches: UrdfPatchSet + authorized_fields: Mapping[str, Sequence[str]] + + def write(self, source_urdf: str | Path, destination_urdf: str | Path) -> Path: + source = Path(source_urdf) + if hashlib.sha256(source.read_bytes()).hexdigest() != self.source_sha256: + raise ValueError("protected source URDF SHA256 changed before write") + return write_urdf_patches( + source_urdf=source, destination_urdf=destination_urdf, + patches=self.patches, authorized_fields=self.authorized_fields, + expected_source_sha256=self.source_sha256, + ) + + +def build_standard_correction_plan( + *, source_urdf: str | Path, source_sha256: str, + zero_offsets_rad: Mapping[str, float], + authorized_fields: Mapping[str, Sequence[str]], + measured_ranges_output_rad: Mapping[str, tuple[float, float]] | None = None, + fitted_mimics_cad: Mapping[str, LinearMimicFit] | None = None, + transferred_zero_sources: Mapping[str, str] | None = None, +) -> StandardUrdfPlan: + """Generate authorized standard-URDF edits, never clamp an invalid fit. + + q_CAD = q_output + delta. Consequently both bounds shift by -delta and + b_output = b_CAD + a*delta_parent - delta_child. Unobserved joints retain + their own CAD geometry and physical range, including transferred joints. + """ + source = Path(source_urdf) + if hashlib.sha256(source.read_bytes()).hexdigest() != source_sha256: + raise ValueError("protected source URDF SHA256 mismatch") + model = UrdfKinematicModel(source) + nodes = {str(node.get("name")): node for node in ET.parse(source).getroot().findall("joint")} + offsets = {str(name): float(value) for name, value in zero_offsets_rad.items()} + ranges = dict(measured_ranges_output_rad or {}) + mimics = dict(fitted_mimics_cad or {}) + requested = set(offsets) | set(ranges) | set(mimics) + if not requested <= model.joints.keys(): + raise ValueError("correction references a missing URDF joint") + for target, donor in (transferred_zero_sources or {}).items(): + if target == donor or target not in offsets or donor not in offsets: + raise ValueError("transfer requires distinct, explicitly solved donor and recipient") + if not math.isclose(offsets[target], offsets[donor], rel_tol=0, abs_tol=1e-12): + raise ValueError("transferred scalar zero differs from its declared donor") + if target in ranges: + raise ValueError("unmeasured transfer must retain recipient CAD physical range") + for name, value in offsets.items(): + if not math.isfinite(value) or model.joints[name].kind not in {"revolute", "continuous"}: + raise ValueError("zero corrections require finite revolute coordinates") + edits: dict[str, UrdfJointPatch] = {} + output_ranges: dict[str, tuple[float, float]] = {} + output_mimics: dict[str, tuple[str, float, float]] = {} + for name, joint in model.joints.items(): + delta = offsets.get(name, 0.0) + fields: dict[str, str] = {} + if delta != 0: + fields["origin_rpy"] = corrected_origin_rpy(nodes[name], delta) + if joint.lower is not None: + mechanical = (joint.lower - delta, joint.upper - delta) + interval = tuple(float(v) for v in ranges.get(name, mechanical)) + if len(interval) != 2 or not all(math.isfinite(v) for v in interval) or interval[0] >= interval[1]: + raise ValueError(f"invalid corrected interval: {name}") + if interval[0] < mechanical[0] - 1e-9 or interval[1] > mechanical[1] + 1e-9: + raise ValueError(f"corrected range exceeds source mechanical range: {name}") + output_ranges[name] = interval + if interval[0] != joint.lower: + fields["limit_lower"] = f"{interval[0]:.15g}" + if interval[1] != joint.upper: + fields["limit_upper"] = f"{interval[1]:.15g}" + elif name in ranges: + raise ValueError(f"cannot bound an unbounded joint without changing its contract: {name}") + if name in mimics and joint.mimic_joint is None: + raise ValueError(f"cannot create a new mimic topology: {name}") + if joint.mimic_joint is not None: + fit = mimics.get(name) + if fit is not None: + if not isinstance(fit, LinearMimicFit) or fit.target_joint != name or fit.source_joint != joint.mimic_joint: + raise ValueError(f"mimic fit does not match source URDF topology: {name}") + if fit.training_cycles != (0, 1, 2) or not fit.residuals_rad or not all(math.isfinite(v) for v in fit.residuals_rad): + raise ValueError(f"mimic fit lacks training-only evidence: {name}") + if not fit.offset_observed and not math.isclose(fit.offset_rad, joint.mimic_offset, rel_tol=0, abs_tol=1e-12): + raise ValueError(f"unobservable passive static offset must retain CAD: {name}") + multiplier, offset = fit.multiplier, fit.offset_rad + else: + multiplier, offset = joint.mimic_multiplier, joint.mimic_offset + if not math.isfinite(multiplier) or not math.isfinite(offset): + raise ValueError(f"nonfinite mimic fit: {name}") + shifted = corrected_mimic_offset(multiplier, offset, offsets.get(joint.mimic_joint, 0), delta) + output_mimics[name] = (joint.mimic_joint, multiplier, shifted) + if multiplier != joint.mimic_multiplier: + fields["mimic_multiplier"] = f"{multiplier:.15g}" + if shifted != joint.mimic_offset: + fields["mimic_offset"] = f"{shifted:.15g}" + if fields: + patch = UrdfJointPatch(**fields) + rights = set(authorized_fields.get(name, ())) + if any(f"{element}.{attribute}" not in rights for element, attribute, _ in patch.replacements()): + raise ValueError(f"correction requests a field absent from Profile authorization: {name}") + edits[name] = patch + # Reject inconsistent full mimic ranges before creating any artifacts. + reachable: dict[str, tuple[float, float]] = {} + + def resolve(name: str) -> tuple[float, float]: + if name in reachable: + return reachable[name] + if name in output_mimics: + parent, a, b = output_mimics[name] + bounds = resolve(parent) + values = (a*bounds[0] + b, a*bounds[1] + b) + interval = (min(values), max(values)) + elif name in output_ranges: + interval = output_ranges[name] + else: + raise ValueError(f"mimic source has no bounded mechanical interval: {name}") + if name in output_ranges and (interval[0] < output_ranges[name][0] - 1e-9 or interval[1] > output_ranges[name][1] + 1e-9): + raise ValueError(f"URDF mimic reachable range exceeds limits: {name}") + reachable[name] = interval + return interval + + for name in output_mimics: + resolve(name) + return StandardUrdfPlan(source_sha256, offsets, output_ranges, output_mimics, + UrdfPatchSet(edits), {name: tuple(fields) for name, fields in authorized_fields.items()}) @dataclass(frozen=True) @@ -21,6 +164,7 @@ class UrdfCorrectionPlan: forbid_calibrated_source: bool = True forbid_overwrite: bool = True preserve_passive_joints: bool = True + authorized_fields: Mapping[str, Sequence[str]] = field(default_factory=dict) def __post_init__(self) -> None: if len(self.source_sha256) != 64 or any( @@ -101,4 +245,5 @@ def build_correction_plan( mimic_source_by_joint=profile.zero.mimic_source_by_joint, frozen_joints=frozen | profile.zero.cad_frozen_joints, frozen_offsets_rad=expected_frozen, + authorized_fields=profile.urdf_authorized_fields, ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py new file mode 100644 index 0000000..4a652d8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/result_plan.py @@ -0,0 +1,57 @@ +"""Prepare one standard-coordinate artifact result before serialization.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from pathlib import Path +from typing import Mapping + +from ..domain.profile import CalibrationProfile +from ..domain.result import CalibrationResult, JointMapping +from .kinematics import UrdfKinematicModel +from .plan import StandardUrdfPlan, build_standard_correction_plan + + +@dataclass(frozen=True) +class PreparedCalibration: + plan: StandardUrdfPlan + mappings: Mapping[str, JointMapping] + + +def prepare_standard_result(profile: CalibrationProfile, source: Path, + source_sha256: str, fit: CalibrationResult) -> PreparedCalibration: + model = UrdfKinematicModel(source) + mimics = {name: evidence.fit for name, evidence in fit.standard_mimic_evidence.items()} + for target, donor in profile.zero.transferred_mimic_sources.items(): + original = model.joints[target] + mimics[target] = replace(mimics[donor], target_joint=target, + source_joint=original.mimic_joint, offset_rad=original.mimic_offset) + ranges = {name: mapping.bounds_rad for name, mapping in fit.output_mappings.items() + if name not in profile.zero.transferred_zero_sources} + for name, mapping in fit.command_mappings.items(): + if name in ranges: + lo, hi = ranges[name] + ranges[name] = (min(lo, mapping.bounds_rad[0]), max(hi, mapping.bounds_rad[1])) + plan = build_standard_correction_plan(source_urdf=source, source_sha256=source_sha256, + zero_offsets_rad=fit.zero_offsets_rad, measured_ranges_output_rad=ranges, + fitted_mimics_cad=mimics, transferred_zero_sources=profile.zero.transferred_zero_sources, + authorized_fields=profile.urdf_authorized_fields) + mappings = dict(fit.output_mappings) + for name, mapping in mappings.items(): + lo, hi = plan.limits_output_rad[name] + actual_lo, actual_hi = mapping.bounds_rad + if actual_lo < lo - 1e-9 or actual_hi > hi + 1e-9: + raise ValueError(f"transferred mapping exceeds recipient CAD physical range:{name}") + def resolve(joint): + if joint in mappings: + return mappings[joint] + source_joint, multiplier, offset = plan.mimic_output[joint] + parent = resolve(source_joint) + branches = [tuple(offset + multiplier*v for v in values) + for values in (parent.angle_rad, parent.increasing_rad, parent.decreasing_rad)] + mappings[joint] = JointMapping(joint, parent.motor_index, parent.input_domain, + parent.knots, *branches, profile.zero.transferred_mimic_sources.get(joint)) + return mappings[joint] + for joint in plan.mimic_output: + resolve(joint) + return PreparedCalibration(plan, mappings) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py new file mode 100644 index 0000000..a5d6d1f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/tag_acceptance.py @@ -0,0 +1,85 @@ +"""Replay raw fourth-cycle Tag poses with the *written* URDF and JSON. + +Mounting transforms and base registration are frozen from training. Passive +poses are resolved only through the exported standard URDF mimic graph. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Mapping, Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..fitting.tag_installation import FrozenTagInstallation, rigid_matrix +from .acceptance import SerializedJointMapping, angular_metrics +from .kinematics import UrdfKinematicModel + + +@dataclass(frozen=True) +class TagHoldout: + sample_id: str + role: str + cycle: int + sdk_values: tuple[float, ...] + sdk_directions_by_joint: Mapping[str, str] + common_from_tag: tuple[tuple[float, ...], ...] + + +def validate_serialized_tag_holdout( + *, corrected_urdf: str | Path, payload: Mapping[str, Any], + common_from_base, installations: Mapping[str, FrozenTagInstallation], + observations: Sequence[TagHoldout], required_roles: Sequence[str], + minimum_samples: int = 40, + input_kind: str = "feedback", +) -> dict[str, dict[str, Any]]: + model = UrdfKinematicModel(corrected_urdf) + mapping = SerializedJointMapping(payload, input_kind=input_kind) + base = rigid_matrix(common_from_base) + roles = frozenset(required_roles) + if not roles or not roles <= installations.keys() or minimum_samples < 1: + raise ValueError("final Tag holdout requires declared frozen installations") + active = tuple(name for name, joint in model.joints.items() + if joint.kind != "fixed" and joint.mimic_joint is None) + training_ids = {sample for mount in installations.values() for sample in mount.training_sample_ids} + seen = set() + errors: dict[str, list[tuple[float, float]]] = {role: [] for role in roles} + for row in observations: + if row.cycle != 3 or not row.sample_id or row.sample_id in training_ids: + raise ValueError("final Tag holdout overlaps training or has no image identity") + if row.role not in roles: + raise ValueError("final Tag holdout contains an undeclared Tag") + identity = (row.role, row.sample_id) + if identity in seen: + raise ValueError("duplicate final Tag holdout image") + seen.add(identity) + mount = installations[row.role] + if mount.role != row.role or mount.link not in model.links: + raise ValueError("frozen Tag installation has a different role/link") + angles = mapping.evaluate(row.sdk_values, "", active_joints=active, + directions_by_joint=row.sdk_directions_by_joint) + for name, value in model.resolve_angles(angles).items(): + joint = model.joints[name] + if joint.lower is not None and not joint.lower - 1e-9 <= value <= joint.upper + 1e-9: + raise ValueError(f"serialized URDF motion exceeds joint limit:{name}") + transform = np.eye(4) if mount.link == model.root_link else model.link_transform( + model.parent_joint_by_child[mount.link], zero_offsets={}, joint_angles=angles) + predicted = base @ transform @ rigid_matrix(mount.link_from_tag) + observed = rigid_matrix(row.common_from_tag) + angle = float(Rotation.from_matrix(predicted[:3, :3].T @ observed[:3, :3]).magnitude()) + distance = float(np.linalg.norm(predicted[:3, 3] - observed[:3, 3])) + errors[row.role].append((angle, distance)) + results = {} + for role, values in errors.items(): + if len(values) < minimum_samples: + raise ValueError(f"missing independent Tag holdout samples:{role}:{len(values)}") + metrics = angular_metrics([value[0] for value in values]) + translation_p95 = float(np.percentile([value[1] for value in values], 95)) + if not metrics.passed or translation_p95 > 0.003: + raise ValueError(f"serialized_urdf_tag_holdout_failed:{role}:mae_deg={metrics.mae_deg}:p95_deg={metrics.p95_deg}:max_deg={metrics.maximum_deg}:translation_p95_m={translation_p95}") + results[role] = {"count": metrics.count, "mae_deg": metrics.mae_deg, + "p95_deg": metrics.p95_deg, "maximum_deg": metrics.maximum_deg, + "translation_p95_m": translation_p95} + return results diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py new file mode 100644 index 0000000..0aedf5a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/validate.py @@ -0,0 +1,183 @@ +"""Post-write structural and runtime-range validation for corrected URDFs.""" + +from __future__ import annotations + +import hashlib +import math +from pathlib import Path +from typing import Mapping, Sequence +import xml.etree.ElementTree as ET + + +URDF_FIELD_TO_XML = { + "origin.rpy": ("origin", "rpy"), + "limit.lower": ("limit", "lower"), + "limit.upper": ("limit", "upper"), + "mimic.multiplier": ("mimic", "multiplier"), + "mimic.offset": ("mimic", "offset"), +} + + +def validate_structural_urdf_diff( + source_urdf: str | Path, + corrected_urdf: str | Path, + *, + authorized_fields: Mapping[str, Sequence[str]], + authorized_mujoco_equalities: Sequence[str] = (), + expected_source_sha256: str | None = None, +) -> tuple[str, ...]: + """Prove that every parsed XML difference is explicitly authorized.""" + source_path = Path(source_urdf) + corrected_path = Path(corrected_urdf) + digest = hashlib.sha256(source_path.read_bytes()).hexdigest() + if expected_source_sha256 is not None and digest != str( + expected_source_sha256 + ).lower(): + raise ValueError("source URDF SHA256 differs from protected input") + source = ET.parse(source_path).getroot() + corrected = ET.parse(corrected_path).getroot() + allowed = { + str(joint): frozenset(str(field) for field in fields) + for joint, fields in authorized_fields.items() + } + allowed_equalities = frozenset( + str(name) for name in authorized_mujoco_equalities + ) + unsupported = { + field + for fields in allowed.values() + for field in fields + if field not in URDF_FIELD_TO_XML + } + if unsupported: + raise ValueError( + "unsupported authorized URDF fields: " + ",".join(sorted(unsupported)) + ) + changes: list[str] = [] + + def compare( + left: ET.Element, + right: ET.Element, + path: str, + joint_name: str | None, + joint_child_tag: str | None, + ) -> None: + if left.tag != right.tag: + raise ValueError(f"URDF topology changed at {path}") + current_joint = joint_name + current_child = joint_child_tag + if path == "/robot" and left.tag != "robot": + raise ValueError("URDF root is not robot") + if left.tag == "joint" and path.count("/") == 2: + if left.get("name") != right.get("name"): + raise ValueError(f"URDF joint topology changed at {path}") + current_joint = str(left.get("name", "")) + current_child = None + elif current_joint is not None and path.count("/") == 3: + current_child = left.tag + else: + # Authorization applies only to direct children of robot/joint. + # An extension's nested origin is not the kinematic joint origin. + current_child = None + if (left.text or "").strip() != (right.text or "").strip(): + raise ValueError(f"URDF element text changed at {path}") + keys = set(left.attrib) | set(right.attrib) + for attribute in sorted(keys): + before = left.attrib.get(attribute) + after = right.attrib.get(attribute) + if before == after: + continue + equality_change = bool( + left.tag == "joint" + and "/mujoco" in path + and str(left.get("name", "")) in allowed_equalities + and attribute == "polycoef" + ) + if equality_change: + changes.append( + f"mujoco.equality.{left.get('name')}.polycoef" + ) + continue + field = next( + ( + name + for name, location in URDF_FIELD_TO_XML.items() + if location == (current_child, attribute) + ), + None, + ) + if ( + current_joint is None + or field is None + or field not in allowed.get(current_joint, frozenset()) + ): + raise ValueError( + "corrected URDF changed an unauthorized field: " + f"{path}@{attribute}" + ) + changes.append(f"{current_joint}.{field}") + left_children = list(left) + right_children = list(right) + if len(left_children) != len(right_children): + raise ValueError(f"URDF topology changed at {path}") + for index, (left_child, right_child) in enumerate( + zip(left_children, right_children) + ): + left_identity = ( + left_child.tag, + left_child.get("name"), + left_child.get("link"), + ) + right_identity = ( + right_child.tag, + right_child.get("name"), + right_child.get("link"), + ) + if left_identity != right_identity: + raise ValueError(f"URDF child ordering/topology changed at {path}") + compare( + left_child, + right_child, + f"{path}/{left_child.tag}[{index}]", + current_joint, + current_child, + ) + + compare(source, corrected, "/robot", None, None) + return tuple(changes) + + +def validate_runtime_curve_limits( + corrected_urdf: str | Path, + curves_rad: Mapping[str, Sequence[float]], +) -> None: + """Reject JSON/runtime curves that exceed their corrected URDF limits.""" + root = ET.parse(Path(corrected_urdf)).getroot() + joints = {str(node.get("name")): node for node in root.findall("joint")} + for name, values in curves_rad.items(): + node = joints.get(str(name)) + if node is None: + raise ValueError(f"runtime curve targets missing URDF joint: {name}") + limit = node.find("limit") + if limit is None: + raise ValueError(f"runtime curve joint has no URDF limit: {name}") + lower = float(limit.get("lower", "nan")) + upper = float(limit.get("upper", "nan")) + samples = tuple(float(value) for value in values) + if ( + not samples + or not math.isfinite(lower) + or not math.isfinite(upper) + or any( + not math.isfinite(value) or value < lower - 1e-9 or value > upper + 1e-9 + for value in samples + ) + ): + raise ValueError(f"runtime curve exceeds corrected URDF limits: {name}") + + +__all__ = [ + "URDF_FIELD_TO_XML", + "validate_runtime_curve_limits", + "validate_structural_urdf_diff", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/full_hand.py b/src/linkerhand_calibration/linkerhand_calibration/full_hand.py index 71a0664..d2018a0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/full_hand.py +++ b/src/linkerhand_calibration/linkerhand_calibration/full_hand.py @@ -2,6 +2,6 @@ import sys -from .models.g20 import profile as _implementation +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20 import profile as _implementation sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py deleted file mode 100644 index 5915a14..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -"""Registered profiles for this hand family.""" - -from ..registry import ProfileRegistry - - -def register_profiles(registry: ProfileRegistry) -> None: - from .legacy_11 import build_left_profile, build_right_profile - from .right_19 import build_profile - - registry.register(build_profile()) - registry.register(build_left_profile()) - registry.register(build_right_profile()) - - -__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py deleted file mode 100644 index 571888f..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py +++ /dev/null @@ -1,13395 +0,0 @@ -"""One-command three-camera calibration for either complete G20 hand.""" - -from __future__ import annotations - -from collections import deque -from dataclasses import dataclass, field, replace -import hashlib -import json -import math -from pathlib import Path -import random -import re -import time -import traceback -from typing import Any, Mapping, Sequence - -import numpy as np -import rclpy -from apriltag_msgs.msg import AprilTagDetectionArray -from rclpy.exceptions import ParameterUninitializedException -from rclpy.node import Node -from rclpy.parameter import Parameter -from rclpy.qos import qos_profile_sensor_data -from scipy.spatial.transform import Rotation -from sensor_msgs.msg import CameraInfo, JointState -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from ...acquisition import ( - StateSample, - TagQuality, - interpolate_state_u8, - required_resume_views, - tag_quality_is_valid, - update_pnp_reset_watchdog, -) -from ...core import ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - 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, - load_three_camera_extrinsics, - matrix_payload, - transform_matrix, -) -from .profile import ( - G20_COMBINATION_REQUIRED_TARGET_KEYS, - G20_REFERENCE_THUMB_CMC_JOINTS, - G20_RIGHT_19_LAYOUT, - LEFT_HAND_PROFILE, - RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, - RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, - THREE_CAMERA_BASELINE_COMMAND, - HandCalibrationProfile, - JointCurveFit, - PalmAxisObserver, - SweepSpec, - build_calibration_motion_command, - build_calibration_preparation_waypoints, - build_calibration_return_waypoints, - build_calibration_speed_profile, - build_compact_payload, - calibration_auxiliary_commands, - canonical_zero_direction, - clamp_runtime_fits_to_urdf_limits, - compare_cross_view_roll_curves, - cross_view_roll_diagnostic_metrics, - derive_mimic_passive_fits, - fit_joint_image_curve, - get_hand_calibration_profile, -) -from ...hikrobot_camera import configure_fastdds_large_image_transport -from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES -from ...pnp import ( - SquareTagGroupPoseTracker, - SquareTagPose, - SquareTagPoseTracker, -) -from ...product import get_product_calibration_contract -from ...sample_schema import ( - SampleDataContractError, - canonical_sample_record, - fitting_sample_record, - fitting_sample_records, - validate_sample_records, -) -from ...storage import append_jsonl, append_jsonl_many, atomic_write_json -from .reporting_zh import render_three_camera_status_text_zh -from .urdf_input import ( - build_g20_urdf_input_payload, - load_g20_urdf_input, -) -from .zero_solver import ( - LEFT_ZERO_PROFILE, - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, - RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, - JointAxisMeasurement, - PalmOrientationMeasurement, - UrdfKinematicModel, - ZeroCalibrationProfile, - ZeroSolveResult, - anchor_right_19_mechanical_endpoint_curves, - axis_line_cycle_rms_m, - axis_line_uses_depth_free_interpretation_plane, - baseline_hysteresis_by_cycle_rad, - circle_direction_is_constrained, - cross_view_side_line_source, - fit_joint_axis_measurement, - fit_partial_palm_orientation_measurements, - fit_rotation_joint_curve, - derive_right_19_mechanical_endpoint_offsets, - expand_right_19_thumb_zero_result_with_cad_fingers, - joint_curve_holdout_errors, - maximum_axis_line_cycle_spread_m, - measure_joint_curve_observation, - merge_right_19_thumb_zero_result, - refit_axis_line_group_with_shared_radius, - select_cross_view_roll_direction_source, - solve_urdf_zero_offsets, - get_zero_calibration_profile, - get_right_19_thumb_zero_profile, - write_zero_corrected_urdf, - with_depth_free_axis_projection, -) - - -STATE_PREFLIGHT = "PREFLIGHT" -STATE_WAIT_START = "WAIT_START" -STATE_IMPORTING_BASE = "IMPORTING_BASE" -STATE_REVALIDATING_INHERITED = "REVALIDATING_INHERITED" -STATE_RETURN_BASELINE = "RETURN_BASELINE" -STATE_PREPARE_SWEEP = "PREPARE_SWEEP" -STATE_SWEEP = "SWEEP" -STATE_FITTING = "FITTING" -STATE_VALIDATION_MOVE = "VALIDATION_MOVE" -STATE_VALIDATION_CAPTURE = "VALIDATION_CAPTURE" -STATE_PAUSED = "PAUSED" -STATE_ABORTED = "ABORTED" -STATE_COMPLETE = "COMPLETE" - -STEADY_COMMAND_CHECKPOINTS: tuple[int, ...] = ( - 255, 224, 192, 160, 128, 96, 64, 32, 0 -) - -COMBINATION_TAG_TARGETS_BY_VIEW: Mapping[ - str, tuple[tuple[str, str], ...] -] = { - "front": tuple( - (name, name) - for name in ( - "thumb_cmc_pitch", "thumb_mcp", "thumb_ip", - "index_mcp_roll", "middle_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", - ) - ), - "side": tuple( - (f"{finger}_{joint}", f"{finger}_{joint}") - for finger in ("index", "middle", "ring", "pinky") - for joint in ("pip", "dip") - ), - # The top-view Tag observes yaw but is mounted on the final CMC link, - # downstream of pitch in the URDF chain. - "top": (("thumb_cmc_yaw", "thumb_cmc_pitch"),), -} - -COMBINATION_BASE_OBSERVER_BY_VIEW = { - "front": "thumb_cmc_pitch", - "side": "index_pip", - "top": "thumb_cmc_yaw", -} - -COMBINATION_BASE_ROLE_BY_VIEW = { - "front": "front_base", - "side": "side_base", - "top": "top_base", -} - - -def combination_target_coverage( - observation_counts: Mapping[str, int], - validation_counts: Mapping[str, int], -) -> dict[str, Any]: - required = G20_COMBINATION_REQUIRED_TARGET_KEYS - observations = { - key: int(observation_counts.get(key, 0)) for key in required - } - validations = { - key: int(validation_counts.get(key, 0)) for key in required - } - missing_observations = sorted( - key for key, count in observations.items() if count < 2 - ) - missing_validations = sorted( - key for key, count in validations.items() if count < 1 - ) - return { - "required_targets": list(required), - "minimum_observations_per_target": 2, - "minimum_validations_per_target": 1, - "observation_counts": observations, - "validation_counts": validations, - "missing_observation_targets": missing_observations, - "missing_validation_targets": missing_validations, - "coverage_passed": not missing_observations and not missing_validations, - } - - -def _combination_observable_joints( - profile: HandCalibrationProfile, - view: str, - roles: Sequence[str], -) -> tuple[str, ...]: - """Return combination targets measurable from the currently visible Tags. - - Combination poses deliberately do not require all four adjacent fingers - to be visible at once. A joint is usable only when both Tags that define - its relative pose are present in the same camera frame. - """ - visible = {str(role) for role in roles} - targets = { - observation_name - for observation_name, _ in COMBINATION_TAG_TARGETS_BY_VIEW[view] - } - return tuple( - name - for name, spec in profile.record_specs.items() - if name in targets - and spec.measured - and spec.view == view - and spec.parent_role in visible - and spec.child_role in visible - ) - - -def _overall_progress( - state: str, - *, - scan_progress: float, - validation_index: int = 0, - validation_total: int = 0, -) -> float: - """Keep 100% reserved for a validated, written calibration result.""" - scan = float(np.clip(scan_progress, 0.0, 1.0)) - if state == STATE_COMPLETE: - return 1.0 - if state in {STATE_FITTING}: - return 0.92 - if state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE}: - validation = ( - 0.0 - if validation_total <= 0 - else float(np.clip(validation_index / validation_total, 0.0, 1.0)) - ) - return 0.92 + 0.07 * validation - return min(0.90, 0.90 * scan) - - -def _stamp_ns(stamp: Any) -> int: - return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) - - -def _safe_name(value: str) -> str: - safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) - return safe or "UNSET" - - -def _sweep_storage_key(spec: SweepSpec) -> str | int: - """Keep legacy motor-keyed retry state while separating product tasks.""" - return spec.key if spec.task_name else int(spec.motor_index) - - -def _sweep_views( - profile: HandCalibrationProfile, spec: SweepSpec -) -> tuple[str, ...]: - """Return every camera that contributes a joint to one physical sweep.""" - views: list[str] = [] - for joint_name in spec.joints: - joint = profile.record_specs[joint_name] - if joint.view is not None and joint.view not in views: - views.append(joint.view) - return tuple(views or [spec.view]) - - -def _palm_axis_observer_for_sweep( - profile: HandCalibrationProfile, - spec: SweepSpec, - view: str, -) -> PalmAxisObserver | None: - """Return the one non-blocking palm observer attached to this task/view.""" - matches = tuple( - observer - for observer in profile.palm_axis_observers - if observer.task_name == spec.key and observer.view == str(view) - ) - if len(matches) > 1: - raise ValueError( - f"multiple palm-axis observers configured for {spec.key}:{view}" - ) - return matches[0] if matches else None - - -def _palm_axis_observer_schema( - profile: HandCalibrationProfile, -) -> list[dict[str, Any]]: - return [ - { - "source_name": item.source_name, - "task_name": item.task_name, - "view": item.view, - "parent_role": item.parent_role, - "child_role": item.child_role, - "model_joint": item.model_joint, - "motor_index": int(item.motor_index), - } - for item in profile.palm_axis_observers - ] - - -def _palm_axis_resume_policy( - profile: HandCalibrationProfile, - session_start: Mapping[str, Any], - compatibility_tokens: frozenset[str] | None = None, -) -> tuple[bool, tuple[str, ...]]: - """Validate optional palm-axis checkpoint data when a model uses it.""" - capability = "palm_axis_relative_motion_v3" - previous = {str(value) for value in session_start.get("capabilities", [])} - current = ( - frozenset(profile.capabilities) - if compatibility_tokens is None - else frozenset(compatibility_tokens) - ) - if not profile.palm_axis_observers and capability not in current: - return True, () - required_previous = set(current) - {capability} - if not required_previous.issubset(previous): - raise ValueError("resume checkpoint lacks required capabilities") - if capability in previous: - if session_start.get("palm_axis_observers") != ( - _palm_axis_observer_schema(profile) - ): - raise ValueError("resume palm-axis observer schema differs") - return True, () - # A new side-channel version changes the zero solver's observation model. - # Do not combine its fresh CMC axes with any trajectory captured under the - # previous PnP transaction semantics. This full invalidation happens only - # once; same-version checkpoints keep their normal task-level resume. - return False, tuple(spec.key for spec in profile.sweep_specs) - - -def _node_profile(node: Any) -> HandCalibrationProfile: - return getattr(node, "profile", LEFT_HAND_PROFILE) - - -def _command_names(node: Any) -> tuple[str, ...]: - profile = _node_profile(node) - names = tuple(getattr(profile, "command_names", ()) or COMMAND_NAMES) - if not names or len(set(names)) != len(names): - raise ValueError("product command names must be non-empty and unique") - return names - - -def _command_count(node: Any) -> int: - return len(_command_names(node)) - - -def _sweep_joints_for_view( - profile: HandCalibrationProfile, - spec: SweepSpec, - view: str, -) -> tuple[str, ...]: - return tuple( - joint_name - for joint_name in spec.joints - if profile.record_specs[joint_name].view == str(view) - ) - - -def resume_tasks_invalidated_by_tag_size_changes( - profile: HandCalibrationProfile, - previous_sizes_m_by_id: Mapping[int, float], - current_sizes_m_by_id: Mapping[int, float], -) -> tuple[tuple[int, ...], tuple[str, ...]]: - """Localise a corrected physical Tag size to dependent motion tasks. - - PnP translations stored in a checkpoint are already scaled by the Tag - size used during acquisition and cannot be repaired exactly afterwards. - A size-only product correction therefore invalidates every task that used - one of those Tags, while complete tasks whose required Tags are unchanged - remain valid sparse checkpoints. - """ - previous = { - int(key): float(value) - for key, value in previous_sizes_m_by_id.items() - } - current = { - int(key): float(value) - for key, value in current_sizes_m_by_id.items() - } - if set(previous) != set(current): - raise ValueError("resume Tag-size maps contain different Tag IDs") - if any( - not math.isfinite(value) or value <= 0.0 - for value in (*previous.values(), *current.values()) - ): - raise ValueError("resume Tag sizes must be finite and positive") - changed_ids = { - tag_id - for tag_id in current - if not math.isclose( - previous[tag_id], current[tag_id], rel_tol=0.0, abs_tol=1.0e-12 - ) - } - affected: list[str] = [] - base_role_by_view = { - "front": "front_base", - "side": "side_base", - "top": "top_base", - } - for spec in profile.sweep_specs: - used_ids: set[int] = set() - for view in _sweep_views(profile, spec): - used_ids.add(int(profile.view_tags[view][base_role_by_view[view]])) - for joint_name in _sweep_joints_for_view(profile, spec, view): - joint = profile.record_specs[joint_name] - for role in (joint.parent_role, joint.child_role): - if role is not None: - used_ids.add(int(profile.view_tags[view][role])) - if used_ids & changed_ids: - affected.append(spec.key) - return tuple(sorted(changed_ids)), tuple(affected) - - -def _fit_retry_joint_names( - profile: HandCalibrationProfile, - spec: SweepSpec, - failures: Sequence[Mapping[str, Any]], -) -> set[str]: - """Resolve the independently failed measurements in a physical task.""" - selected: set[str] = set() - validation_sources = profile.axis_validation_sources or {} - for failure in failures: - quality_sources = failure.get("quality_source_joints", ()) - if ( - isinstance(quality_sources, Sequence) - and not isinstance(quality_sources, (str, bytes)) - ): - selected_sources = { - str(source) - for source in quality_sources - if str(source) in spec.joints - } - if selected_sources: - selected.update(selected_sources) - continue - name = str(failure.get("joint", "")) - if str(failure.get("metric", "")) == "cross_view_roll_curve": - name = str(validation_sources.get(name, name)) - if name in spec.joints: - selected.add(name) - return selected or set(spec.joints) - - -def _isolated_axis_cycle_outliers( - axes: Sequence[np.ndarray], limit_rad: float -) -> set[int]: - """Return one unambiguous outlier outside an all-other-cycle cluster. - - At least three mutually consistent inlier cycles are required. Ambiguous - or generally scattered axes deliberately return an empty set so callers - retain the conservative full-task retry. - """ - if len(axes) < 4: - return set() - candidates: list[tuple[float, int]] = [] - for excluded in range(len(axes)): - inliers = [ - np.asarray(axis, dtype=float) - for index, axis in enumerate(axes) - if index != excluded - ] - consistent = True - maximum_inlier_difference = 0.0 - for left_index, left in enumerate(inliers): - for right in inliers[left_index + 1 :]: - difference = math.acos( - abs(float(np.clip(left @ right, -1.0, 1.0))) - ) - maximum_inlier_difference = max( - maximum_inlier_difference, difference - ) - if difference > float(limit_rad): - consistent = False - break - if not consistent: - break - if consistent: - candidates.append((maximum_inlier_difference, excluded)) - if not candidates: - return set() - candidates.sort() - best_spread, best_excluded = candidates[0] - # A gradual end-to-end drift can produce two different all-but-one - # subsets just under the formal limit. Localize only when one three-cycle - # cluster is substantially tighter than the acceptance band. - if best_spread > 0.5 * float(limit_rad): - return set() - if ( - len(candidates) > 1 - and abs(candidates[1][0] - best_spread) <= math.radians(0.01) - ): - return set() - return {best_excluded} - - -def _isolated_scalar_cycle_outlier( - values: Sequence[float], limit_rad: float -) -> set[int]: - """Return one clearly separated scalar outlier among three rounds. - - Zero-offset fits have three training cycles, so the axis helper above - cannot localize them (it deliberately requires three inliers). Use the - unique closest pair as a two-cycle consensus only when that pair is tight - and clearly better than either alternative. Symmetric drift and - generally scattered values retain the conservative full-task retry. - """ - samples = np.asarray(values, dtype=float) - limit = float(limit_rad) - if ( - samples.ndim != 1 - or samples.size != 3 - or not np.all(np.isfinite(samples)) - or not math.isfinite(limit) - or limit <= 0.0 - ): - return set() - pair_candidates = sorted( - ( - abs(float(samples[left] - samples[right])), - left, - right, - ) - for left in range(samples.size) - for right in range(left + 1, samples.size) - ) - pair_spread, left, right = pair_candidates[0] - second_pair_spread = pair_candidates[1][0] - overall_spread = pair_candidates[-1][0] - if overall_spread <= limit or pair_spread > 0.5 * limit: - return set() - # Do not arbitrarily pick one side of a gradual or nearly symmetric - # three-cycle drift. The winning pair must be materially tighter than - # the next candidate, not merely win due to floating-point noise. - if second_pair_spread - pair_spread < 0.1 * limit: - return set() - excluded = [ - index for index in range(samples.size) if index not in {left, right} - ] - if len(excluded) != 1: - return set() - outlier = excluded[0] - inlier_center = 0.5 * float(samples[left] + samples[right]) - if abs(float(samples[outlier]) - inlier_center) < 0.75 * limit: - return set() - return {outlier} - - -def _thumb_yaw_zero_repeatability_failures( - zero_result: ZeroSolveResult, - *, - maximum_cycle_range_rad: float, - maximum_confidence_half_width_rad: float, -) -> list[dict[str, Any]]: - """Apply a stricter, retry-aware publication gate to thumb CMC yaw.""" - joint_name = "thumb_cmc_yaw" - cycle_offsets = tuple( - float(value) - for value in zero_result.cycle_offsets_rad.get(joint_name, ()) - ) - failures: list[dict[str, Any]] = [] - if len(cycle_offsets) >= 2 and all( - math.isfinite(value) for value in cycle_offsets - ): - cycle_range = max(cycle_offsets) - min(cycle_offsets) - if cycle_range > float(maximum_cycle_range_rad): - failure: dict[str, Any] = { - "joint": joint_name, - "metric": "zero_cycle_offset_range_deg", - "actual": round(math.degrees(cycle_range), 6), - "limit": round( - math.degrees(maximum_cycle_range_rad), 6 - ), - "comparison": "maximum", - "cycle_offset_deg": [ - round(math.degrees(value), 6) - for value in cycle_offsets - ], - } - outliers = _isolated_scalar_cycle_outlier( - cycle_offsets, maximum_cycle_range_rad - ) - if outliers: - outlier = next(iter(outliers)) - failure["cycle"] = outlier + 1 - failure["inlier_cycles"] = [ - index + 1 - for index in range(len(cycle_offsets)) - if index != outlier - ] - failures.append(failure) - - # A range failure is more actionable: it can often identify exactly one - # acquisition cycle. Avoid adding a second, non-localized failure that - # would unnecessarily turn that retry back into a full yaw rescan. - if failures: - return failures - confidence = zero_result.offset_confidence_half_width_rad.get(joint_name) - if ( - confidence is not None - and math.isfinite(float(confidence)) - and float(confidence) > float(maximum_confidence_half_width_rad) - ): - failures.append( - { - "joint": joint_name, - "metric": "zero_confidence_95_half_width_deg", - "actual": round(math.degrees(float(confidence)), 6), - "limit": round( - math.degrees(maximum_confidence_half_width_rad), 6 - ), - "comparison": "maximum", - } - ) - return failures - - -def _previous_passed_joint_zero_offset( - session_dir: Path, serial_number: str, joint_name: str -) -> tuple[Path, float] | None: - """Read one zero from the previous passed sibling session, if present.""" - pointer = session_dir.parent / "latest_passed" - try: - previous = pointer.resolve(strict=True) - except OSError: - return None - if previous == session_dir or previous.parent != session_dir.parent: - return None - payload_path = previous / ( - f"g20_right_{_safe_name(serial_number)}_calibration.json" - ) - try: - payload = json.loads(payload_path.read_text(encoding="utf-8")) - offset = float( - payload["joints"][joint_name]["zero_angles"][ - "urdf_zero_offset_rad" - ] - ) - except (KeyError, OSError, TypeError, ValueError, json.JSONDecodeError): - return None - if not math.isfinite(offset): - return None - return previous, offset - - -def _isolated_axis_line_cycle_outliers( - measurements: Sequence[JointAxisMeasurement], limit_m: float -) -> set[int]: - """Return one cycle whose removal restores line-position repeatability. - - Four independently fitted near-parallel lines can contain one bad PnP - branch or one mechanically unsettled pass. When three cycles form a - clearly tighter cluster below the unchanged formal RMS limit, reacquire - only the excluded cycle. Ambiguous gradual drift still returns no - outlier and therefore keeps the conservative full four-cycle retry. - """ - if len(measurements) < 4 or float(limit_m) <= 0.0: - return set() - full_rms = axis_line_cycle_rms_m(measurements) - if full_rms <= float(limit_m): - return set() - candidates: list[tuple[float, int]] = [] - for excluded in range(len(measurements)): - inliers = [ - measurement - for index, measurement in enumerate(measurements) - if index != excluded - ] - inlier_rms = axis_line_cycle_rms_m(inliers) - if inlier_rms <= float(limit_m): - candidates.append((inlier_rms, excluded)) - if not candidates: - return set() - candidates.sort() - best_rms, best_excluded = candidates[0] - # Require a strong three-cycle cluster and a unique choice. This rejects - # the common gradual-drift case where excluding either endpoint happens - # to move a marginal RMS just below the gate. - if best_rms > 0.75 * float(limit_m): - return set() - if ( - len(candidates) > 1 - and candidates[1][0] - best_rms <= 0.10 * float(limit_m) - ): - return set() - return {best_excluded} - - -def _preserve_pnp_task_reference_for_sweep( - item: SweepItem, *, is_fit_retry: bool, has_precheck_anchor: bool -) -> bool: - """Keep the accepted IPPE branch after precheck and across retries.""" - return bool( - not item.precheck - and ( - item.cycle > 0 - or bool(is_fit_retry) - or bool(has_precheck_anchor) - ) - ) - - -def _requires_pnp_tracker_reset_for_sweep( - profile: HandCalibrationProfile, - item: SweepItem, - *, - is_fit_retry: bool, - preserve_retry_continuity: bool = False, -) -> bool: - """Reset G20-right PnP once per task, not once per formal cycle. - - The low-speed decreasing precheck establishes a branch with a static - endpoint window. Clearing frame-to-frame state at every later cycle made - four measurements of one rigid installation depend on four independent - planar-PnP initializations. Keep the tracker continuous for the whole - normal task transaction; a bounded retry remains a deliberately fresh - initialization while retaining the accepted task reference. - """ - if item.direction != DIRECTION_DECREASING: - return False - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return True - if is_fit_retry and preserve_retry_continuity: - # A dependent retry (currently the top-view thumb-yaw axis pair) - # replaces motion samples inside an already accepted task-relative - # PnP generation. Resetting here can move the two planar Tags to a - # different, internally smooth pose branch and shift every yaw zero - # by several degrees. Keep the exact live tracker/reference while - # replacing only the localized physical acquisition. - return False - if is_fit_retry: - return True - if profile.precheck_sweeps: - return bool(item.precheck) - return not item.precheck and item.cycle == 0 - - -def _fixed_base_role(view: str) -> str: - return { - "front": "front_base", - "side": "side_base", - "top": "top_base", - }[str(view)] - - -def _sweep_uses_locked_base_reference( - profile: HandCalibrationProfile, - spec: SweepSpec, - view: str, -) -> bool: - """Use one session-fixed base pose wherever task geometry needs it. - - Pinky/ring flexion is required to expose the middle/index side Tag, but - that same safe pose physically covers front Tag 0. The clearance remains - parked through that finger's roll, pitch and PIP tasks, including stages - where front is only an inactive status camera. Tag 0 is therefore locked - from baseline for every four-finger task; any moving Tag used by the - actual capture remains live and mandatory. - - Top Tag 8 has a different contract: it remains live and visible, but its - pose is frozen after preflight. Pitch and roll are collected in separate - tasks and jointly define thumb yaw. Re-solving a near-frontal planar base - Tag independently in those tasks expresses the two axes in subtly - different frames and turns image-position-dependent IPPE tilt into a yaw - zero shift. One session reference makes any fixed pose bias common to - both axes, where it cancels geometrically. - """ - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return False - selected_view = str(view) - if selected_view == "front": - return spec.key.startswith( - ("pinky_", "ring_", "middle_", "index_") - ) - return bool( - selected_view == "top" - and spec.key - in { - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - "thumb_cmc_yaw_top", - } - ) - - -def _maximum_corner_drift_px( - reference: Sequence[Sequence[float]], - current: Sequence[Sequence[float]], -) -> float: - """Return the largest same-corner displacement for one fixed Tag.""" - reference_array = np.asarray(reference, dtype=float) - current_array = np.asarray(current, dtype=float) - if ( - reference_array.shape != (4, 2) - or current_array.shape != (4, 2) - or not np.all(np.isfinite(reference_array)) - or not np.all(np.isfinite(current_array)) - ): - return float("inf") - return float( - np.max(np.linalg.norm(current_array - reference_array, axis=1)) - ) - - -def _resume_fixed_base_position_compatibility( - rows: Sequence[Mapping[str, Any]], - current_corners_by_view: Mapping[ - str, Sequence[Sequence[float]] | None - ], - maximum_corner_drift_px: float, -) -> tuple[dict[str, float], tuple[str, ...], tuple[str, ...]]: - """Compare the previous and current session-start palm references. - - Calibration measurements are invariant to one rigid hand placement, but - samples expressed in two independently established common frames must - never be combined. Fixed palm-Tag corners provide a camera-native check - before any durable task is imported. - """ - previous_corners_by_view: dict[str, Any] = {} - for row in rows: - if str(row.get("kind", "")) != "fixed_base_reference_locked": - continue - view = str(row.get("view", "")) - if view in current_corners_by_view: - # Keep the last lock in case a future compatible schema records a - # deliberate pre-scan relock in the same raw stream. - previous_corners_by_view[view] = row.get("corner_reference_xy") - - drift_by_view_px: dict[str, float] = {} - changed_views: list[str] = [] - unverifiable_views: list[str] = [] - for view, current in current_corners_by_view.items(): - previous = previous_corners_by_view.get(str(view)) - if previous is None or current is None: - unverifiable_views.append(str(view)) - continue - drift = _maximum_corner_drift_px(previous, current) - if not math.isfinite(drift): - unverifiable_views.append(str(view)) - continue - drift_by_view_px[str(view)] = drift - if drift > float(maximum_corner_drift_px): - changed_views.append(str(view)) - return ( - drift_by_view_px, - tuple(sorted(changed_views)), - tuple(sorted(unverifiable_views)), - ) - - -def _selected_pose_qualities( - selected: Mapping[str, SquareTagPose], - live_qualities: Mapping[str, TagQuality], - *, - locked_base_role: str | None, - locked_base_quality: TagQuality | None, -) -> dict[str, TagQuality]: - """Attach PnP residuals without pretending a cached Tag is live. - - A hidden fixed palm Tag has no entry in ``live_qualities``. Its cached - pose can still be selected together with the live moving Tag, so quality - propagation must explicitly use the quality captured at baseline instead - of indexing a missing live detection. - """ - updated = dict(live_qualities) - for role, pose in selected.items(): - source = ( - locked_base_quality - if role == locked_base_role - else live_qualities.get(role) - ) - if source is None: - raise KeyError(f"selected role has no quality source: {role}") - updated[role] = TagQuality( - hamming=source.hamming, - decision_margin=source.decision_margin, - edge_pixels=source.edge_pixels, - reprojection_error_px=pose.reprojection_error_px, - ) - return updated - - -def _frames_for_joint( - frames: Sequence[Any], joint_name: str -) -> list[Any]: - """Filter an asynchronous multi-camera buffer to one joint observation.""" - return [ - frame - for frame in frames - if ( - not frame.joint_quaternions_xyzw - and not frame.joint_vectors_xyz_m - and not frame.image_vectors_xy_px - and not frame.parent_poses_common - and not frame.child_poses_common - ) - or ( - joint_name in frame.joint_quaternions_xyzw - and joint_name in frame.joint_vectors_xyz_m - and joint_name in frame.image_vectors_xy_px - and joint_name in frame.parent_poses_common - and joint_name in frame.child_poses_common - ) - ] - - -def _frames_cover_sweep_joints( - frames: Sequence[Any], spec: SweepSpec, minimum_per_joint: int -) -> bool: - minimum = max(1, int(minimum_per_joint)) - return all( - len(_frames_for_joint(frames, joint_name)) >= minimum - for joint_name in spec.joints - ) - - -def _records_have_formal_sweep_coverage( - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - joint_names: Sequence[str], - *, - repetitions: int, - minimum_bins: int, - maximum_bin_gap: int, -) -> bool: - """Return whether every persisted direction has formal bin coverage.""" - expected_groups = { - (cycle, direction) - for cycle in range(int(repetitions)) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) - } - if not joint_names or not expected_groups: - return False - for joint_name in joint_names: - grouped: dict[tuple[int, str], set[int]] = {} - for record in records_by_joint.get(joint_name, ()): - direction = str(record.get("direction", "")) - if direction not in { - DIRECTION_DECREASING, - DIRECTION_INCREASING, - }: - continue - command = int( - np.clip( - np.rint( - float( - record.get( - "command_u8", - record.get("feedback_u8", -1), - ) - ) - ), - 0, - 255, - ) - ) - grouped.setdefault( - (int(record.get("cycle", -1)), direction), set() - ).add(command) - if not expected_groups.issubset(grouped): - return False - for key in expected_groups: - commands = sorted(grouped[key]) - if ( - not commands - or commands[0] != 0 - or commands[-1] != 255 - or len(commands) < int(minimum_bins) - or max(np.diff(commands), default=0) > int(maximum_bin_gap) - ): - return False - return True - - -def _frames_cover_sweep_motion( - frames: Sequence[Any], - spec: SweepSpec, - *, - motor_index: int, - minimum_per_joint: int, - minimum_span_u8: float, -) -> bool: - for joint_name in spec.joints: - selected = _frames_for_joint(frames, joint_name) - if len(selected) < int(minimum_per_joint): - return False - values = [float(frame.state_u8[motor_index]) for frame in selected] - if max(values) - min(values) < float(minimum_span_u8): - return False - return True - - -def _joint_failure_reason( - reason: str, spec: SweepSpec, joint_name: str -) -> str: - return ( - str(reason) - if len(spec.joints) == 1 - else f"{reason}:{joint_name}" - ) - - -def _steady_checkpoint_commands( - profile: HandCalibrationProfile, item: "SweepItem" -) -> tuple[int, ...]: - """Return first-training-round steady commands after the start pose.""" - if ( - profile.layout_id != G20_RIGHT_19_LAYOUT - or item.precheck - or item.cycle != 0 - ): - return () - ordered = ( - STEADY_COMMAND_CHECKPOINTS - if item.direction == DIRECTION_DECREASING - else tuple(reversed(STEADY_COMMAND_CHECKPOINTS)) - ) - if int(ordered[0]) != item.start_u8 or int(ordered[-1]) != item.target_u8: - raise RuntimeError("steady command checkpoint direction is inconsistent") - return tuple(int(value) for value in ordered[1:]) - - -RESUMABLE_SAMPLE_KINDS = frozenset( - {"sample", "baseline_hold_sample", "steady_command_sample"} -) - -RECALIBRATION_SCOPES = frozenset({"full", "thumb", "fingers"}) - - -def recalibration_task_keys( - profile: HandCalibrationProfile, scope: str -) -> tuple[str, ...]: - """Return tasks that must be freshly acquired for a partial session.""" - selected = str(scope).strip().lower() - if selected not in RECALIBRATION_SCOPES: - raise ValueError( - "recalibration_scope must be one of: " - + ", ".join(sorted(RECALIBRATION_SCOPES)) - ) - if selected == "full": - return () - if ( - profile.side != "right" - or profile.layout_id != G20_RIGHT_19_LAYOUT - ): - raise ValueError( - "partial recalibration is supported only for the G20 right " - "19-Tag product" - ) - thumb_tasks = tuple( - spec.key - for spec in profile.sweep_specs - if any(str(name).startswith("thumb_") for name in spec.joints) - ) - if len(thumb_tasks) != 4: - raise ValueError( - "G20 right thumb recalibration must resolve exactly four tasks" - ) - if selected == "thumb": - return thumb_tasks - thumb_task_set = set(thumb_tasks) - return tuple( - spec.key - for spec in profile.sweep_specs - if spec.key not in thumb_task_set - ) - - -def recalibration_quality_joints( - profile: HandCalibrationProfile, scope: str -) -> tuple[str, ...]: - """Return joints whose quality is decided by freshly acquired data.""" - selected = str(scope).strip().lower() - if selected == "full": - return tuple(profile.measured_joints) - # Reuse the scope validation and product guard in the task selector. - recalibration_task_keys(profile, selected) - want_thumb = selected == "thumb" - return tuple( - name - for name in profile.measured_joints - if str(name).startswith("thumb_") == want_thumb - ) - - -def build_standalone_thumb_payload( - *, - profile: HandCalibrationProfile, - serial_number: str, - measured_fits: Mapping[str, JointCurveFit], - thumb_offsets_rad: Mapping[str, float], - validation_errors_rad: Sequence[float], - baseline_command_u8: Sequence[int], - source_urdf_sha256: str, - camera_extrinsics_sha256: str, - corrected_urdf_sha256: str, -) -> dict[str, Any]: - """Build a non-runtime artifact for a fully standalone thumb solve.""" - if profile.side != "right" or profile.layout_id != G20_RIGHT_19_LAYOUT: - raise ValueError("standalone thumb payload requires G20 right 19-Tag") - expected_fits = set(recalibration_quality_joints(profile, "thumb")) - if set(measured_fits) != expected_fits: - raise ValueError("standalone thumb fits have the wrong joint set") - expected_offsets = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - offsets = { - str(name): float(value) for name, value in thumb_offsets_rad.items() - } - if set(offsets) != expected_offsets or any( - not math.isfinite(value) for value in offsets.values() - ): - raise ValueError("standalone thumb offsets must contain four finite zeros") - if len(baseline_command_u8) != 20: - raise ValueError("standalone thumb baseline must contain 20 commands") - joints: dict[str, dict[str, Any]] = {} - for name in sorted(expected_fits): - fit = measured_fits[name] - curve = np.asarray(fit.angle_rad, dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"standalone thumb curve is invalid: {name}") - spec = profile.joint_specs[name] - item: dict[str, Any] = { - "motor_index": int(spec.motor_index), - "angle_rad": [round(float(value), 8) for value in curve], - } - if name in offsets: - item["zero_command_u8"] = int( - baseline_command_u8[spec.motor_index] - ) - item["zero_angles"] = { - "urdf_zero_offset_rad": round(offsets[name], 8) - } - else: - item["passive"] = True - joints[name] = item - absolute_errors = np.abs( - np.asarray(validation_errors_rad, dtype=float) - ) - if absolute_errors.size == 0 or not np.all(np.isfinite(absolute_errors)): - raise ValueError("standalone thumb validation errors are missing") - return { - "schema_version": 1, - "artifact_type": "g20_right_standalone_thumb_calibration", - "model": "G20", - "side": "right", - "serial_number": str(serial_number), - "angle_unit": "rad", - "command_range": [0, 255], - "baseline_command_u8": [int(value) for value in baseline_command_u8], - "non_thumb_zero_policy": "source_cad_unchanged", - "joints": joints, - "quality": { - "passed": True, - "validation_mae_rad": round(float(np.mean(absolute_errors)), 8), - "validation_p95_rad": round( - float(np.percentile(absolute_errors, 95.0)), 8 - ), - }, - "hashes": { - "source_urdf_sha256": str(source_urdf_sha256), - "camera_extrinsics_sha256": str(camera_extrinsics_sha256), - "corrected_urdf_sha256": str(corrected_urdf_sha256), - }, - } - - -def _partial_scope_frozen_zero_offsets( - profile: HandCalibrationProfile, - scope: str, - payload: Mapping[str, Any], -) -> dict[str, float]: - """Return certified non-target zeros that a partial run must preserve.""" - selected = str(scope).strip().lower() - if selected == "full": - return {} - if selected not in {"thumb", "fingers"}: - raise ValueError(f"unsupported partial recalibration scope: {scope}") - joints = payload.get("joints") - if not isinstance(joints, Mapping): - raise ValueError("base calibration payload is missing joints") - freeze_thumb = selected == "fingers" - result: dict[str, float] = {} - for name in get_zero_calibration_profile( - profile.side, profile.layout_id - ).direct_zero_joints: - is_thumb = str(name).startswith("thumb_") - if is_thumb != freeze_thumb: - continue - try: - value = float( - joints[name]["zero_angles"]["urdf_zero_offset_rad"] - ) - except (KeyError, TypeError, ValueError) as error: - raise ValueError( - f"base calibration is missing certified zero for {name}" - ) from error - if not math.isfinite(value): - raise ValueError(f"base calibration zero is invalid for {name}") - result[name] = value - expected = 4 if freeze_thumb else 12 - if len(result) != expected: - raise ValueError( - f"partial {selected} scope must freeze exactly {expected} zeros" - ) - return result - - -def _normalise_legacy_split_roll_resume_rows( - profile: HandCalibrationProfile, - rows: Sequence[Mapping[str, Any]], -) -> tuple[dict[str, Any], ...]: - """Map the former front/side roll tasks onto one multiview checkpoint. - - This lets a calibration that was already running during the task-layout - upgrade donate all fully completed data if it later needs to resume. The - two historical motions remain independent observations; they are never - presented as timestamp-synchronised frames or accepted unless both joint - record sets pass the normal completeness checks. - """ - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return tuple(dict(row) for row in rows) - aliases = { - f"{finger}_roll_{view}": f"{finger}_roll_multiview" - for finger in ("pinky", "ring", "middle", "index") - for view in ("front", "side") - } - normalised: list[dict[str, Any]] = [] - for source in rows: - row = dict(source) - old_task = str(row.get("task_name", "")) - replacement = aliases.get(old_task) - if ( - replacement is not None - and row.get("kind") != "synchronised_frame" - ): - row["task_name"] = replacement - row["resume_source_task_name"] = old_task - normalised.append(row) - return tuple(normalised) - - -def _discard_automatically_retried_sweep_rows( - rows: Sequence[Mapping[str, Any]], -) -> tuple[dict[str, Any], ...]: - """Replay automatic-sweep retry tombstones before checkpoint selection. - - An automatic acquisition retry clears that cycle/direction from the - online in-memory stores. The raw JSONL is append-only, so the superseded - steady checkpoints remain physically present before the - ``automatic_sweep_retry`` event. Treat that event as a write-ahead - tombstone: records before it must never be merged with the replacement - acquisition written afterwards. - - Older sessions did not include ``task_name`` on retry events. Their - motor/cycle/direction and joint list still identify the invalidated - acquisition unambiguously. New sessions include both task and attempt - for a stronger durable identity. - """ - materialised = tuple(dict(row) for row in rows) - retry_events = [ - (index, row) - for index, row in enumerate(materialised) - if str(row.get("kind", "")) == "automatic_sweep_retry" - ] - if not retry_events: - return materialised - - durable_kinds = set(RESUMABLE_SAMPLE_KINDS) | { - "palm_axis_sample", - "synchronised_frame", - } - - def invalidated( - row_index: int, row: Mapping[str, Any] - ) -> bool: - if str(row.get("kind", "")) not in durable_kinds: - return False - row_task = str(row.get("task_name", "")) - row_cycle = int(row.get("cycle", -999)) - row_direction = str(row.get("direction", "")) - row_motor = int(row.get("motor_index", -1)) - row_joints = { - str(name) - for name in ( - row.get("joint"), - row.get("model_joint"), - *(row.get("joints", ()) or ()), - ) - if name - } - for retry_index, retry in retry_events: - if retry_index <= row_index: - continue - if int(retry.get("cycle", -998)) != row_cycle: - continue - if str(retry.get("direction", "")) != row_direction: - continue - retry_task = str(retry.get("task_name", "")) - if retry_task and row_task != retry_task: - continue - retry_motor = int(retry.get("motor_index", -1)) - if ( - row_motor >= 0 - and retry_motor >= 0 - and row_motor != retry_motor - ): - continue - retry_joints = { - str(name) for name in (retry.get("joints", ()) or ()) if name - } - if ( - row_joints - and retry_joints - and row_joints.isdisjoint(retry_joints) - ): - continue - return True - return False - - return tuple( - row - for index, row in enumerate(materialised) - if not invalidated(index, row) - ) - - -def _latest_resume_rows( - rows: Sequence[Mapping[str, Any]], - *, - kind: str, - minimum_sweep_bins: int = 0, -) -> dict[tuple[str, str, int, str], list[dict[str, Any]]]: - """Select the latest *complete* persisted attempt for each group. - - An interrupted retry must not hide the previous complete scan. This is - especially important for the nine steady checkpoints, which are written - before the continuous trajectory starts. - """ - selected = [ - dict(row) - for row in rows - if row.get("kind") == kind - and row.get("task_name") - and row.get("joint") - and row.get("direction") in {DIRECTION_DECREASING, DIRECTION_INCREASING} - ] - grouped: dict[ - tuple[str, str, int, str], dict[int, list[dict[str, Any]]] - ] = {} - for row in selected: - key = ( - str(row["task_name"]), - str(row["joint"]), - int(row.get("cycle", -1)), - str(row["direction"]), - ) - grouped.setdefault(key, {}).setdefault( - int(row.get("attempt", 1)), [] - ).append(row) - result: dict[tuple[str, str, int, str], list[dict[str, Any]]] = {} - for key, attempts in grouped.items(): - for _attempt, candidate in sorted( - attempts.items(), reverse=True - ): - if kind == "steady_command_sample": - requested = { - int(row.get("requested_command_u8", -1)) - for row in candidate - } - complete = requested == set(STEADY_COMMAND_CHECKPOINTS) - elif kind == "sample": - feedback_bins = { - int(round(float(row.get("feedback_u8", -1)))) - for row in candidate - } - complete = ( - 0 in feedback_bins - and 255 in feedback_bins - and len(feedback_bins) >= int(minimum_sweep_bins) - ) - else: - complete = bool(candidate) - if complete: - result[key] = candidate - break - return result - - -def _unresolved_fit_failure_tasks( - profile: HandCalibrationProfile, - rows: Sequence[Mapping[str, Any]], -) -> set[str]: - """Return tasks whose newest complete acquisition was still rejected. - - A newer model may deliberately retire a metric for a constrained joint. - Such historical failures are safe to replay because the final fit will - evaluate the imported raw samples against all current quality gates. - """ - sample_attempts: dict[str, int] = {} - for row in rows: - task_name = str(row.get("task_name", "")) - if row.get("kind") == "sample" and task_name: - sample_attempts[task_name] = max( - sample_attempts.get(task_name, 0), - int(row.get("attempt", 1)), - ) - zero_profile = get_zero_calibration_profile( - profile.side, profile.layout_id - ) - unresolved: set[str] = set() - for row in rows: - if row.get("kind") != "fit_failure": - continue - task_name = str(row.get("task_name", "")) - if not task_name: - matches = [ - spec - for spec in profile.sweep_specs - if spec.view == str(row.get("view", "")) - and spec.motor_index == int(row.get("motor_index", -1)) - and set(spec.joints) - == {str(name) for name in row.get("joints", [])} - ] - if len(matches) != 1: - continue - task_name = matches[0].key - failure_attempt = int(row.get("attempt", 1)) - if failure_attempt < sample_attempts.get(task_name, 0): - continue - failures = [ - item - for item in row.get("failures", []) - if isinstance(item, Mapping) - ] - def retired(item: Mapping[str, Any]) -> bool: - metric = str(item.get("metric", "")) - joint_name = str(item.get("joint", "")) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric == "rotation_orthogonal_rms_deg" - and joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS - ): - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric in { - "hysteresis_deg", - "command_direction_gap_deg", - } - ): - # Product curves now gate direction dependence with settled - # feedback-domain samples and final-cycle holdout. Historical - # failures that mixed velocity lag or firmware tracking - # deadband with mechanical hysteresis are safe to revalidate. - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric == "axis_pose_line_rms_mm" - and joint_name in profile.record_specs - and not profile.record_specs[ - joint_name - ].pose_axis_line_required - ): - # Current profile policy owns whether this monocular 3-D - # diagnostic is release-critical. Revalidate the complete - # raw task under that policy instead of making an old failure - # force another acquisition forever. - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and joint_name - in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS - and metric == "axis_pose_line_rms_mm" - ): - # Product v19 originally treated the correlated per-frame - # pose-line residual of an incorrectly scaled passive-DIP Tag as the - # uncertainty of its fitted axis centre. Current code - # revalidates those complete raw trajectories using radial, - # cross-cycle and holdout gates, so the old rejection itself - # must not force another eight-direction scan. - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric == "cross_view_roll_curve" - and joint_name in (profile.axis_validation_sources or {}) - ): - # The side roll alias is a validation-only observation in the - # current product model. Its raw samples are still - # revalidated through side-line radial/cycle/holdout gates, - # but a historical front/side curve-shape warning must not - # make a complete task look unfinished on every restart. - return True - return metric in { - "rotation_circle_axis_difference_deg", - "axis_plane_rms_mm", - } and circle_direction_is_constrained( - joint_name, - zero_profile.constrained_circle_joints, - ) - - retired_by_current_model = bool(failures) and all( - retired(item) for item in failures - ) - if not retired_by_current_model: - unresolved.add(task_name) - return unresolved - - -def resumable_completed_task_prefix( - profile: HandCalibrationProfile, - repetitions: int, - baseline: Sequence[int], - rows: Sequence[Mapping[str, Any]], - *, - minimum_sweep_bins: int = 32, - allow_sparse: bool = False, -) -> tuple[tuple[str, ...], tuple[dict[str, Any], ...]]: - """Return quality-preserving complete tasks and their raw records. - - A partially written task is never reused. Every joint, all four cycles, - both directions, both endpoints, the two nine-point command curves and - any required directional baseline holds must be present before the whole - task becomes a durable restart checkpoint. The compatibility default - returns only a contiguous prefix; product resume can opt into sparse - recovery so a failed early task does not discard independent later tasks. - """ - rows = _discard_automatically_retried_sweep_rows( - _normalise_legacy_split_roll_resume_rows(profile, rows) - ) - samples = _latest_resume_rows( - rows, - kind="sample", - minimum_sweep_bins=minimum_sweep_bins, - ) - baselines = _latest_resume_rows(rows, kind="baseline_hold_sample") - checkpoints = _latest_resume_rows(rows, kind="steady_command_sample") - directions = (DIRECTION_DECREASING, DIRECTION_INCREASING) - expected_checkpoints = set(STEADY_COMMAND_CHECKPOINTS) - unresolved_fit_failures = _unresolved_fit_failure_tasks(profile, rows) - completed: list[str] = [] - selected_group_keys: set[tuple[str, tuple[str, str, int, str]]] = set() - - for spec in profile.sweep_specs: - if spec.key in unresolved_fit_failures: - if allow_sparse: - continue - break - complete = True - task_group_keys: set[ - tuple[str, tuple[str, str, int, str]] - ] = set() - for joint_name in spec.joints: - joint = profile.record_specs[joint_name] - needs_baseline_hold = int(baseline[joint.motor_index]) not in { - 0, - 255, - } - for cycle in range(int(repetitions)): - for direction in directions: - key = (spec.key, joint_name, cycle, direction) - sample_rows = samples.get(key, []) - feedback_bins = { - int(round(float(row.get("feedback_u8", -1)))) - for row in sample_rows - } - if ( - len(feedback_bins) < int(minimum_sweep_bins) - or 0 not in feedback_bins - or 255 not in feedback_bins - ): - complete = False - break - task_group_keys.add(("sample", key)) - if needs_baseline_hold: - if not baselines.get(key): - complete = False - break - task_group_keys.add(("baseline_hold_sample", key)) - if not complete: - break - if not complete: - break - for direction in directions: - key = (spec.key, joint_name, 0, direction) - requested = { - int(row.get("requested_command_u8", -1)) - for row in checkpoints.get(key, []) - } - if requested != expected_checkpoints: - complete = False - break - task_group_keys.add(("steady_command_sample", key)) - if not complete: - break - if not complete: - if allow_sparse: - continue - break - completed.append(spec.key) - selected_group_keys.update(task_group_keys) - - reusable: list[dict[str, Any]] = [] - grouped = { - "sample": samples, - "baseline_hold_sample": baselines, - "steady_command_sample": checkpoints, - } - for kind, key in selected_group_keys: - reusable.extend(grouped[kind][key]) - completed_set = set(completed) - latest_task_attempt = { - (key[0], key[2], key[3]): max( - int(row.get("attempt", 1)) for row in sample_rows - ) - for key, sample_rows in samples.items() - if key[0] in completed_set and sample_rows - } - reusable.extend( - dict(row) - for row in rows - if row.get("kind") == "synchronised_frame" - and str(row.get("task_name", "")) in completed_set - and int(row.get("attempt", 1)) - == latest_task_attempt.get( - ( - str(row.get("task_name", "")), - int(row.get("cycle", -1)), - str(row.get("direction", "")), - ), - -1, - ) - ) - reusable.extend( - dict(row) - for row in rows - if row.get("kind") == "palm_axis_sample" - and str(row.get("task_name", "")) in completed_set - and int(row.get("attempt", 1)) - == latest_task_attempt.get( - ( - str(row.get("task_name", "")), - int(row.get("cycle", -1)), - str(row.get("direction", "")), - ), - -1, - ) - ) - reusable.sort( - key=lambda row: ( - profile.sweep_specs.index( - next(spec for spec in profile.sweep_specs if spec.key == row["task_name"]) - ), - int(row.get("cycle", -1)), - 0 if row.get("direction") == DIRECTION_DECREASING else 1, - 0 if row.get("kind") == "synchronised_frame" else 1, - str(row.get("joint", "")), - float(row.get("feedback_u8", row.get("requested_command_u8", 0))), - ) - ) - return tuple(completed), tuple(reusable) - - -def _file_sha256(path: str | Path) -> str: - digest = hashlib.sha256() - with Path(path).open("rb") as stream: - for chunk in iter(lambda: stream.read(1024 * 1024), b""): - digest.update(chunk) - return digest.hexdigest() - - -def _publish_import_status(node: Any) -> None: - """Publish synchronous import progress when running inside a ROS node.""" - publisher = getattr(node, "_publish_status", None) - if callable(publisher): - publisher(time.monotonic()) - - -def _steady_records_in_feedback_domain( - records: Sequence[Mapping[str, Any]], -) -> list[dict[str, Any]]: - """Re-index settled poses by feedback for mechanical backlash checks. - - Runtime curves remain indexed by requested command. This companion - representation separates intrinsic hysteresis from the firmware's - direction-dependent command/feedback deadband. Endpoint records have - already passed their motor-specific reach gate, so they are snapped to - the exact endpoints required by the curve fitter. - """ - return fitting_sample_records( - records, - domain="feedback", - allow_legacy_command=True, - snap_requested_endpoints=True, - ) - - -def _classify_cross_view_roll_hysteresis( - front_values_deg: Sequence[float], - side_values_deg: Sequence[float], - *, - limit_deg: float, -) -> str: - """Classify a diagnostic without turning it into a formal acceptance.""" - front = np.asarray(front_values_deg, dtype=float) - side = np.asarray(side_values_deg, dtype=float) - if front.size == 0 or side.size == 0: - raise ValueError("cross-view diagnostic requires both camera results") - front_max = float(np.max(front)) - side_max = float(np.max(side)) - if front_max <= limit_deg and side_max <= limit_deg: - return "both_views_within_formal_hysteresis_limit" - if front_max > limit_deg and side_max > limit_deg: - return "both_views_confirm_direction_dependent_pose" - if front_max > limit_deg: - return "front_only_difference_check_roll_tag_bracket_or_front_pnp" - return "side_only_difference_check_side_tag_chain_or_side_pnp" - - -def _fit_failure_is_systematic( - failures: Sequence[Mapping[str, Any]], repetitions: int -) -> bool: - """Identify a repeatable model conflict that more motion cannot repair.""" - if not failures or int(repetitions) <= 1: - return False - if all(bool(item.get("systematic", False)) for item in failures): - return True - if any( - str(item.get("metric", "")) - != "rotation_circle_axis_difference_deg" - for item in failures - ): - return False - cycles = {int(item.get("cycle", -1)) for item in failures} - if cycles != set(range(1, int(repetitions) + 1)): - return False - actual = np.asarray([float(item["actual"]) for item in failures]) - limits = np.asarray([float(item["limit"]) for item in failures]) - if ( - not np.all(np.isfinite(actual)) - or not np.all(np.isfinite(limits)) - or np.any(limits <= 0.0) - ): - return False - # Every independent round is far outside the limit and agrees with the - # others. This is a fixed geometry/PnP model conflict, not random capture - # loss, so two full four-round retries would only reproduce it. - return bool( - np.all(actual >= 1.5 * limits) - and float(np.ptp(actual)) <= max(0.5, 0.2 * float(np.mean(actual))) - ) - - -def _cross_view_curve_failure( - primary_name: str, - validation_name: str, - primary: JointCurveFit, - validation: JointCurveFit, - *, - scope: str, - maximum_rms_difference_rad: float, - maximum_branch_gap_difference_rad: float, - allow_projection_scale: bool = False, - maximum_projection_scale_ratio: float = 1.5, - cycle_fits: Sequence[tuple[JointCurveFit, JointCurveFit]] = (), -) -> dict[str, Any] | None: - """Return a localized, retry-aware cross-view failure. - - A stable offset repeated in every cycle is an installation/model conflict; - reacquiring the same four motions cannot change it. A single bad cycle is - instead localized so only that side-view cycle is reacquired. - """ - try: - compare_cross_view_roll_curves( - primary, - validation, - maximum_rms_difference_rad=maximum_rms_difference_rad, - maximum_branch_gap_difference_rad=( - maximum_branch_gap_difference_rad - ), - allow_projection_scale=allow_projection_scale, - maximum_projection_scale_ratio=( - maximum_projection_scale_ratio - ), - ) - return None - except ValueError as error: - reason = str(error) - - metrics: dict[str, float] = {} - if reason != "cross_view_roll_curve_direction_disagrees": - metrics = compare_cross_view_roll_curves( - primary, - validation, - maximum_rms_difference_rad=float("inf"), - maximum_branch_gap_difference_rad=float("inf"), - allow_projection_scale=allow_projection_scale, - maximum_projection_scale_ratio=float("inf"), - ) - failure: dict[str, Any] = { - "joint": primary_name, - "metric": "cross_view_roll_curve", - "reason": reason, - "scope": scope, - "quality_source_joints": [validation_name], - } - metric_key: str | None = None - metric_limit: float | None = None - metric_in_degrees = True - if reason.startswith("cross_view_roll_curve_difference_too_large:"): - field_name = reason.split(":", 2)[1] - metric_key = f"{field_name}_rms_difference_rad" - metric_limit = float(maximum_rms_difference_rad) - elif reason.startswith( - "cross_view_roll_branch_gap_difference_too_large:" - ): - metric_key = "baseline_branch_gap_difference_rad" - metric_limit = float(maximum_branch_gap_difference_rad) - elif reason.startswith( - "cross_view_roll_projection_scale_ratio_too_large:" - ): - metric_key = "projection_scale_ratio" - metric_limit = float(maximum_projection_scale_ratio) - metric_in_degrees = False - - cycle_values_rad: list[float] = [] - cycle_direction_disagrees: list[bool] = [] - for primary_cycle, validation_cycle in cycle_fits: - try: - cycle_metrics = compare_cross_view_roll_curves( - primary_cycle, - validation_cycle, - maximum_rms_difference_rad=float("inf"), - maximum_branch_gap_difference_rad=float("inf"), - allow_projection_scale=allow_projection_scale, - maximum_projection_scale_ratio=float("inf"), - ) - except ValueError as cycle_error: - if str(cycle_error) != "cross_view_roll_curve_direction_disagrees": - raise - cycle_metrics = {} - if metric_key is not None: - value = cycle_metrics.get(metric_key) - if value is not None: - cycle_values_rad.append(float(value)) - primary_travel = float( - primary_cycle.angle_rad[0] - primary_cycle.angle_rad[255] - ) - validation_travel = float( - validation_cycle.angle_rad[0] - - validation_cycle.angle_rad[255] - ) - cycle_direction_disagrees.append( - primary_travel * validation_travel <= 0.0 - ) - - if metric_key is not None and metric_limit is not None: - actual_value = float(metrics[metric_key]) - rendered_actual = ( - math.degrees(actual_value) - if metric_in_degrees - else actual_value - ) - rendered_limit = ( - math.degrees(metric_limit) - if metric_in_degrees - else metric_limit - ) - failure.update( - { - "actual": round(rendered_actual, 6), - "limit": round(rendered_limit, 6), - "comparison": "maximum", - "unit": "deg" if metric_in_degrees else "ratio", - } - ) - if cycle_values_rad: - values = np.asarray(cycle_values_rad, dtype=float) - cycle_key = ( - "cycle_values_deg" - if metric_in_degrees - else "cycle_values_ratio" - ) - failure[cycle_key] = [ - round( - math.degrees(value) if metric_in_degrees else value, - 6, - ) - for value in values - ] - over = np.flatnonzero(values > metric_limit) - if ( - over.size == 1 - and np.all( - np.delete(values, over[0]) <= 0.9 * metric_limit - ) - ): - failure["cycle"] = int(over[0]) + 1 - # All cycles tell the same near-threshold story: this is a fixed - # view/mount/model bias, not random data that another scan heals. - if ( - np.all(values >= 0.85 * metric_limit) - and float(np.ptp(values)) <= 0.35 * metric_limit - ): - failure["systematic"] = True - failure["classification"] = ( - "stable_cross_view_installation_or_model_bias" - ) - elif reason == "cross_view_roll_curve_direction_disagrees" and ( - cycle_direction_disagrees - and all(cycle_direction_disagrees) - ): - failure["systematic"] = True - failure["classification"] = "stable_cross_view_direction_conflict" - return failure - - -def _fit_failure_repeats_branch_clusters( - previous: Sequence[Mapping[str, Any]], - current: Sequence[Mapping[str, Any]], -) -> bool: - """Detect the same cross-cycle PnP branch split on two full retries.""" - - def travel_failure( - failures: Sequence[Mapping[str, Any]], - ) -> Mapping[str, Any] | None: - return next( - ( - item - for item in failures - if str(item.get("metric")) == "cycle_travel_range_deg" - and isinstance(item.get("cycle_travel_deg"), Sequence) - ), - None, - ) - - left = travel_failure(previous) - right = travel_failure(current) - if left is None or right is None: - return False - left_values = np.asarray(left["cycle_travel_deg"], dtype=float) - right_values = np.asarray(right["cycle_travel_deg"], dtype=float) - if ( - left_values.shape != right_values.shape - or left_values.size < 3 - or not np.all(np.isfinite(left_values)) - or not np.all(np.isfinite(right_values)) - or np.max(np.abs(left_values - right_values)) > 1.0 - ): - return False - limit = float(right.get("limit", 0.0)) - if limit <= 0.0 or float(np.ptp(right_values)) <= limit: - return False - - previous_by_metric = { - (str(item.get("joint")), str(item.get("metric"))): item - for item in previous - if "actual" in item and "limit" in item - } - stable_model_failures = 0 - for item in current: - key = (str(item.get("joint")), str(item.get("metric"))) - old = previous_by_metric.get(key) - if old is None or key[1] == "cycle_travel_range_deg": - continue - actual = float(item.get("actual", float("nan"))) - old_actual = float(old.get("actual", float("nan"))) - item_limit = float(item.get("limit", 0.0)) - if ( - np.isfinite(actual) - and np.isfinite(old_actual) - and item_limit > 0.0 - and actual > item_limit - and old_actual > item_limit - and abs(actual - old_actual) <= max(0.25, 0.1 * item_limit) - ): - stable_model_failures += 1 - return stable_model_failures >= 1 - - -def _robust_pose_payload( - poses: Sequence[Mapping[str, Sequence[float]]], -) -> dict[str, list[float]]: - if not poses: - raise ValueError("at least one pose is required") - translation = np.median( - np.asarray([pose["translation_xyz_m"] for pose in poses], dtype=float), - axis=0, - ) - quaternion = robust_rotation_summary( - [pose["quaternion_xyzw"] for pose in poses] - )[0] - return { - "translation_xyz_m": [float(value) for value in translation], - "quaternion_xyzw": [float(value) for value in quaternion], - } - - -def _view_pairs( - view: str, reference_finger: str = "index" -) -> tuple[tuple[str, str], ...]: - if view == "front": - return ( - ("front_base", "thumb_cmc"), - ("thumb_cmc", "thumb_mcp"), - ("thumb_mcp", "thumb_ip"), - ("front_base", f"{reference_finger}_roll"), - ) - if view == "side": - return ( - ("side_base", "thumb_cmc"), - ("side_base", f"{reference_finger}_mcp"), - (f"{reference_finger}_mcp", f"{reference_finger}_pip"), - (f"{reference_finger}_pip", f"{reference_finger}_dip"), - ) - if view == "top": - return (("top_base", "thumb_yaw"),) - raise ValueError(f"unknown view {view}") - - -@dataclass(frozen=True) -class FrameObservation: - stamp_ns: int - received_at: float - view: str - state_u8: tuple[float, ...] - state_sync_error_ns: int - joint_vectors_xyz_m: Mapping[str, tuple[float, float, float]] - image_vectors_xy_px: Mapping[str, tuple[float, float]] - joint_quaternions_xyzw: Mapping[str, tuple[float, float, float, float]] - parent_poses_common: Mapping[str, Mapping[str, list[float]]] - child_poses_common: Mapping[str, Mapping[str, list[float]]] - joint_reprojection_error_px: Mapping[str, float] - - -@dataclass(frozen=True) -class SweepItem: - spec: SweepSpec - cycle: int - direction: str - precheck: bool = False - - @property - def start_u8(self) -> int: - return 255 if self.direction == DIRECTION_DECREASING else 0 - - @property - def target_u8(self) -> int: - return 0 if self.direction == DIRECTION_DECREASING else 255 - - -def _build_sweep_plan( - profile: HandCalibrationProfile, repetitions: int -) -> tuple[SweepItem, ...]: - """Build one deterministic qualification-and-measurement transaction. - - Each product task owns a low-speed outbound/return qualification followed - by fixed-order formal cycles. Keeping this construction in one place - prevents resume, progress and runtime transitions from silently inventing - different task lifecycles. - """ - plan: list[SweepItem] = [] - for spec in profile.sweep_specs: - if profile.precheck_sweeps: - plan.extend( - SweepItem(spec, -1, direction, precheck=True) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - plan.extend( - SweepItem(spec, cycle, direction) - for cycle in range(int(repetitions)) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - return tuple(plan) - - -def _sweep_plan_transition( - profile: HandCalibrationProfile, - completed: SweepItem, - following: SweepItem, -) -> str: - """Return the only allowed boundary between two planned sweep items.""" - if completed.spec != following.spec: - return "task_change" - # Both qualification directions and the qualification-to-formal handoff - # are one continuous transaction. The physical endpoint and task - # clearance pose are unchanged, so a baseline detour can only discard - # good observations and perturb the PnP branch. - if completed.precheck: - return "immediate_reverse" - if completed.direction == DIRECTION_INCREASING: - # Formal cycles deliberately re-establish the same mechanical approach - # between repetitions so hysteresis measurements remain comparable. - return "cycle_reset" - return "immediate_reverse" - - -@dataclass(frozen=True) -class ValidationItem: - spec: SweepSpec - command_u8: int - - -@dataclass(frozen=True) -class CombinationValidationItem: - name: str - label_zh: str - command_u8: tuple[int, ...] - - -def _combination_validation_items( - baseline: Sequence[int], -) -> tuple[CombinationValidationItem, ...]: - if len(baseline) != 20: - raise ValueError("combination baseline must contain 20 commands") - base = [int(value) for value in baseline] - - def item( - name: str, label: str, changes: Mapping[int, int] - ) -> CombinationValidationItem: - values = list(base) - for index, value in changes.items(): - values[int(index)] = int(value) - return CombinationValidationItem(name, label, tuple(values)) - - return ( - item("all_open", "全开", {}), - item("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), - item("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), - item("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), - item("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), - item("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), - item( - "half_grip", - "四指半握", - {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, - ), - item( - "light_pinch", - "轻捏", - {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, - ), - ) - - -def _combination_joint_angles( - profile: HandCalibrationProfile, - measured_fits: Mapping[str, JointCurveFit], - command_u8: Sequence[int], - motor_directions: Sequence[str], -) -> dict[str, float]: - """Evaluate a combination pose on the branch used to reach it. - - Using ``angle_rad`` here averages the increasing and decreasing branches; - on a serial chain (especially the thumb) those half-hysteresis errors - accumulate and can reject an otherwise correct pose. - """ - if len(command_u8) != 20 or len(motor_directions) != 20: - raise ValueError("combination commands and directions require 20 motors") - angles: dict[str, float] = {} - for name, spec in profile.joint_specs.items(): - target = int(command_u8[spec.motor_index]) - fit = measured_fits[name] - direction = str(motor_directions[spec.motor_index]) - if direction == DIRECTION_DECREASING: - curve = fit.decreasing_rad - elif direction == DIRECTION_INCREASING: - curve = fit.increasing_rad - else: - raise ValueError(f"invalid combination direction: {direction}") - angles[name] = float(curve[target]) - return angles - - -def _combination_motor_directions( - items: Sequence[CombinationValidationItem], - pose_index: int, - baseline_u8: Sequence[int], -) -> tuple[str, ...]: - """Reconstruct the direction in which every motor reached a pose. - - Pose zero establishes the all-open Tag mounts. Each later pose starts - from that baseline. A motor moved by an earlier pose returns along the - opposite branch and stays on that branch until another pose moves it. - """ - if len(baseline_u8) != 20: - raise ValueError("combination baseline must contain 20 motors") - if pose_index < 0 or pose_index >= len(items): - raise ValueError("combination pose index is out of range") - baseline = tuple(int(value) for value in baseline_u8) - directions = [DIRECTION_DECREASING] * 20 - for previous in items[1:pose_index]: - for motor, target in enumerate(previous.command_u8): - if int(target) < baseline[motor]: - directions[motor] = DIRECTION_INCREASING - elif int(target) > baseline[motor]: - directions[motor] = DIRECTION_DECREASING - current = items[pose_index] - for motor, target in enumerate(current.command_u8): - if int(target) < baseline[motor]: - directions[motor] = DIRECTION_DECREASING - elif int(target) > baseline[motor]: - directions[motor] = DIRECTION_INCREASING - return tuple(directions) - - -def _model_link_in_observer_base( - observer_base_common: Sequence[Sequence[float]], - model_base_common: Sequence[Sequence[float]], - model_link_from_base: Sequence[Sequence[float]], -) -> np.ndarray: - """Express a URDF link in the fixed palm Tag's coordinate frame.""" - observer = np.asarray(observer_base_common, dtype=float) - model_base = np.asarray(model_base_common, dtype=float) - link = np.asarray(model_link_from_base, dtype=float) - if any(value.shape != (4, 4) for value in (observer, model_base, link)): - raise ValueError("combination transforms must be 4x4 matrices") - return np.linalg.inv(observer) @ model_base @ link - - -@dataclass -class ViewRuntime: - name: str - tag_size_m: float - preflight_frames: int - tracker: SquareTagPoseTracker - group_tracker: SquareTagGroupPoseTracker - view_tags: Mapping[str, int] = field(default_factory=dict) - tag_sizes_m_by_role: Mapping[str, float] = field(default_factory=dict) - preflight_roles: tuple[str, ...] = () - role_by_id: dict[int, str] = field(default_factory=dict) - group_trackers: dict[ - tuple[str, ...], SquareTagGroupPoseTracker - ] = field(default_factory=dict) - current_required_roles: tuple[str, ...] = () - camera_matrix: np.ndarray | None = None - camera_info_valid: bool = False - camera_frame: str = "" - image_width: int = 0 - image_height: int = 0 - intrinsics_sha256: str = "" - extrinsics_valid: bool = False - valid_flags: deque[bool] = field(default_factory=deque) - detection_times: deque[float] = field(default_factory=deque) - # Task-scoped capture validity: only frames judged against an active - # task's required roles are counted. The rolling valid_flags window is - # cleared whenever the required-role set changes, so right after a task - # finishes it silently switches to scoring idle frames against the full - # preflight role set — the wrong measurement for a per-task data gate. - task_valid_frames: int = 0 - task_total_frames: int = 0 - latest_tag_quality: dict[str, TagQuality] = field(default_factory=dict) - latest_pnp_rejections: dict[str, str] = field(default_factory=dict) - latest_group_pnp_reason: str = "" - latest_group_missing_candidate_roles: tuple[str, ...] = () - # Task-scoped counters preserve the reasons that occurred before the last - # camera frame. A final ``group_initializing:5/8`` frame must not hide a - # recurring per-Tag rejection or a completed-window geometry rejection. - pnp_rejection_counts: dict[str, int] = field(default_factory=dict) - group_pnp_rejection_counts: dict[str, int] = field(default_factory=dict) - pnp_initialization_progress: tuple[int, int] | None = None - latest_pnp_valid: bool = False - last_pnp_diagnostic_signature: tuple[Any, ...] | None = None - pnp_invalid_since: float | None = None - pnp_reset_count: int = 0 - last_message_at: float = 0.0 - last_valid_at: float = 0.0 - fixed_base_observations: deque[ - tuple[SquareTagPose, tuple[float, float], TagQuality] - ] = field(default_factory=deque) - fixed_base_corner_observations: deque[np.ndarray] = field( - default_factory=deque - ) - locked_base_pose: SquareTagPose | None = None - locked_base_center_xy_px: tuple[float, float] | None = None - locked_base_corners_xy: tuple[ - tuple[float, float], - tuple[float, float], - tuple[float, float], - tuple[float, float], - ] | None = None - locked_base_quality: TagQuality | None = None - locked_base_corner_drift_count: int = 0 - latest_locked_base_corner_drift_px: float = 0.0 - - def __post_init__(self) -> None: - self.role_by_id = { - tag_id: role for role, tag_id in self.view_tags.items() - } - self.valid_flags = deque(maxlen=self.preflight_frames) - self.detection_times = deque(maxlen=self.preflight_frames) - self.fixed_base_observations = deque(maxlen=self.preflight_frames) - self.fixed_base_corner_observations = deque( - maxlen=self.preflight_frames - ) - if not self.preflight_roles: - self.preflight_roles = tuple(self.view_tags) - if any(role not in self.view_tags for role in self.preflight_roles): - raise ValueError("preflight roles must be configured for the view") - if any(role not in self.view_tags for role in self.tag_sizes_m_by_role): - raise ValueError("Tag-size roles must be configured for the view") - if any( - not math.isfinite(float(size)) or float(size) <= 0.0 - for size in self.tag_sizes_m_by_role.values() - ): - raise ValueError("Tag sizes must be finite and positive") - self.current_required_roles = tuple(self.preflight_roles) - self.group_trackers[tuple(self.preflight_roles)] = self.group_tracker - - @property - def roles(self) -> tuple[str, ...]: - return tuple(self.view_tags) - - @property - def valid_rate(self) -> float: - if not self.valid_flags: - return 0.0 - return float(sum(self.valid_flags) / len(self.valid_flags)) - - @property - def detection_hz(self) -> float: - if len(self.detection_times) < 2: - return 0.0 - elapsed = self.detection_times[-1] - self.detection_times[0] - return 0.0 if elapsed <= 0.0 else (len(self.detection_times) - 1) / elapsed - - -class G20ThreeCameraCalibrationNode(Node): - """Own all hand commands while observing three fixed Hikrobot views.""" - - def __init__(self) -> None: - super().__init__("g20_calibration") - self._declare_parameters() - self._load_parameters() - - self.session_dir.mkdir(parents=True, exist_ok=True) - self.raw_path = self.session_dir / "raw_samples.jsonl" - self.final_path = self.session_dir / ( - f"g20_{self.hand_type}_{_safe_name(self.serial_number)}_calibration.json" - ) - - self.views = { - name: self._make_view_runtime(name) - for name in ("front", "side", "top") - } - self.latest_state_u8: tuple[float, ...] = () - self.last_state_at = 0.0 - self.state_history: deque[StateSample] = deque(maxlen=1200) - self.state_receive_times: deque[float] = deque(maxlen=300) - self.latest_hand_info: dict[str, Any] = {} - self.commanded_speed_profile: tuple[int, ...] = () - self.speed_commanded_at = 0.0 - - self.state = STATE_PREFLIGHT - self.reason = "waiting_for_devices_and_sdk" - self.sample_schema_version = 1 - self.base_import_progress: dict[str, Any] = {} - self.paused_reason = "" - self.started = False - # Startup has two deliberately separate gates. Camera/SDK health is - # enough to let us recover the hand from an interrupted pose; fixed - # palm Tags are checked only after that recovery reaches baseline. - self.startup_baseline_recovered = False - self.abort_original_reason = "" - self.baseline_after = "" - self.position_hold_since: float | None = None - self.motion_stage_started_at = 0.0 - self.return_waypoints: deque[tuple[int, ...]] = deque() - self.preparation_waypoints: deque[tuple[int, ...]] = deque() - self.preparation_command_u8: tuple[int, ...] = tuple( - self.baseline_command - ) - - self.sweep_items: list[SweepItem] = [] - self.sweep_index = 0 - self.active_sweep: SweepItem | None = None - # Owns required Tag roles and PnP continuity across the brief - # active_sweep=None interval used for same-task cycle resets. - self.pnp_task_spec: SweepSpec | None = None - self.sweep_frames: list[FrameObservation] = [] - # Roll joints use command 127 as their zero. A frame acquired while - # merely passing 127 contains velocity/latency error and is not a - # valid backlash observation. Keep a separate buffer populated only - # after the motor has reached 127 and the commanded mid-sweep hold has - # started. - self.sweep_baseline_frames: list[FrameObservation] = [] - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since: float | None = None - self.sweep_checkpoint_commands: deque[int] = deque() - self.sweep_checkpoint_mode = False - self.sweep_checkpoint_target_u8: int | None = None - self.sweep_checkpoint_hold_since: float | None = None - self.sweep_checkpoint_frames: list[FrameObservation] = [] - # Keep synchronised endpoint observations acquired while the motor is - # held at the sweep start. If these frames are discarded and the - # target is commanded immediately, a fast motor can leave the endpoint - # before the next camera/state pair arrives. - self.sweep_start_frames: list[FrameObservation] = [] - # A completed direction already supplies fresh, synchronised frames at - # the physical endpoint where the immediately reversed direction - # starts. Stage those frames across the state transition instead of - # discarding them and waiting again at the unchanged pose. - self.carried_sweep_start_frames: list[FrameObservation] = [] - self.sweep_started_at = 0.0 - self.sweep_last_valid_at = 0.0 - self.sweep_last_valid_at_by_view: dict[str, float] = {} - self.sweep_endpoint_since: float | None = None - self.sweep_detection_total_frames = 0 - self.sweep_detection_valid_frames = 0 - self.sweep_detection_total_by_view: dict[str, int] = {} - self.sweep_detection_valid_by_view: dict[str, int] = {} - self.precheck_speed_metrics: dict[ - str, dict[str, dict[str, float | int]] - ] = {} - self.formal_speed_scales: dict[str, float] = {} - self.motion_progress_reference_error_u8 = float("inf") - self.motion_last_progress_at = 0.0 - self.motion_stall_details: dict[str, Any] = {} - self.retry_sweep_spec: SweepSpec | None = None - self.retry_resume_index: int | None = None - self.retry_sweep_items: list[SweepItem] = [] - self.retry_joint_names: set[str] = set() - # Some fitted quantities are observed by data collected during other - # physical tasks. In particular, right-hand thumb yaw zero comes from - # the two top-view side channels captured during CMC pitch and roll. - self.retry_source_failure_task_key: str | None = None - self.retry_source_task_keys: tuple[str, ...] = () - self.retry_cycle_override: set[int] = set() - self.retry_preserve_pnp_continuity = False - self.active_sweep_is_fit_retry = False - self.fit_failure: dict[str, Any] = {} - self.fit_failure_history_by_task: dict[ - str, list[list[dict[str, Any]]] - ] = {} - self.cross_view_roll_diagnostic_result: dict[str, Any] = {} - self.sweep_attempts: dict[str | int, int] = { - _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs - } - self.sweep_retry_counts: dict[tuple[str | int, int, str], int] = {} - self.motion_retry_counts: dict[str, int] = {} - self.validation_retry_counts: dict[tuple[str | int, int], int] = {} - self.records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.record_joints - } - self.baseline_records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.record_joints - } - self.command_records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.record_joints - } - self.palm_axis_records_by_source: dict[ - str, list[dict[str, Any]] - ] = { - observer.source_name: [] - for observer in self.profile.palm_axis_observers - } - - self.measured_fits: dict[str, JointCurveFit] = {} - self.axis_measurements: list[JointAxisMeasurement] = [] - self.palm_orientation_measurements: list[ - PalmOrientationMeasurement - ] = [] - self.palm_orientation_rejections: dict[str, str] = {} - self.zero_result: ZeroSolveResult | None = None - self.validated_endpoint_zero_offsets_rad: dict[str, float] = {} - self.corrected_urdf_path: Path | None = None - self.fit_quality_passed = False - - self.validation_items: list[ValidationItem] = [] - self.validation_index = 0 - self.active_validation: ValidationItem | None = None - self.active_validation_direction: str | None = None - self.validation_frames_buffer: list[FrameObservation] = [] - self.validation_errors_rad: list[float] = [] - self.validation_stage_started_at = 0.0 - self.combination_validation_items: list[CombinationValidationItem] = [] - self.combination_validation_index = 0 - self.active_combination_validation: CombinationValidationItem | None = None - self.combination_validation_frames_buffer: dict[ - str, list[FrameObservation] - ] = {view: [] for view in ("front", "side", "top")} - self.combination_validation_completed = False - self.combination_tag_mounts: dict[str, np.ndarray] = {} - self.combination_tag_observation_counts: dict[str, int] = {} - self.combination_tag_validation_counts: dict[str, int] = {} - self.combination_position_errors_m: list[float] = [] - self.combination_orientation_errors_rad: list[float] = [] - self.completed_payload: dict[str, Any] | None = None - - self.command_publisher = self.create_publisher( - JointState, self.command_topic, 1 - ) - self.setting_publisher = self.create_publisher( - String, self.setting_topic, 10 - ) - self.status_publisher = self.create_publisher(String, "~/status", 10) - self.status_text_publisher = self.create_publisher( - String, "~/status_text", 10 - ) - self.create_subscription( - JointState, self.state_topic, self._state_callback, 10 - ) - self.create_subscription( - String, self.info_topic, self._info_callback, 10 - ) - for name, runtime in self.views.items(): - del runtime - self.create_subscription( - CameraInfo, - self.camera_info_topics[name], - lambda message, view=name: self._camera_info_callback( - view, message - ), - qos_profile_sensor_data, - ) - self.create_subscription( - AprilTagDetectionArray, - self.detections_topics[name], - lambda message, view=name: self._detections_callback( - view, message - ), - qos_profile_sensor_data, - ) - - self.create_service(Trigger, "~/start", self._start_callback) - self.create_service(Trigger, "~/pause", self._pause_callback) - self.create_service(Trigger, "~/resume", self._resume_callback) - self.create_service(Trigger, "~/abort", self._abort_callback) - self.last_status_publish = 0.0 - self.timer = self.create_timer(0.05, self._timer_callback) - self.get_logger().info( - f"Three-camera calibration session: {self.session_dir}" - ) - - def _declare_parameters(self) -> None: - self.declare_parameter("model", "G20") - self.declare_parameter("hand_type", "left") - self.declare_parameter("tag_layout", "legacy_11") - self.declare_parameter("serial_number", "UNSET") - self.declare_parameter("session_dir", "calibration_output/session") - self.declare_parameter("resume_raw_samples_path", "") - self.declare_parameter("recalibration_scope", "full") - self.declare_parameter("camera_extrinsics_file", "") - self.declare_parameter("source_urdf_path", "") - self.declare_parameter("source_urdf_expected_sha256", "") - self.declare_parameter("corrected_urdf_output_dir", "") - self.declare_parameter("commands_enabled", True) - self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") - self.declare_parameter("state_topic", "/g20/cb_left_hand_state") - self.declare_parameter("info_topic", "/g20/cb_left_hand_info") - self.declare_parameter( - "setting_topic", "/g20/cb_hand_setting_cmd" - ) - for view in ("front", "side", "top"): - self.declare_parameter(f"{view}_camera_serial", "") - self.declare_parameter( - f"{view}_camera_info_topic", - f"/g20_calibration/{view}/camera/camera_info", - ) - self.declare_parameter( - f"{view}_detections_topic", - f"/g20_calibration/{view}/apriltag/detections", - ) - self.declare_parameter("tag_size_m", 0.016) - self.declare_parameter( - "tag_size_override_ids", Parameter.Type.INTEGER_ARRAY - ) - self.declare_parameter( - "tag_size_overrides_m", Parameter.Type.DOUBLE_ARRAY - ) - self.declare_parameter( - "baseline_command_u8", list(THREE_CAMERA_BASELINE_COMMAND) - ) - self.declare_parameter("normal_calibration_speed", 15) - self.declare_parameter("index_roll_calibration_speed", 5) - self.declare_parameter("index_flex_calibration_speed", 10) - self.declare_parameter("adaptive_formal_speed_enabled", True) - self.declare_parameter("adaptive_formal_speed_max_scale", 1.5) - self.declare_parameter("adaptive_formal_speed_minimum_bins", 64) - self.declare_parameter("adaptive_formal_speed_maximum_bin_gap", 8) - self.declare_parameter("speed_setting_settle_seconds", 0.25) - self.declare_parameter("parallel_pose_transitions", True) - self.declare_parameter("repetitions", 3) - self.declare_parameter("g20_right_19_repetitions", 4) - self.declare_parameter("preflight_frames", 60) - self.declare_parameter("minimum_detection_rate", 0.95) - self.declare_parameter("minimum_detection_hz", 15.0) - self.declare_parameter("minimum_feedback_hz", 25.0) - self.declare_parameter("maximum_hamming", 0) - self.declare_parameter("minimum_decision_margin", 30.0) - self.declare_parameter("minimum_edge_pixels", 30.0) - self.declare_parameter("pnp_maximum_reprojection_error_px", 1.5) - self.declare_parameter("pnp_reprojection_tie_px", 1.5) - self.declare_parameter("pnp_maximum_pose_jump_deg", 35.0) - self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) - self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) - self.declare_parameter("pnp_tracker_reset_seconds", 5.0) - 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", 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( - "thumb_ip_pnp_maximum_coupling_residual_deg", 7.5 - ) - self.declare_parameter("top_pnp_invalid_reset_seconds", 1.0) - self.declare_parameter("maximum_state_image_skew_ms", 50.0) - self.declare_parameter("axis_maximum_plane_rms_m", 0.003) - self.declare_parameter("passive_axis_maximum_plane_rms_m", 0.004) - self.declare_parameter("axis_maximum_radial_rms_m", 0.003) - self.declare_parameter("axis_maximum_pose_line_rms_m", 0.001) - self.declare_parameter("axis_maximum_rotation_circle_difference_deg", 1.0) - self.declare_parameter( - "active_maximum_rotation_orthogonal_rms_deg", 2.5 - ) - self.declare_parameter( - "passive_maximum_rotation_orthogonal_rms_deg", 7.5 - ) - self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) - self.declare_parameter( - "thumb_yaw_maximum_zero_cycle_difference_deg", 0.5 - ) - self.declare_parameter( - "thumb_yaw_maximum_confidence_half_width_deg", 0.75 - ) - self.declare_parameter( - "thumb_yaw_cross_session_diagnostic_deg", 0.75 - ) - self.declare_parameter("zero_maximum_axis_cone_mismatch_deg", 5.0) - self.declare_parameter( - "zero_maximum_observability_condition_number", 1.0e10 - ) - self.declare_parameter("zero_maximum_offset_deg", 20.0) - self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) - self.declare_parameter("mechanical_endpoint_maximum_offset_deg", 5.0) - self.declare_parameter("endpoint_tolerance_u8", 2.0) - self.declare_parameter( - "synchronised_endpoint_tolerance_margin_u8", 1.0 - ) - self.declare_parameter( - "steady_checkpoint_command_feedback_tolerance_u8", 8.0 - ) - self.declare_parameter( - "steady_checkpoint_maximum_feedback_range_u8", 2.0 - ) - self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 4.0) - self.declare_parameter( - "right_thumb_yaw_255_endpoint_tolerance_u8", 5.0 - ) - self.declare_parameter("pinky_pip_zero_endpoint_tolerance_u8", 5.0) - self.declare_parameter("endpoint_hold_seconds", 0.5) - self.declare_parameter("baseline_hold_seconds", 0.5) - self.declare_parameter("minimum_baseline_hold_frames", 10) - 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", 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) - self.declare_parameter("minimum_sweep_frames", 40) - 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", 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", [1.0]) - self.declare_parameter( - "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) - self.declare_parameter("trajectory_minimum_radius_m", 0.003) - self.declare_parameter("trajectory_minimum_arc_deg", 15.0) - self.declare_parameter("image_trajectory_maximum_radial_rms_px", 2.0) - self.declare_parameter("image_trajectory_maximum_radial_p95_px", 3.5) - self.declare_parameter("image_trajectory_minimum_radius_px", 20.0) - self.declare_parameter( - "trajectory_maximum_cycle_travel_difference_deg", 3.0 - ) - self.declare_parameter( - "passive_maximum_cycle_travel_difference_deg", 10.0 - ) - self.declare_parameter("maximum_monotonic_correction_deg", 2.0) - self.declare_parameter("maximum_hysteresis_deg", 5.0) - self.declare_parameter("baseline_maximum_hysteresis_deg", 0.5) - self.declare_parameter( - "directional_zero_maximum_branch_gap_deg", 1.5 - ) - self.declare_parameter( - "directional_zero_maximum_branch_gap_range_deg", 0.3 - ) - self.declare_parameter( - "cross_view_roll_maximum_branch_gap_difference_deg", 0.3 - ) - self.declare_parameter( - "cross_view_roll_maximum_shape_rms_deg", 1.25 - ) - self.declare_parameter( - "cross_view_roll_maximum_projection_scale_ratio", 1.5 - ) - self.declare_parameter( - "cross_view_roll_maximum_axis_difference_deg", 15.0 - ) - self.declare_parameter("passive_maximum_monotonic_correction_deg", 3.0) - self.declare_parameter("passive_maximum_hysteresis_deg", 7.5) - self.declare_parameter("command_maximum_direction_gap_deg", 2.0) - self.declare_parameter("validation_enabled", False) - self.declare_parameter("combination_validation_enabled", True) - self.declare_parameter("combination_validation_frames", 10) - self.declare_parameter("combination_maximum_position_p95_m", 0.003) - self.declare_parameter("combination_maximum_orientation_p95_deg", 2.0) - self.declare_parameter("validation_command_count", 3) - self.declare_parameter("validation_frames", 10) - self.declare_parameter("validation_seed", 20260804) - self.declare_parameter("validation_timeout_seconds", 20.0) - self.declare_parameter("maximum_validation_mae_deg", 1.0) - self.declare_parameter("maximum_validation_p95_deg", 2.0) - self.declare_parameter("maximum_validation_error_deg", 1.5) - self.declare_parameter("zero_maximum_confidence_half_width_deg", 0.5) - - def _load_parameters(self) -> None: - def value(name: str) -> Any: - return self.get_parameter(name).value - - def optional_array(name: str) -> Sequence[Any]: - # An empty YAML sequence has no element type. With a typed ROS 2 - # declaration it can therefore remain NOT_SET and ``.value`` - # raises instead of returning an empty list. These overrides are - # optional, so preserve the default-size semantics in that case. - try: - result = value(name) - except ParameterUninitializedException: - return () - return () if result is None else result - - self.model = str(value("model")).strip().upper() - self.hand_type = str(value("hand_type")).lower() - self.tag_layout = str(value("tag_layout")).lower() - product_contract = get_product_calibration_contract( - self.model, self.hand_type, self.tag_layout - ) - 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") - self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() - 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() - ) - self.recalibration_scope = str( - value("recalibration_scope") - ).strip().lower() - self.recalibration_task_keys = recalibration_task_keys( - self.profile, self.recalibration_scope - ) - self.standalone_thumb_calibration = bool( - self.recalibration_scope == "thumb" - and self.resume_raw_samples_path is None - ) - if ( - self.recalibration_scope == "fingers" - and self.resume_raw_samples_path is None - ): - raise ValueError( - "finger recalibration requires resume_raw_samples_path from " - "a passed thumb or complete session" - ) - self.partial_scope_fixed_zero_offsets_rad: dict[str, float] = {} - if ( - self.recalibration_scope != "full" - and self.resume_raw_samples_path is not None - ): - assert self.resume_raw_samples_path is not None - base_payload_path = self.resume_raw_samples_path.parent / ( - f"g20_right_{_safe_name(self.serial_number)}_calibration.json" - ) - try: - base_payload = json.loads( - base_payload_path.read_text(encoding="utf-8") - ) - except (OSError, json.JSONDecodeError) as error: - raise ValueError( - "partial recalibration base calibration JSON is invalid" - ) from error - if not isinstance(base_payload, Mapping): - raise ValueError( - "partial recalibration base calibration JSON must be an object" - ) - self.partial_scope_fixed_zero_offsets_rad = ( - _partial_scope_frozen_zero_offsets( - self.profile, - self.recalibration_scope, - base_payload, - ) - ) - self.resumed_task_keys: tuple[str, ...] = () - self.resume_source_session = "" - self.resume_checkpoint_pending = False - self.resume_position_policy = "not_requested" - self.resume_position_changed_views: tuple[str, ...] = () - self.resume_position_unverifiable_views: tuple[str, ...] = () - self.resume_position_drift_by_view_px: dict[str, float] = {} - self.camera_extrinsics_file = Path( - str(value("camera_extrinsics_file")) - ).expanduser().resolve() - self.source_urdf_path = Path( - str(value("source_urdf_path")) - ).expanduser().resolve() - self.source_urdf_expected_sha256 = str( - value("source_urdf_expected_sha256") - ).strip().lower() - output_value = str(value("corrected_urdf_output_dir")) - self.corrected_urdf_output_dir = ( - Path(output_value).expanduser().resolve() - if output_value - else self.source_urdf_path.parent - ) - self.camera_serials = { - view: str(value(f"{view}_camera_serial")) - for view in ("front", "side", "top") - } - self.extrinsics: ThreeCameraExtrinsics | None = None - self.extrinsics_error = "" - try: - self.extrinsics = load_three_camera_extrinsics( - self.camera_extrinsics_file - ) - except Exception as error: - self.extrinsics_error = str(error) - self.commands_enabled = bool(value("commands_enabled")) - self.command_topic = str(value("command_topic")) - self.state_topic = str(value("state_topic")) - self.info_topic = str(value("info_topic")) - if self.hand_type == "right": - if self.command_topic == "/g20/cb_left_hand_control_cmd": - self.command_topic = "/g20/cb_right_hand_control_cmd" - if self.state_topic == "/g20/cb_left_hand_state": - self.state_topic = "/g20/cb_right_hand_state" - if self.info_topic == "/g20/cb_left_hand_info": - self.info_topic = "/g20/cb_right_hand_info" - self.setting_topic = str(value("setting_topic")) - self.camera_info_topics = { - view: str(value(f"{view}_camera_info_topic")) - for view in ("front", "side", "top") - } - self.detections_topics = { - view: str(value(f"{view}_detections_topic")) - for view in ("front", "side", "top") - } - self.tag_size_m = float(value("tag_size_m")) - override_ids = tuple( - int(item) for item in optional_array("tag_size_override_ids") - ) - override_sizes = tuple( - float(item) for item in optional_array("tag_size_overrides_m") - ) - if len(override_ids) != len(override_sizes): - raise ValueError( - "tag_size_override_ids and tag_size_overrides_m must have " - "the same length" - ) - if len(set(override_ids)) != len(override_ids): - raise ValueError("tag_size_override_ids must be unique") - configured_tag_ids = { - int(tag_id) - for tags in self.profile.view_tags.values() - for tag_id in tags.values() - } - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and any(tag_id not in configured_tag_ids for tag_id in override_ids) - ): - raise ValueError("Tag-size override contains an unconfigured Tag ID") - if ( - not math.isfinite(self.tag_size_m) - or self.tag_size_m <= 0.0 - or any( - not math.isfinite(size) or size <= 0.0 - for size in override_sizes - ) - ): - raise ValueError("Tag sizes must be finite and positive") - self.tag_sizes_m_by_id = { - tag_id: self.tag_size_m for tag_id in configured_tag_ids - } - self.tag_sizes_m_by_id.update( - (tag_id, size) - for tag_id, size in zip(override_ids, override_sizes) - if tag_id in configured_tag_ids - ) - self.baseline_command = tuple( - int(item) for item in value("baseline_command_u8") - ) - self.return_command_u8 = tuple(self.baseline_command) - self.normal_calibration_speed = int(value("normal_calibration_speed")) - self.index_roll_calibration_speed = int( - value("index_roll_calibration_speed") - ) - self.index_flex_calibration_speed = int( - value("index_flex_calibration_speed") - ) - self.adaptive_formal_speed_enabled = bool( - value("adaptive_formal_speed_enabled") - ) - self.adaptive_formal_speed_max_scale = float( - value("adaptive_formal_speed_max_scale") - ) - self.adaptive_formal_speed_minimum_bins = int( - value("adaptive_formal_speed_minimum_bins") - ) - self.adaptive_formal_speed_maximum_bin_gap = int( - value("adaptive_formal_speed_maximum_bin_gap") - ) - self.speed_setting_settle_seconds = float( - value("speed_setting_settle_seconds") - ) - self.repetitions = int(value("repetitions")) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - self.repetitions = int(value("g20_right_19_repetitions")) - self.preflight_frames = int(value("preflight_frames")) - self.minimum_detection_rate = float(value("minimum_detection_rate")) - self.minimum_detection_hz = float(value("minimum_detection_hz")) - self.minimum_feedback_hz = float(value("minimum_feedback_hz")) - self.maximum_hamming = int(value("maximum_hamming")) - self.minimum_decision_margin = float(value("minimum_decision_margin")) - self.minimum_edge_pixels = float(value("minimum_edge_pixels")) - self.pnp_maximum_reprojection_error_px = float( - value("pnp_maximum_reprojection_error_px") - ) - self.pnp_reprojection_tie_px = float(value("pnp_reprojection_tie_px")) - self.pnp_maximum_pose_jump_rad = math.radians( - float(value("pnp_maximum_pose_jump_deg")) - ) - self.pnp_maximum_translation_jump_m = float( - value("pnp_maximum_translation_jump_m") - ) - self.pnp_maximum_tag_tilt_rad = math.radians( - float(value("pnp_maximum_tag_tilt_deg")) - ) - self.pnp_tracker_reset_seconds = float(value("pnp_tracker_reset_seconds")) - self.pnp_group_initialization_frames = int( - value("pnp_group_initialization_frames") - ) - self.pnp_group_normal_alignment_scale_rad = math.radians( - float(value("pnp_group_normal_alignment_scale_deg")) - ) - self.pnp_group_maximum_normal_alignment_rad = math.radians( - float(value("pnp_group_maximum_normal_alignment_deg")) - ) - self.fixed_base_maximum_corner_drift_px = float( - value("fixed_base_maximum_corner_drift_px") - ) - self.fixed_base_movement_confirmation_frames = int( - value("fixed_base_movement_confirmation_frames") - ) - self.thumb_ip_pnp_coupling_multiplier = float( - value("thumb_ip_pnp_coupling_multiplier") - ) - self.thumb_ip_pnp_coupling_scale_rad = math.radians( - float(value("thumb_ip_pnp_coupling_scale_deg")) - ) - self.thumb_ip_pnp_maximum_coupling_residual_rad = math.radians( - float(value("thumb_ip_pnp_maximum_coupling_residual_deg")) - ) - self.top_pnp_invalid_reset_seconds = float( - value("top_pnp_invalid_reset_seconds") - ) - self.maximum_state_image_skew_ns = int( - float(value("maximum_state_image_skew_ms")) * 1_000_000.0 - ) - self.axis_maximum_plane_rms_m = float( - value("axis_maximum_plane_rms_m") - ) - self.passive_axis_maximum_plane_rms_m = float( - value("passive_axis_maximum_plane_rms_m") - ) - self.axis_maximum_radial_rms_m = float( - value("axis_maximum_radial_rms_m") - ) - self.axis_maximum_pose_line_rms_m = float( - value("axis_maximum_pose_line_rms_m") - ) - self.axis_maximum_rotation_circle_difference_rad = math.radians( - float(value("axis_maximum_rotation_circle_difference_deg")) - ) - self.active_maximum_rotation_orthogonal_rms_rad = math.radians( - float(value("active_maximum_rotation_orthogonal_rms_deg")) - ) - self.passive_maximum_rotation_orthogonal_rms_rad = math.radians( - float(value("passive_maximum_rotation_orthogonal_rms_deg")) - ) - self.zero_maximum_axis_cycle_difference_rad = math.radians( - float(value("zero_maximum_axis_cycle_difference_deg")) - ) - self.thumb_yaw_maximum_zero_cycle_difference_rad = math.radians( - float(value("thumb_yaw_maximum_zero_cycle_difference_deg")) - ) - self.thumb_yaw_maximum_confidence_half_width_rad = math.radians( - float(value("thumb_yaw_maximum_confidence_half_width_deg")) - ) - self.thumb_yaw_cross_session_diagnostic_rad = math.radians( - float(value("thumb_yaw_cross_session_diagnostic_deg")) - ) - self.zero_maximum_axis_cone_mismatch_rad = math.radians( - float(value("zero_maximum_axis_cone_mismatch_deg")) - ) - self.zero_maximum_observability_condition_number = float( - value("zero_maximum_observability_condition_number") - ) - self.zero_maximum_offset_rad = math.radians( - float(value("zero_maximum_offset_deg")) - ) - self.zero_finger_maximum_offset_rad = math.radians( - float(value("zero_finger_maximum_offset_deg")) - ) - self.mechanical_endpoint_maximum_offset_rad = math.radians( - float(value("mechanical_endpoint_maximum_offset_deg")) - ) - self.zero_joint_maximum_offsets_rad = {} - self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) - self.synchronised_endpoint_tolerance_margin_u8 = float( - value("synchronised_endpoint_tolerance_margin_u8") - ) - self.steady_checkpoint_command_feedback_tolerance_u8 = float( - value("steady_checkpoint_command_feedback_tolerance_u8") - ) - self.steady_checkpoint_maximum_feedback_range_u8 = float( - value("steady_checkpoint_maximum_feedback_range_u8") - ) - self.thumb_yaw_zero_endpoint_tolerance_u8 = float( - value("thumb_yaw_zero_endpoint_tolerance_u8") - ) - self.right_thumb_yaw_255_endpoint_tolerance_u8 = float( - value("right_thumb_yaw_255_endpoint_tolerance_u8") - ) - self.pinky_pip_zero_endpoint_tolerance_u8 = float( - value("pinky_pip_zero_endpoint_tolerance_u8") - ) - self.endpoint_hold_seconds = float(value("endpoint_hold_seconds")) - self.baseline_hold_seconds = float(value("baseline_hold_seconds")) - self.minimum_baseline_hold_frames = int( - value("minimum_baseline_hold_frames") - ) - self.task_precheck_hold_seconds = float( - value("task_precheck_hold_seconds") - ) - self.position_timeout_seconds = float(value("position_timeout_seconds")) - self.parallel_pose_transitions = bool( - value("parallel_pose_transitions") - ) - self.sweep_timeout_seconds = float(value("sweep_timeout_seconds")) - self.motor_stall_timeout_seconds = float( - value("motor_stall_timeout_seconds") - ) - self.motor_stall_startup_grace_seconds = float( - value("motor_stall_startup_grace_seconds") - ) - self.motor_stall_minimum_progress_u8 = float( - value("motor_stall_minimum_progress_u8") - ) - self.invalid_timeout_seconds = float(value("invalid_timeout_seconds")) - self.minimum_sweep_frames = int(value("minimum_sweep_frames")) - self.minimum_state_span_u8 = float(value("minimum_state_span_u8")) - self.minimum_sweep_bins = int(value("minimum_sweep_bins")) - self.maximum_bin_gap = int(value("maximum_bin_gap")) - self.automatic_sweep_retry_limit = int( - value("automatic_sweep_retry_limit") - ) - self.automatic_fit_retry_limit = int( - value("automatic_fit_retry_limit") - ) - self.cross_view_roll_diagnostic_finger = str( - value("cross_view_roll_diagnostic_finger") - ).strip().lower() - self.automatic_motion_retry_limit = int( - value("automatic_motion_retry_limit") - ) - self.provisional_warning_ratio = float( - value("provisional_warning_ratio") - ) - self.retry_minimum_speed = int(value("retry_minimum_speed")) - self.retry_speed_scales = tuple( - float(item) for item in value("retry_speed_scales") - ) - self.retry_endpoint_hold_seconds = tuple( - float(item) for item in value("retry_endpoint_hold_seconds") - ) - self.trajectory_maximum_plane_rms_m = float( - value("trajectory_maximum_plane_rms_m") - ) - self.trajectory_maximum_radial_rms_m = float( - value("trajectory_maximum_radial_rms_m") - ) - self.trajectory_minimum_radius_m = float( - value("trajectory_minimum_radius_m") - ) - self.trajectory_minimum_arc_rad = math.radians( - float(value("trajectory_minimum_arc_deg")) - ) - self.image_trajectory_maximum_radial_rms_px = float( - value("image_trajectory_maximum_radial_rms_px") - ) - self.image_trajectory_maximum_radial_p95_px = float( - value("image_trajectory_maximum_radial_p95_px") - ) - self.image_trajectory_minimum_radius_px = float( - value("image_trajectory_minimum_radius_px") - ) - self.trajectory_maximum_cycle_travel_difference_rad = math.radians( - float(value("trajectory_maximum_cycle_travel_difference_deg")) - ) - self.passive_maximum_cycle_travel_difference_rad = math.radians( - float(value("passive_maximum_cycle_travel_difference_deg")) - ) - self.maximum_monotonic_correction_rad = math.radians( - float(value("maximum_monotonic_correction_deg")) - ) - self.maximum_hysteresis_rad = math.radians( - float(value("maximum_hysteresis_deg")) - ) - self.baseline_maximum_hysteresis_rad = math.radians( - float(value("baseline_maximum_hysteresis_deg")) - ) - self.directional_zero_maximum_branch_gap_rad = math.radians( - float(value("directional_zero_maximum_branch_gap_deg")) - ) - self.directional_zero_maximum_branch_gap_range_rad = math.radians( - float(value("directional_zero_maximum_branch_gap_range_deg")) - ) - self.cross_view_roll_maximum_branch_gap_difference_rad = math.radians( - float(value("cross_view_roll_maximum_branch_gap_difference_deg")) - ) - self.cross_view_roll_maximum_shape_rms_rad = math.radians( - float(value("cross_view_roll_maximum_shape_rms_deg")) - ) - self.cross_view_roll_maximum_projection_scale_ratio = float( - value("cross_view_roll_maximum_projection_scale_ratio") - ) - self.cross_view_roll_maximum_axis_difference_rad = math.radians( - float(value("cross_view_roll_maximum_axis_difference_deg")) - ) - self.passive_maximum_monotonic_correction_rad = math.radians( - float(value("passive_maximum_monotonic_correction_deg")) - ) - self.passive_maximum_hysteresis_rad = math.radians( - float(value("passive_maximum_hysteresis_deg")) - ) - self.command_maximum_direction_gap_rad = math.radians( - float(value("command_maximum_direction_gap_deg")) - ) - self.validation_enabled = bool(value("validation_enabled")) - self.combination_validation_enabled = bool( - value("combination_validation_enabled") - ) - if self.standalone_thumb_calibration: - # A standalone thumb artifact has no four-finger curves or - # Cartesian combination model to validate. - self.combination_validation_enabled = False - self.combination_validation_frames = int( - value("combination_validation_frames") - ) - self.combination_maximum_position_p95_m = float( - value("combination_maximum_position_p95_m") - ) - self.combination_maximum_orientation_p95_rad = math.radians( - float(value("combination_maximum_orientation_p95_deg")) - ) - self.validation_command_count = int(value("validation_command_count")) - self.validation_frames = int(value("validation_frames")) - self.validation_seed = int(value("validation_seed")) - self.validation_timeout_seconds = float( - value("validation_timeout_seconds") - ) - self.maximum_validation_mae_rad = math.radians( - float(value("maximum_validation_mae_deg")) - ) - self.maximum_validation_p95_rad = math.radians( - float(value("maximum_validation_p95_deg")) - ) - self.maximum_validation_error_rad = math.radians( - float(value("maximum_validation_error_deg")) - ) - self.zero_maximum_confidence_half_width_rad = math.radians( - float(value("zero_maximum_confidence_half_width_deg")) - ) - if len(self.baseline_command) != self.profile.command_count: - raise ValueError( - "baseline_command_u8 length does not match the registered " - f"product command schema ({self.profile.command_count})" - ) - if any(value < 0 or value > 255 for value in self.baseline_command): - raise ValueError("baseline_command_u8 values must be in [0, 255]") - if self.baseline_command != tuple(self.profile.baseline_command): - raise ValueError( - "baseline_command_u8 differs from the registered product " - "calibration baseline" - ) - if any(not serial for serial in self.camera_serials.values()): - raise ValueError("front/side/top camera serial parameters are required") - if not self.source_urdf_path.is_file(): - raise ValueError( - f"source_urdf_path does not exist: {self.source_urdf_path}" - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - if re.fullmatch( - r"[0-9a-f]{64}", self.source_urdf_expected_sha256 - ) is None: - raise ValueError( - "g20_right_19 requires source_urdf_expected_sha256 from " - "the CAD/hardware owner; unconfirmed source CAD cannot " - "produce a formal corrected URDF" - ) - actual_source_hash = _file_sha256(self.source_urdf_path) - if actual_source_hash != self.source_urdf_expected_sha256: - raise ValueError( - "source_urdf_path SHA-256 does not match the confirmed " - "G20 CAD hash" - ) - source_model = UrdfKinematicModel(self.source_urdf_path) - thumb_ip_model = source_model.joints.get("thumb_ip") - if ( - thumb_ip_model is None - or thumb_ip_model.mimic_joint != "thumb_mcp" - or abs(thumb_ip_model.mimic_offset) > 1.0e-9 - ): - raise ValueError( - "source URDF thumb_ip mimic structure is invalid" - ) - source_name = self.source_urdf_path.name.lower() - opposite = "right" if self.hand_type == "left" else "left" - if f"g20_{opposite}" in source_name: - raise ValueError( - "source_urdf_path hand side does not match hand_type" - ) - if ( - "zero_calibrated" in self.source_urdf_path.stem.lower() - or re.search( - r"calibrated_20\d{6}", - self.source_urdf_path.stem.lower(), - ) - ): - raise ValueError( - "source_urdf_path must be the original CAD URDF" - ) - if not 0 <= self.normal_calibration_speed <= 255: - raise ValueError("normal_calibration_speed must be in [0, 255]") - if not 0 <= self.index_roll_calibration_speed <= 255: - raise ValueError("index_roll_calibration_speed must be in [0, 255]") - if not 0 <= self.index_flex_calibration_speed <= 255: - raise ValueError("index_flex_calibration_speed must be in [0, 255]") - if not 1.0 <= self.adaptive_formal_speed_max_scale <= 2.0: - raise ValueError( - "adaptive_formal_speed_max_scale must be in [1, 2]" - ) - if ( - self.adaptive_formal_speed_minimum_bins - < self.minimum_sweep_bins - ): - raise ValueError( - "adaptive_formal_speed_minimum_bins cannot be below the " - "formal minimum_sweep_bins" - ) - if not ( - 1 - <= self.adaptive_formal_speed_maximum_bin_gap - <= self.maximum_bin_gap - ): - raise ValueError( - "adaptive_formal_speed_maximum_bin_gap must be in " - "[1, maximum_bin_gap]" - ) - if self.speed_setting_settle_seconds < 0.0: - raise ValueError("speed_setting_settle_seconds must be non-negative") - if not 0.0 <= self.endpoint_tolerance_u8 <= 10.0: - raise ValueError("endpoint_tolerance_u8 must be in [0, 10]") - if not 0.0 <= self.synchronised_endpoint_tolerance_margin_u8 <= 4.0: - raise ValueError( - "synchronised_endpoint_tolerance_margin_u8 must be in [0, 4]" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.steady_checkpoint_command_feedback_tolerance_u8 - <= 16.0 - ): - raise ValueError( - "steady_checkpoint_command_feedback_tolerance_u8 must be " - "between endpoint_tolerance_u8 and 16" - ) - if not 0.0 < self.steady_checkpoint_maximum_feedback_range_u8 <= 4.0: - raise ValueError( - "steady_checkpoint_maximum_feedback_range_u8 must be in (0, 4]" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.thumb_yaw_zero_endpoint_tolerance_u8 - <= 10.0 - ): - raise ValueError( - "thumb_yaw_zero_endpoint_tolerance_u8 must be between the " - "default endpoint tolerance and 10" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.right_thumb_yaw_255_endpoint_tolerance_u8 - <= 10.0 - ): - raise ValueError( - "right_thumb_yaw_255_endpoint_tolerance_u8 must be between " - "the default endpoint tolerance and 10" - ) - if self.top_pnp_invalid_reset_seconds <= 0.0: - raise ValueError("top_pnp_invalid_reset_seconds must be positive") - if not 3 <= self.pnp_group_initialization_frames <= 30: - raise ValueError( - "pnp_group_initialization_frames must be in [3, 30]" - ) - if not 0.5 <= self.fixed_base_maximum_corner_drift_px <= 10.0: - raise ValueError( - "fixed_base_maximum_corner_drift_px must be in [0.5, 10]" - ) - if not 1 <= self.fixed_base_movement_confirmation_frames <= 30: - raise ValueError( - "fixed_base_movement_confirmation_frames must be in [1, 30]" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.pinky_pip_zero_endpoint_tolerance_u8 - <= 8.0 - ): - raise ValueError( - "pinky_pip_zero_endpoint_tolerance_u8 must be between " - "endpoint_tolerance_u8 and 8" - ) - if self.motor_stall_timeout_seconds <= 0.0: - raise ValueError("motor_stall_timeout_seconds must be positive") - if not 0.0 <= self.motor_stall_startup_grace_seconds <= 5.0: - raise ValueError("motor_stall_startup_grace_seconds must be in [0, 5]") - if not 0.0 < self.motor_stall_minimum_progress_u8 <= 10.0: - raise ValueError( - "motor_stall_minimum_progress_u8 must be in (0, 10]" - ) - if not all( - value > 0.0 - for value in ( - self.image_trajectory_maximum_radial_rms_px, - self.image_trajectory_maximum_radial_p95_px, - self.image_trajectory_minimum_radius_px, - self.pnp_group_normal_alignment_scale_rad, - self.pnp_group_maximum_normal_alignment_rad, - self.thumb_ip_pnp_coupling_multiplier, - self.thumb_ip_pnp_coupling_scale_rad, - self.thumb_ip_pnp_maximum_coupling_residual_rad, - self.axis_maximum_plane_rms_m, - self.passive_axis_maximum_plane_rms_m, - self.axis_maximum_pose_line_rms_m, - self.zero_maximum_axis_cycle_difference_rad, - self.thumb_yaw_maximum_zero_cycle_difference_rad, - self.thumb_yaw_maximum_confidence_half_width_rad, - self.thumb_yaw_cross_session_diagnostic_rad, - self.zero_maximum_axis_cone_mismatch_rad, - self.zero_maximum_offset_rad, - self.zero_finger_maximum_offset_rad, - self.mechanical_endpoint_maximum_offset_rad, - self.maximum_validation_error_rad, - self.zero_maximum_confidence_half_width_rad, - self.task_precheck_hold_seconds, - self.active_maximum_rotation_orthogonal_rms_rad, - self.passive_maximum_rotation_orthogonal_rms_rad, - self.trajectory_maximum_cycle_travel_difference_rad, - self.passive_maximum_cycle_travel_difference_rad, - self.baseline_maximum_hysteresis_rad, - self.directional_zero_maximum_branch_gap_rad, - self.directional_zero_maximum_branch_gap_range_rad, - self.cross_view_roll_maximum_branch_gap_difference_rad, - self.cross_view_roll_maximum_shape_rms_rad, - self.passive_maximum_monotonic_correction_rad, - self.passive_maximum_hysteresis_rad, - self.command_maximum_direction_gap_rad, - ) - ): - raise ValueError("trajectory quality thresholds must be positive") - if ( - self.thumb_yaw_maximum_zero_cycle_difference_rad - > self.zero_maximum_axis_cycle_difference_rad - ): - raise ValueError( - "thumb_yaw_maximum_zero_cycle_difference_deg must not " - "exceed zero_maximum_axis_cycle_difference_deg" - ) - if ( - self.thumb_yaw_maximum_confidence_half_width_rad - > self.zero_maximum_confidence_half_width_rad - ): - raise ValueError( - "thumb_yaw_maximum_confidence_half_width_deg must not " - "exceed zero_maximum_confidence_half_width_deg" - ) - if not 1.0 < self.cross_view_roll_maximum_projection_scale_ratio <= 2.0: - raise ValueError( - "cross-view roll projection-scale ratio must be in (1, 2]" - ) - if self.zero_finger_maximum_offset_rad > self.zero_maximum_offset_rad: - raise ValueError( - "zero_finger_maximum_offset_deg cannot exceed " - "zero_maximum_offset_deg" - ) - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and self.repetitions < 4 - ): - raise ValueError( - "g20_right_19 requires at least 4 repetitions: at least 3 " - "training cycles and one isolated holdout" - ) - if ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - and self.repetitions != 3 - ): - raise ValueError( - "legacy three-camera calibration requires exactly 3 repetitions" - ) - if self.preflight_frames < 10 or self.minimum_sweep_bins < 3: - raise ValueError("preflight_frames or minimum_sweep_bins is too small") - if self.minimum_feedback_hz <= 0.0: - raise ValueError("minimum_feedback_hz must be positive") - if self.minimum_baseline_hold_frames < 3: - raise ValueError("minimum_baseline_hold_frames must be at least 3") - if not 0 <= self.automatic_sweep_retry_limit <= 5: - raise ValueError("automatic_sweep_retry_limit must be in [0, 5]") - if not 0 <= self.automatic_fit_retry_limit <= 2: - raise ValueError("automatic_fit_retry_limit must be in [0, 2]") - if not 0 <= self.automatic_motion_retry_limit <= 5: - raise ValueError("automatic_motion_retry_limit must be in [0, 5]") - if self.cross_view_roll_diagnostic_finger not in { - "", "pinky", "ring", "middle", "index" - }: - raise ValueError( - "cross_view_roll_diagnostic_finger must be empty or one of " - "pinky/ring/middle/index" - ) - if ( - self.cross_view_roll_diagnostic_finger - and self.profile.layout_id != G20_RIGHT_19_LAYOUT - ): - raise ValueError( - "cross-view roll diagnostic requires g20_right_19" - ) - if not 1.0 <= self.provisional_warning_ratio <= 2.0: - raise ValueError("provisional_warning_ratio must be in [1, 2]") - if not 1 <= self.retry_minimum_speed <= 255: - raise ValueError("retry_minimum_speed must be in [1, 255]") - if ( - len(self.retry_speed_scales) != self.automatic_sweep_retry_limit - or any(not 0.0 < item <= 1.0 for item in self.retry_speed_scales) - ): - raise ValueError("retry_speed_scales must match the sweep retry limit") - if ( - len(self.retry_endpoint_hold_seconds) - != self.automatic_sweep_retry_limit - or any(item < self.endpoint_hold_seconds for item in self.retry_endpoint_hold_seconds) - ): - raise ValueError( - "retry endpoint holds must match retries and not shorten the hold" - ) - if not 1 <= self.validation_command_count <= 10: - raise ValueError("validation_command_count must be in [1, 10]") - if self.validation_frames < 3: - raise ValueError("validation_frames must be at least 3") - if self.combination_validation_frames < 3: - raise ValueError("combination_validation_frames must be at least 3") - if ( - self.combination_maximum_position_p95_m <= 0.0 - or self.combination_maximum_orientation_p95_rad <= 0.0 - ): - raise ValueError("combination pose validation thresholds must be positive") - - def _make_group_pose_tracker( - self, name: str, roles: Sequence[str] - ) -> SquareTagGroupPoseTracker: - role_tuple = tuple(str(role) for role in roles) - role_set = set(role_tuple) - adjacent_pairs = tuple( - pair - for pair in dict.fromkeys( - (spec.parent_role, spec.child_role) - for spec in self.profile.record_specs.values() - if spec.measured - and spec.view == name - and spec.parent_role is not None - and spec.child_role is not None - ) - if pair[0] in role_set and pair[1] in role_set - ) - # Do not impose an absolute face-normal relationship between the - # fixed palm Tag and an articulated finger Tag. That angle depends on - # the physical Tag bracket and the endpoint joint pose; it is not a - # product invariant. The old "nearly parallel" prior repeatedly - # cleared otherwise stable 8-frame initialisation windows for ID4/ID5 - # on ring MCP flexion. Multi-frame relative rigidity, reprojection - # error and the task reference retained across cycles provide the - # valid branch evidence without assuming a particular mounting plane. - normal_alignment_pairs: tuple[tuple[str, str], ...] = () - thumb_mcp_ip_roles = {"thumb_cmc", "thumb_mcp", "thumb_ip"} - thumb_mcp_ip_coupling = () - if name == "front" and thumb_mcp_ip_roles.issubset(role_set): - thumb_mcp_ip_coupling = ( - ( - "thumb_cmc", - "thumb_mcp", - "thumb_mcp", - "thumb_ip", - self.thumb_ip_pnp_coupling_multiplier, - ), - ) - return SquareTagGroupPoseTracker( - roles=role_tuple, - adjacent_pairs=adjacent_pairs, - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, - relative_rotation_scale_rad=math.radians(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=self.pnp_tracker_reset_seconds, - # Five-tag front preflight would make the exhaustive multi-frame - # search unnecessarily expensive. Four-tag side groups remain - # small enough and need the robust initializer so the fixed base - # Tag can anchor the three reference-finger IPPE branches. - initialization_frames=( - 1 - if len(role_tuple) > 4 - else self.pnp_group_initialization_frames - ), - normal_alignment_pairs=normal_alignment_pairs, - normal_alignment_scale_rad=( - self.pnp_group_normal_alignment_scale_rad - ), - maximum_normal_alignment_rad=( - self.pnp_group_maximum_normal_alignment_rad - if normal_alignment_pairs - else None - ), - # The source-URDF thumb_ip mimic ratio is used only to disambiguate - # the two planar-IPPE candidates while motor 15 drives MCP and IP - # together. If no candidate matches this weak prior, selection - # falls back to visual continuity instead of dropping the frame. - # Accepted Tag poses still fit both visual curves independently. - coupled_rotation_pairs=thumb_mcp_ip_coupling, - coupled_rotation_scale_rad=self.thumb_ip_pnp_coupling_scale_rad, - maximum_coupled_rotation_residual_rad=( - self.thumb_ip_pnp_maximum_coupling_residual_rad - if thumb_mcp_ip_coupling - else None - ), - ) - - def _make_view_runtime(self, name: str) -> ViewRuntime: - tracker = SquareTagPoseTracker( - maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, - reprojection_tie_px=self.pnp_reprojection_tie_px, - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, - maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, - reset_after_seconds=self.pnp_tracker_reset_seconds, - ) - preflight_roles = tuple(self.profile.preflight_view_roles[name]) - if getattr(self, "standalone_thumb_calibration", False): - base_role = _fixed_base_role(name) - preflight_roles = tuple( - role - for role in self.profile.view_tags[name] - if role == base_role or role.startswith("thumb_") - ) - group = self._make_group_pose_tracker( - name, - preflight_roles, - ) - return ViewRuntime( - name=name, - tag_size_m=self.tag_size_m, - preflight_frames=self.preflight_frames, - tracker=tracker, - group_tracker=group, - view_tags=self.profile.view_tags[name], - tag_sizes_m_by_role={ - role: self.tag_sizes_m_by_id[int(tag_id)] - for role, tag_id in self.profile.view_tags[name].items() - }, - preflight_roles=preflight_roles, - ) - - def _required_roles_for_view(self, view: str) -> tuple[str, ...]: - """Return only the roles required by the active task in this view.""" - runtime = self.views[view] - if getattr(self, "active_combination_validation", None) is not None: - # Combination validation consumes every reliable visible target, - # but only the fixed palm reference is a blocking requirement. - # Four adjacent fingers cannot expose all of their Tags to one - # side camera at the same instant. - return tuple(runtime.preflight_roles) - active_spec: SweepSpec | None = None - if ( - self.active_sweep is not None - and view in _sweep_views(_node_profile(self), self.active_sweep.spec) - ): - active_spec = self.active_sweep.spec - elif ( - self.active_validation is not None - and view in _sweep_views( - self.profile, self.active_validation.spec - ) - ): - active_spec = self.active_validation.spec - elif ( - self.retry_sweep_spec is not None - and view in _sweep_views(_node_profile(self), self.retry_sweep_spec) - ): - active_spec = self.retry_sweep_spec - elif ( - getattr(self, "pnp_task_spec", None) is not None - and view - in _sweep_views( - _node_profile(self), self.pnp_task_spec - ) - ): - active_spec = self.pnp_task_spec - if active_spec is None: - return tuple(runtime.preflight_roles) - required: set[str] = set() - for joint_name in _sweep_joints_for_view( - self.profile, active_spec, view - ): - joint = self.profile.record_specs[joint_name] - if joint.parent_role is not None: - required.add(joint.parent_role) - if joint.child_role is not None: - required.add(joint.child_role) - # Every task keeps its view's fixed palm Tag. Besides suppressing an - # unanchored IPPE mirror branch, this makes the thumb MCP/IP task obey - # its explicit front 0/1/2/3 visibility contract. - base_role = { - "front": "front_base", - "side": "side_base", - "top": "top_base", - }[view] - required.add(base_role) - return tuple(role for role in runtime.roles if role in required) - - def _locked_base_role_for_active_capture(self, view: str) -> str | None: - runtime = self.views[view] - if ( - getattr(runtime, "locked_base_pose", None) is None - or getattr(runtime, "locked_base_center_xy_px", None) is None - or getattr(runtime, "locked_base_quality", None) is None - ): - return None - if getattr(self, "active_combination_validation", None) is not None: - return _fixed_base_role(view) - spec: SweepSpec | None = None - if self.active_sweep is not None: - spec = self.active_sweep.spec - elif self.active_validation is not None: - spec = self.active_validation.spec - elif self.retry_sweep_spec is not None: - spec = self.retry_sweep_spec - elif getattr(self, "pnp_task_spec", None) is not None: - spec = self.pnp_task_spec - if spec is None or not _sweep_uses_locked_base_reference( - _node_profile(self), spec, view - ): - return None - return _fixed_base_role(view) - - def _live_required_roles_for_view( - self, view: str, required_roles: Sequence[str] - ) -> tuple[str, ...]: - locked = self._locked_base_role_for_active_capture(view) - # Front Tag 0 may be intentionally hidden by finger clearance. Top - # Tag 8 must remain visible so its raw corners can prove that the - # session reference has not moved, even though its PnP pose is frozen. - if str(view) == "top": - return tuple(required_roles) - return tuple(role for role in required_roles if role != locked) - - def _lock_fixed_base_references(self) -> bool: - """Freeze robust palm references captured at the confirmed baseline.""" - minimum = min(30, int(self.preflight_frames)) - pending = False - for view, runtime in self.views.items(): - if runtime.locked_base_pose is not None: - continue - observations = list(runtime.fixed_base_observations) - if len(observations) < minimum: - pending = True - continue - poses = [item[0] for item in observations] - centres = np.asarray([item[1] for item in observations], dtype=float) - qualities = [item[2] for item in observations] - translation = np.median( - np.asarray( - [pose.translation_xyz_m for pose in poses], dtype=float - ), - axis=0, - ) - quaternion = robust_rotation_summary( - [pose.quaternion_xyzw for pose in poses] - )[0] - reprojection = float( - np.percentile( - [pose.reprojection_error_px for pose in poses], 95.0 - ) - ) - runtime.locked_base_pose = SquareTagPose( - tuple(float(value) for value in quaternion), - tuple(float(value) for value in translation), - reprojection, - ) - runtime.locked_base_center_xy_px = tuple( - float(value) for value in np.median(centres, axis=0) - ) - corner_observations = list( - getattr(runtime, "fixed_base_corner_observations", ()) - ) - if len(corner_observations) >= minimum: - locked_corners = np.median( - np.asarray(corner_observations, dtype=float), axis=0 - ) - runtime.locked_base_corners_xy = tuple( - tuple(float(value) for value in point) - for point in locked_corners - ) - else: - runtime.locked_base_corners_xy = None - runtime.locked_base_corner_drift_count = 0 - runtime.latest_locked_base_corner_drift_px = 0.0 - runtime.locked_base_quality = TagQuality( - hamming=max(item.hamming for item in qualities), - decision_margin=min(item.decision_margin for item in qualities), - edge_pixels=min(item.edge_pixels for item in qualities), - reprojection_error_px=reprojection, - ) - translation_rms_m = float( - np.sqrt( - np.mean( - np.sum( - ( - np.asarray( - [pose.translation_xyz_m for pose in poses], - dtype=float, - ) - - translation - ) - ** 2, - axis=1, - ) - ) - ) - ) - rotation_p95_deg = math.degrees( - float( - np.percentile( - [ - ( - Rotation.from_quat(quaternion).inv() - * Rotation.from_quat(pose.quaternion_xyzw) - ).magnitude() - for pose in poses - ], - 95.0, - ) - ) - ) - append_jsonl( - self.raw_path, - { - "kind": "fixed_base_reference_locked", - "view": view, - "role": _fixed_base_role(view), - "tag_id": runtime.view_tags[_fixed_base_role(view)], - "sample_count": len(observations), - "translation_rms_m": round(translation_rms_m, 9), - "rotation_p95_deg": round(rotation_p95_deg, 6), - "reprojection_p95_px": round(reprojection, 6), - "corner_reference_xy": ( - None - if runtime.locked_base_corners_xy is None - else [ - list(point) - for point in runtime.locked_base_corners_xy - ] - ), - }, - ) - return not pending and all( - runtime.locked_base_pose is not None - for runtime in self.views.values() - ) - - def _group_tracker_for_roles( - self, runtime: ViewRuntime, roles: Sequence[str] - ) -> SquareTagGroupPoseTracker: - key = tuple(str(role) for role in roles) - group = runtime.group_trackers.get(key) - if group is None: - group = self._make_group_pose_tracker(runtime.name, key) - runtime.group_trackers[key] = group - return group - - @staticmethod - def _reset_view_trackers( - runtime: ViewRuntime, *, preserve_task_reference: bool = False - ) -> None: - runtime.tracker.reset() - for group in runtime.group_trackers.values(): - group.reset( - preserve_task_reference=preserve_task_reference - ) - - @staticmethod - def _reset_view_pnp_diagnostics(runtime: ViewRuntime) -> None: - """Clear PnP evidence only when a new capture scope begins.""" - runtime.latest_pnp_rejections.clear() - runtime.latest_group_pnp_reason = "" - runtime.latest_group_missing_candidate_roles = () - runtime.pnp_rejection_counts.clear() - runtime.group_pnp_rejection_counts.clear() - runtime.pnp_initialization_progress = None - runtime.latest_pnp_valid = False - runtime.last_pnp_diagnostic_signature = None - - def _record_group_pnp_candidate_event( - self, - *, - runtime: ViewRuntime, - stamp_ns: int, - required_roles: Sequence[str], - pose_roles: Sequence[str], - corners_by_role: Mapping[str, np.ndarray], - qualities: Mapping[str, TagQuality], - matched_tracking: tuple[tuple[float, ...], int] | None, - selected: Mapping[str, SquareTagPose] | None, - pnp_rejections: Mapping[str, str], - group_pnp_reason: str, - ) -> None: - """Persist bounded, replayable evidence for group candidate decisions. - - Valid FrameObservation records intentionally contain only accepted - poses. Without a separate event, a deterministic PnP dropout leaves - no corners, per-candidate tilt, reprojection error or missing role in - raw_samples.jsonl, making the next run guess at the cause. Record one - event per eight-count motor bucket and decision signature. This is - dense enough to locate a repeatable geometry boundary without writing - every 30 Hz rejected camera message. - """ - item = self.active_sweep - if ( - item is None - or self.state not in {STATE_PREPARE_SWEEP, STATE_SWEEP} - or runtime.name not in _sweep_views(_node_profile(self), item.spec) - ): - return - candidate_diagnostics = { - role: dict( - runtime.tracker.last_candidate_diagnostics_by_role.get( - role, {} - ) - ) - for role in pose_roles - if runtime.tracker.last_candidate_diagnostics_by_role.get(role) - } - oblique_group_roles = tuple( - role - for role, diagnostics in candidate_diagnostics.items() - if int(diagnostics.get("reprojection_candidate_count", 0)) > 0 - and int( - diagnostics.get("independent_tilt_candidate_count", 0) - ) - == 0 - ) - if ( - not group_pnp_reason - and not pnp_rejections - and not oblique_group_roles - ): - return - if selected is not None and not oblique_group_roles: - return - state_u8 = ( - matched_tracking[0] - if matched_tracking is not None - else self.latest_state_u8 - ) - motor_value = ( - None - if len(state_u8) != 20 - else float(state_u8[item.spec.motor_index]) - ) - motor_bucket = ( - -1 - if motor_value is None or not math.isfinite(motor_value) - else int(np.clip(math.floor(motor_value / 8.0), 0, 31)) - ) - missing_roles = tuple(runtime.latest_group_missing_candidate_roles) - signature = ( - item.spec.key, - item.cycle, - item.direction, - motor_bucket, - selected is not None, - str(group_pnp_reason), - tuple( - sorted( - (str(key), str(value)) - for key, value in pnp_rejections.items() - ) - ), - missing_roles, - oblique_group_roles, - ) - if runtime.last_pnp_diagnostic_signature == signature: - return - runtime.last_pnp_diagnostic_signature = signature - append_jsonl( - self.raw_path, - { - "kind": "group_pnp_candidate_event", - "outcome": ( - "selected_with_oblique_group_constraint" - if selected is not None - else "group_pose_rejected" - ), - "view": runtime.name, - "task_name": item.spec.key, - "motor_index": item.spec.motor_index, - "cycle": item.cycle, - "direction": item.direction, - "feedback_u8": motor_value, - "feedback_bucket_u8": ( - None if motor_bucket < 0 else motor_bucket * 8 - ), - "image_stamp_ns": int(stamp_ns), - "required_roles": list(required_roles), - "required_tag_ids": { - role: runtime.view_tags[role] - for role in required_roles - }, - "group_pnp_reason": str(group_pnp_reason), - "group_missing_candidate_roles": list(missing_roles), - "pnp_rejections": dict(pnp_rejections), - "oblique_group_roles": list(oblique_group_roles), - "candidate_diagnostics": candidate_diagnostics, - "tag_quality": { - role: { - "hamming": int(quality.hamming), - "decision_margin": float(quality.decision_margin), - "mean_edge_pixels": float(quality.edge_pixels), - } - for role, quality in qualities.items() - if role in required_roles - }, - "corners_xy": { - role: np.asarray(corners, dtype=float).tolist() - for role, corners in corners_by_role.items() - if role in required_roles - }, - "camera_intrinsics_sha256": runtime.intrinsics_sha256, - "camera_matrix": ( - None - if runtime.camera_matrix is None - else runtime.camera_matrix.tolist() - ), - }, - ) - - def _camera_info_callback(self, view: str, message: CameraInfo) -> None: - runtime = self.views[view] - valid = ( - int(message.width) > 0 - and int(message.height) > 0 - and len(message.p) == 12 - and float(message.p[0]) > 0.0 - and float(message.p[5]) > 0.0 - ) - matrix = None - if valid: - matrix = np.asarray(message.p, dtype=float).reshape(3, 4)[:, :3] - valid = bool( - np.all(np.isfinite(matrix)) - and matrix[0, 0] > 0.0 - and matrix[1, 1] > 0.0 - ) - if ( - matrix is not None - and runtime.camera_matrix is not None - and not np.allclose(matrix, runtime.camera_matrix) - ): - self._reset_view_trackers(runtime) - runtime.camera_info_valid = valid - runtime.camera_matrix = matrix if valid else None - runtime.camera_frame = str(message.header.frame_id) - runtime.image_width = int(message.width) - runtime.image_height = int(message.height) - runtime.intrinsics_sha256 = ( - "" - if matrix is None - else camera_info_fingerprint( - width=message.width, - height=message.height, - camera_matrix=message.k, - distortion=message.d, - rectification=message.r, - projection=message.p, - ) - ) - runtime.extrinsics_valid = False - if valid and self.extrinsics is not None: - runtime.extrinsics_valid = self.extrinsics.camera_matches( - view, - serial_number=self.camera_serials[view], - width=runtime.image_width, - height=runtime.image_height, - intrinsics_sha256=runtime.intrinsics_sha256, - ) - - def _state_callback(self, message: JointState) -> None: - command_names = _command_names(self) - 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 - stamp = _stamp_ns(message.header.stamp) - if stamp <= 0: - stamp = int(self.get_clock().now().nanoseconds) - self.latest_state_u8 = state - self.last_state_at = time.monotonic() - self.state_receive_times.append(self.last_state_at) - if not self.state_history or stamp > self.state_history[-1].stamp_ns: - self.state_history.append(StateSample(stamp, state)) - - def _info_callback(self, message: String) -> None: - try: - value = json.loads(message.data) - except json.JSONDecodeError: - return - if isinstance(value, dict): - self.latest_hand_info = value - - def _quality_valid(self, quality: TagQuality, *, include_pnp: bool) -> bool: - return tag_quality_is_valid( - quality, - maximum_hamming=self.maximum_hamming, - minimum_decision_margin=self.minimum_decision_margin, - minimum_edge_pixels=self.minimum_edge_pixels, - maximum_reprojection_error_px=( - self.pnp_maximum_reprojection_error_px if include_pnp else None - ), - ) - - def _detections_callback( - self, view: str, message: AprilTagDetectionArray - ) -> None: - runtime = self.views[view] - now = time.monotonic() - stamp = _stamp_ns(message.header.stamp) - required_roles = self._required_roles_for_view(view) - locked_base_role = self._locked_base_role_for_active_capture(view) - live_required_roles = self._live_required_roles_for_view( - view, required_roles - ) - palm_observer: PalmAxisObserver | None = None - if ( - self.active_sweep is not None - and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - ): - palm_observer = _palm_axis_observer_for_sweep( - _node_profile(self), self.active_sweep.spec, view - ) - palm_observer_roles = ( - () - if palm_observer is None - else (palm_observer.parent_role, palm_observer.child_role) - ) - combination_capture = bool( - self.active_combination_validation is not None - and self.state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE} - ) - if required_roles != runtime.current_required_roles: - runtime.current_required_roles = required_roles - runtime.valid_flags.clear() - runtime.detection_times.clear() - self._reset_view_pnp_diagnostics(runtime) - runtime.task_valid_frames = 0 - runtime.task_total_frames = 0 - self._reset_view_trackers(runtime) - qualities: dict[str, TagQuality] = {} - corners_by_role: dict[str, np.ndarray] = {} - centres_by_role: dict[str, np.ndarray] = {} - for detection in message.detections: - role = runtime.role_by_id.get(int(detection.id)) - if role is None: - continue - corners = np.asarray( - [[float(point.x), float(point.y)] for point in detection.corners], - dtype=float, - ) - if corners.shape != (4, 2): - continue - edges = np.linalg.norm(corners - np.roll(corners, -1, axis=0), axis=1) - qualities[role] = TagQuality( - hamming=int(detection.hamming), - decision_margin=float(detection.decision_margin), - edge_pixels=float(np.mean(edges)), - ) - corners_by_role[role] = corners - centres_by_role[role] = np.mean(corners, axis=0) - - if ( - view == "top" - and locked_base_role is not None - and locked_base_role in corners_by_role - and runtime.locked_base_corners_xy is not None - ): - corner_drift = _maximum_corner_drift_px( - runtime.locked_base_corners_xy, - corners_by_role[locked_base_role], - ) - runtime.latest_locked_base_corner_drift_px = corner_drift - if corner_drift > self.fixed_base_maximum_corner_drift_px: - runtime.locked_base_corner_drift_count += 1 - else: - runtime.locked_base_corner_drift_count = 0 - if ( - runtime.locked_base_corner_drift_count - >= self.fixed_base_movement_confirmation_frames - ): - append_jsonl( - self.raw_path, - { - "kind": "fixed_base_reference_moved", - "view": view, - "role": locked_base_role, - "tag_id": runtime.view_tags[locked_base_role], - "corner_drift_px": round(corner_drift, 6), - "maximum_corner_drift_px": ( - self.fixed_base_maximum_corner_drift_px - ), - "confirmation_frames": ( - runtime.locked_base_corner_drift_count - ), - "task_name": ( - "" - if self.active_sweep is None - else self.active_sweep.spec.key - ), - }, - ) - self._pause("fixed_base_reference_moved") - return - - pose_roles = ( - tuple( - role - for role in runtime.roles - if role in qualities - and role != locked_base_role - and self._quality_valid(qualities[role], include_pnp=False) - ) - if combination_capture - else tuple( - dict.fromkeys( - ( - *live_required_roles, - *( - role - for role in palm_observer_roles - if role in qualities - and role != locked_base_role - and self._quality_valid( - qualities[role], include_pnp=False - ) - ), - ) - ) - ) - ) - if locked_base_role is not None: - # The fixed pose is injected below. A live top base remains a - # detection/movement requirement but is never re-solved into the - # pitch/roll/yaw geometry after the session reference is locked. - pose_roles = tuple( - role for role in pose_roles if role != locked_base_role - ) - observable_combination_joints = ( - _combination_observable_joints( - self.profile, - view, - ( - *pose_roles, - *((locked_base_role,) if locked_base_role else ()), - ), - ) - if combination_capture - else () - ) - detection_good = bool( - all(role in qualities for role in live_required_roles) - and all( - self._quality_valid(qualities[role], include_pnp=False) - for role in live_required_roles - ) - and ( - not combination_capture - or bool(observable_combination_joints) - ) - ) - tracking_command_u8: int | None = None - tracking_direction: str | None = None - matched_tracking: tuple[tuple[float, ...], int] | None = None - if ( - self.active_sweep is not None - and ( - view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - or palm_observer is not None - ) - and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - ): - matched_tracking = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) - if matched_tracking is not None: - matched_tracking_state = matched_tracking[0] - if ( - self.state == STATE_SWEEP - or self._motion_command_reached( - self.active_sweep.spec, - self.active_sweep.start_u8, - matched_tracking_state, - ) - ): - # During PREPARE, learn only the final held start pose. - # In particular a roll task approaches 255 from its 127 - # baseline; caching that preparation motion as the formal - # decreasing trajectory would contaminate the same-command - # return reference before the real scan overwrites it. - tracking_command_u8 = int( - np.clip( - np.rint( - matched_tracking_state[ - self.active_sweep.spec.motor_index - ] - ), - 0, - 255, - ) - ) - tracking_direction = self.active_sweep.direction - selected: dict[str, SquareTagPose] | None = None - pnp_rejections: dict[str, str] = {} - group_pnp_reason = "" - if detection_good and runtime.camera_matrix is not None: - candidates: dict[str, tuple[SquareTagPose, ...]] = {} - individual_selected: dict[str, SquareTagPose] = {} - for role in pose_roles: - pose, rejection = runtime.tracker.estimate( - role, - corners_by_role[role], - tag_size_m=float( - runtime.tag_sizes_m_by_role.get( - role, runtime.tag_size_m - ) - ), - camera_matrix=runtime.camera_matrix, - stamp_ns=stamp, - ) - if pose is not None: - individual_selected[role] = pose - candidates[role] = runtime.tracker.last_candidates_by_role.get( - role, () - ) - # A group-constrained task deliberately retains an oblique - # candidate that the independent per-Tag selector would not - # use. Report a rejection only if candidate generation itself - # failed; otherwise the group tracker owns the decision. - if rejection and ( - combination_capture or not candidates[role] - ): - pnp_rejections[role] = rejection - if combination_capture: - # Each visible target is tracked independently. This avoids - # an exponential all-Tag IPPE search and, more importantly, - # lets an occluded neighbouring finger remain optional. - selected = individual_selected - else: - if locked_base_role is not None: - assert runtime.locked_base_pose is not None - candidates[locked_base_role] = ( - runtime.locked_base_pose, - ) - group_tracker = self._group_tracker_for_roles( - runtime, required_roles - ) - selected, group_pnp_reason = group_tracker.select( - candidates, - stamp_ns=stamp, - trajectory_command_u8=tracking_command_u8, - trajectory_direction=tracking_direction, - ) - runtime.latest_group_missing_candidate_roles = ( - group_tracker.last_missing_roles - ) - if selected is not None and palm_observer_roles: - selected = dict(selected) - for role in palm_observer_roles: - pose = individual_selected.get(role) - quality = qualities.get(role) - if pose is None or quality is None: - continue - pose_quality = TagQuality( - hamming=quality.hamming, - decision_margin=quality.decision_margin, - edge_pixels=quality.edge_pixels, - reprojection_error_px=pose.reprojection_error_px, - ) - if self._quality_valid( - pose_quality, include_pnp=True - ): - selected[role] = pose - if selected is not None: - qualities = _selected_pose_qualities( - selected, - qualities, - locked_base_role=locked_base_role, - locked_base_quality=runtime.locked_base_quality, - ) - # Preserve exactly which Tags were present in this camera message. - # The cached palm reference participates in geometry and quality - # checks below, but must never be reported as a live detection. - live_qualities = { - role: quality - for role, quality in qualities.items() - if role in corners_by_role - } - if selected is not None and locked_base_role is not None: - assert runtime.locked_base_pose is not None - assert runtime.locked_base_center_xy_px is not None - assert runtime.locked_base_quality is not None - selected = dict(selected) - selected[locked_base_role] = runtime.locked_base_pose - centres_by_role[locked_base_role] = np.asarray( - runtime.locked_base_center_xy_px, dtype=float - ) - qualities[locked_base_role] = runtime.locked_base_quality - valid = bool( - selected is not None - and all(role in selected for role in required_roles) - and all( - self._quality_valid(qualities[role], include_pnp=True) - for role in ( - selected - if combination_capture - else required_roles - ) - ) - and ( - not combination_capture - or bool( - _combination_observable_joints( - self.profile, view, tuple(selected) - ) - ) - ) - ) - if ( - self.state == STATE_SWEEP - and self.active_sweep is not None - and view in _sweep_views(_node_profile(self), self.active_sweep.spec) - ): - self.sweep_detection_total_frames += 1 - total_by_view = getattr( - self, "sweep_detection_total_by_view", None - ) - if total_by_view is None: - self.sweep_detection_total_by_view = {} - total_by_view = self.sweep_detection_total_by_view - total_by_view[view] = total_by_view.get(view, 0) + 1 - if valid: - self.sweep_detection_valid_frames += 1 - valid_by_view = getattr( - self, "sweep_detection_valid_by_view", None - ) - if valid_by_view is None: - self.sweep_detection_valid_by_view = {} - valid_by_view = self.sweep_detection_valid_by_view - valid_by_view[view] = valid_by_view.get(view, 0) + 1 - runtime.latest_pnp_rejections = pnp_rejections - runtime.latest_group_pnp_reason = group_pnp_reason - if not group_pnp_reason: - runtime.latest_group_missing_candidate_roles = () - runtime.latest_pnp_valid = valid - task_pnp_capture = bool( - self.active_sweep is not None - and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - and view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - if task_pnp_capture: - for role, rejection in pnp_rejections.items(): - key = f"{role}:{rejection}" - runtime.pnp_rejection_counts[key] = ( - runtime.pnp_rejection_counts.get(key, 0) + 1 - ) - if group_pnp_reason.startswith("group_initializing:"): - try: - progress = group_pnp_reason.split(":", 1)[1] - accepted, required = progress.split("/", 1) - runtime.pnp_initialization_progress = ( - int(accepted), - int(required), - ) - except (IndexError, TypeError, ValueError): - runtime.pnp_initialization_progress = None - elif group_pnp_reason: - runtime.group_pnp_rejection_counts[group_pnp_reason] = ( - runtime.group_pnp_rejection_counts.get( - group_pnp_reason, 0 - ) - + 1 - ) - elif selected is not None and runtime.pnp_initialization_progress: - required = runtime.pnp_initialization_progress[1] - runtime.pnp_initialization_progress = (required, required) - self._record_group_pnp_candidate_event( - runtime=runtime, - stamp_ns=stamp, - required_roles=required_roles, - pose_roles=pose_roles, - corners_by_role=corners_by_role, - qualities=qualities, - matched_tracking=matched_tracking, - selected=selected, - pnp_rejections=pnp_rejections, - group_pnp_reason=group_pnp_reason, - ) - if view == "top": - runtime.pnp_invalid_since, reset_due = update_pnp_reset_watchdog( - detection_good=bool( - detection_good and runtime.camera_matrix is not None - ), - pnp_valid=valid, - now=now, - invalid_since=runtime.pnp_invalid_since, - reset_after_seconds=self.top_pnp_invalid_reset_seconds, - ) - if reset_due: - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 - self.get_logger().warning( - "Top-view PnP was continuously invalid for " - f"{self.top_pnp_invalid_reset_seconds:.1f}s; reset trackers " - f"(count={runtime.pnp_reset_count}, " - f"group_reason={group_pnp_reason or 'none'}, " - f"tag_rejections={pnp_rejections})" - ) - if ( - valid - and selected is not None - and self.state == STATE_PREFLIGHT - and self.started - and self.startup_baseline_recovered - ): - base_role = _fixed_base_role(view) - if ( - base_role in selected - and base_role in centres_by_role - and base_role in live_qualities - ): - runtime.fixed_base_observations.append( - ( - selected[base_role], - tuple( - float(value) - for value in centres_by_role[base_role] - ), - live_qualities[base_role], - ) - ) - runtime.fixed_base_corner_observations.append( - np.asarray(corners_by_role[base_role], dtype=float).copy() - ) - runtime.latest_tag_quality = live_qualities - runtime.last_message_at = now - runtime.detection_times.append(now) - runtime.valid_flags.append(valid) - task_sweep_capture = bool( - self.state == STATE_SWEEP - and self.active_sweep is not None - and view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - if ( - runtime.current_required_roles - != tuple(runtime.preflight_roles) - and task_sweep_capture - ): - # Attempt-level validity is a motion-trajectory metric. PREPARE - # deliberately rejects the first N frames while the group PnP - # tracker initializes; counting those expected frames made clean - # side tasks report about 94% and triggered full four-round - # rescans despite 100% visibility prechecks and dense trajectories. - runtime.task_total_frames += 1 - if valid: - runtime.task_valid_frames += 1 - if not valid or selected is None: - return - runtime.last_valid_at = now - - # Preflight and motion-only states need pose quality/status, but do not - # consume FrameObservation objects. Building transforms and payloads - # for every measured joint on all three 30 Hz streams was pure work - # and could starve the detection subscriptions. Only materialize an - # observation for the one view used by an active capture stage. - sweep_capture = bool( - self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - and self.active_sweep is not None - and view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - palm_axis_capture = bool( - self.state == STATE_SWEEP - and self.active_sweep is not None - and not self.active_sweep.precheck - and self.active_sweep.cycle >= 0 - and palm_observer is not None - ) - validation_capture = bool( - self.state == STATE_VALIDATION_CAPTURE - and ( - ( - self.active_validation is not None - and view in _sweep_views( - self.profile, self.active_validation.spec - ) - ) - or self.active_combination_validation is not None - ) - ) - if not sweep_capture and not validation_capture and not palm_axis_capture: - return - - matched = matched_tracking - if matched is None: - matched = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) - if matched is None: - return - state_u8, sync_error_ns = matched - joint_vectors: dict[str, tuple[float, float, float]] = {} - image_vectors: dict[str, tuple[float, float]] = {} - joint_quaternions: dict[str, tuple[float, float, float, float]] = {} - parent_poses_common: dict[str, Mapping[str, list[float]]] = {} - child_poses_common: dict[str, Mapping[str, list[float]]] = {} - joint_reprojection: dict[str, float] = {} - if self.extrinsics is None or not runtime.extrinsics_valid: - return - front_from_view = self.extrinsics.transform(view) - if palm_axis_capture and palm_observer is not None: - self._record_palm_axis_sample( - palm_observer, - selected, - front_from_view, - state_u8, - sync_error_ns=int(sync_error_ns), - stamp_ns=stamp, - ) - if not sweep_capture and not validation_capture: - return - for name, spec in self.profile.record_specs.items(): - if not spec.measured or spec.view != view: - continue - assert spec.parent_role is not None and spec.child_role is not None - if ( - spec.parent_role not in selected - or spec.child_role not in selected - ): - continue - parent = selected[spec.parent_role] - child = selected[spec.child_role] - parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) - relative = parent_rotation.inv().apply( - np.asarray(child.translation_xyz_m, dtype=float) - - np.asarray(parent.translation_xyz_m, dtype=float) - ) - image_relative = ( - centres_by_role[spec.child_role] - - centres_by_role[spec.parent_role] - ) - parent_matrix = front_from_view @ transform_matrix( - parent.translation_xyz_m, parent.quaternion_xyzw - ) - child_matrix = front_from_view @ transform_matrix( - child.translation_xyz_m, child.quaternion_xyzw - ) - relative_rotation = Rotation.from_matrix( - parent_matrix[:3, :3] - ).inv() * Rotation.from_matrix(child_matrix[:3, :3]) - joint_vectors[name] = tuple(float(value) for value in relative) - image_vectors[name] = tuple(float(value) for value in image_relative) - joint_quaternions[name] = tuple( - float(value) for value in relative_rotation.as_quat() - ) - parent_poses_common[name] = matrix_payload(parent_matrix) - child_poses_common[name] = matrix_payload(child_matrix) - joint_reprojection[name] = max( - float(parent.reprojection_error_px), - float(child.reprojection_error_px), - ) - observation = FrameObservation( - stamp_ns=stamp, - received_at=now, - view=view, - state_u8=state_u8, - state_sync_error_ns=int(sync_error_ns), - joint_vectors_xyz_m=joint_vectors, - image_vectors_xy_px=image_vectors, - joint_quaternions_xyzw=joint_quaternions, - parent_poses_common=parent_poses_common, - child_poses_common=child_poses_common, - joint_reprojection_error_px=joint_reprojection, - ) - self._accept_frame(observation) - - def _record_palm_axis_sample( - self, - observer: PalmAxisObserver, - selected: Mapping[str, SquareTagPose], - common_from_view: np.ndarray, - state_u8: Sequence[float], - *, - sync_error_ns: int, - stamp_ns: int, - ) -> None: - """Buffer one direction-only sample without touching sweep validity.""" - item = self.active_sweep - if ( - item is None - or item.spec.key != observer.task_name - or observer.parent_role not in selected - or observer.child_role not in selected - ): - return - parent = selected[observer.parent_role] - child = selected[observer.child_role] - 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 = ( - Rotation.from_matrix(parent_matrix[:3, :3]).inv() - * Rotation.from_matrix(child_matrix[:3, :3]) - ) - feedback = float(state_u8[observer.motor_index]) - durable = canonical_sample_record({ - "kind": "palm_axis_sample", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": observer.view, - "source_joint": observer.source_name, - "model_joint": observer.model_joint, - "motor_index": int(observer.motor_index), - "cycle": int(item.cycle), - "direction": item.direction, - "requested_command_u8": int(item.target_u8), - "feedback_u8": feedback, - "relative_quaternion_xyzw": [ - float(value) for value in relative_rotation.as_quat() - ], - "parent_pose_common": matrix_payload(parent_matrix), - "child_pose_common": matrix_payload(child_matrix), - "state_u8": [float(value) for value in state_u8], - "state_image_sync_error_ms": round( - abs(int(sync_error_ns)) / 1_000_000.0, 6 - ), - "pnp_reprojection_error_px": round( - max( - float(parent.reprojection_error_px), - float(child.reprojection_error_px), - ), - 6, - ), - "image_stamp_ns": int(stamp_ns), - }) - record = fitting_sample_record(durable) - self.palm_axis_records_by_source.setdefault( - observer.source_name, [] - ).append(record) - - def _persist_palm_axis_samples(self, item: SweepItem) -> None: - """Commit the side channel only after its authoritative sweep passes.""" - observer = next( - ( - candidate - for candidate in self.profile.palm_axis_observers - if candidate.task_name == item.spec.key - ), - None, - ) - if observer is None: - return - attempt = int( - self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) - ) - rows = [ - record - for record in self.palm_axis_records_by_source.get( - observer.source_name, () - ) - if int(record.get("attempt", 1)) == attempt - and int(record.get("cycle", -1)) == int(item.cycle) - and str(record.get("direction", "")) == item.direction - ] - append_jsonl_many( - self.raw_path, - (canonical_sample_record(record) for record in rows), - ) - - def _accept_frame(self, observation: FrameObservation) -> None: - if self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: - if observation.view not in _sweep_views( - _node_profile(self), self.active_sweep.spec - ): - return - if not self._motion_command_reached( - self.active_sweep.spec, - self.active_sweep.start_u8, - observation.state_u8, - ): - return - self.sweep_start_frames.append(observation) - maximum_start_frames = max( - 1, int(getattr(self, "preflight_frames", 30)) - ) * len( - _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - if len(self.sweep_start_frames) > maximum_start_frames: - del self.sweep_start_frames[:-maximum_start_frames] - return - if self.state == STATE_SWEEP and self.active_sweep is not None: - if observation.view not in _sweep_views( - self.profile, self.active_sweep.spec - ): - return - motor = self.active_sweep.spec.motor_index - value = float(observation.state_u8[motor]) - if not -3.0 <= value <= 258.0: - return - if not self._motion_command_reached( - self.active_sweep.spec, - int(np.clip(np.rint(value), 0, 255)), - observation.state_u8, - ): - return - self.sweep_frames.append(observation) - checkpoint_target = getattr( - self, "sweep_checkpoint_target_u8", None - ) - if ( - checkpoint_target is not None - and getattr(self, "sweep_checkpoint_hold_since", None) - is not None - and self._steady_checkpoint_reached( - self.active_sweep.spec, - int(checkpoint_target), - observation.state_u8, - ) - ): - self.sweep_checkpoint_frames.append(observation) - if ( - getattr(self, "sweep_baseline_pending", False) - and getattr(self, "sweep_baseline_hold_since", None) - is not None - and observation.received_at - - float(self.sweep_baseline_hold_since) - >= float(self.baseline_hold_seconds) - and self._motion_command_reached( - self.active_sweep.spec, - int( - self.baseline_command[ - self.active_sweep.spec.motor_index - ] - ), - observation.state_u8, - ) - ): - self.sweep_baseline_frames.append(observation) - self.sweep_last_valid_at = observation.received_at - last_by_view = getattr( - self, "sweep_last_valid_at_by_view", None - ) - if last_by_view is None: - self.sweep_last_valid_at_by_view = {} - last_by_view = self.sweep_last_valid_at_by_view - last_by_view[observation.view] = observation.received_at - return - if ( - self.state == STATE_VALIDATION_CAPTURE - and self.active_validation is not None - and observation.view in _sweep_views( - self.profile, self.active_validation.spec - ) - ): - if self._motion_command_reached( - self.active_validation.spec, - self.active_validation.command_u8, - observation.state_u8, - ): - self.validation_frames_buffer.append(observation) - return - if ( - self.state == STATE_VALIDATION_CAPTURE - and self.active_combination_validation is not None - and self._command_vector_reached( - self.active_combination_validation.command_u8 - ) - ): - self.combination_validation_frames_buffer[ - observation.view - ].append(observation) - - def _view_ready(self, runtime: ViewRuntime, now: float) -> bool: - minimum_frames = min(30, self.preflight_frames) - return bool( - runtime.camera_info_valid - and runtime.extrinsics_valid - and len(runtime.valid_flags) >= minimum_frames - and runtime.valid_rate >= self.minimum_detection_rate - and runtime.detection_hz >= self.minimum_detection_hz - and now - runtime.last_message_at <= 1.0 - ) - - def _all_preflight_ready(self, now: float) -> bool: - feedback_hz = ( - 0.0 - if len(self.state_receive_times) < 2 - or self.state_receive_times[-1] <= self.state_receive_times[0] - else (len(self.state_receive_times) - 1) - / (self.state_receive_times[-1] - self.state_receive_times[0]) - ) - return bool( - self.extrinsics is not None - and len(self.latest_state_u8) == 20 - and now - self.last_state_at <= 1.0 - and feedback_hz >= self.minimum_feedback_hz - and all(self._view_ready(runtime, now) for runtime in self.views.values()) - ) - - def _all_devices_ready(self, now: float) -> bool: - """Check only hardware transport needed for a safe baseline return. - - Tag visibility must not gate this check: an interrupted calibration can - leave the fingers in a pose that occludes a fixed palm Tag. Requiring - that Tag before commanding baseline would create a startup deadlock. - """ - feedback_hz = ( - 0.0 - if len(self.state_receive_times) < 2 - or self.state_receive_times[-1] <= self.state_receive_times[0] - else (len(self.state_receive_times) - 1) - / (self.state_receive_times[-1] - self.state_receive_times[0]) - ) - return bool( - self.extrinsics is not None - and len(self.latest_state_u8) == 20 - and now - self.last_state_at <= 1.0 - and feedback_hz >= self.minimum_feedback_hz - and all( - runtime.camera_info_valid - and runtime.extrinsics_valid - and runtime.last_message_at > 0.0 - and now - runtime.last_message_at <= 1.0 - for runtime in self.views.values() - ) - ) - - def _finish_startup_baseline_recovery(self) -> None: - """Start a fresh fixed-palm-Tag window at confirmed baseline.""" - for runtime in self.views.values(): - runtime.valid_flags.clear() - runtime.detection_times.clear() - observations = getattr(runtime, "fixed_base_observations", None) - if observations is not None: - observations.clear() - corner_observations = getattr( - runtime, "fixed_base_corner_observations", None - ) - if corner_observations is not None: - corner_observations.clear() - runtime.locked_base_pose = None - runtime.locked_base_center_xy_px = None - runtime.locked_base_corners_xy = None - runtime.locked_base_quality = None - runtime.locked_base_corner_drift_count = 0 - runtime.latest_locked_base_corner_drift_px = 0.0 - self._reset_view_trackers(runtime) - self.startup_baseline_recovered = True - self.state = STATE_PREFLIGHT - self.reason = "waiting_for_baseline_tags_after_recovery" - - def _active_view_for_resume(self) -> str | None: - if self.active_sweep is not None: - return self.active_sweep.spec.view - if self.active_validation is not None: - return self.active_validation.spec.view - if self.retry_sweep_spec is not None: - return self.retry_sweep_spec.view - return None - - def _active_views_for_resume(self) -> tuple[str, ...]: - if self.active_sweep is not None: - return _sweep_views(_node_profile(self), self.active_sweep.spec) - if self.active_validation is not None: - return _sweep_views(_node_profile(self), self.active_validation.spec) - if self.retry_sweep_spec is not None: - return _sweep_views(_node_profile(self), self.retry_sweep_spec) - return required_resume_views(None) - - def _resume_preflight_ready(self, now: float) -> bool: - if ( - len(self.latest_state_u8) != _command_count(self) - or now - self.last_state_at > 1.0 - ): - return False - return all( - self._view_ready(self.views[view], now) - for view in self._active_views_for_resume() - ) - - def _prepare_failed_sweep_retry(self) -> SweepSpec: - """Discard only failed measurements/cycles and queue two directions.""" - if self.retry_sweep_spec is None: - raise RuntimeError("no failed sweep is pending retry") - spec = self.retry_sweep_spec - repetitions = int(getattr(self, "repetitions", 3)) - cycles = set(getattr(self, "retry_cycles", set(range(repetitions)))) - profile = _node_profile(self) - source_task_keys = ( - tuple(getattr(self, "retry_source_task_keys", ())) - if getattr(self, "retry_source_failure_task_key", None) == spec.key - else () - ) - source_key_set = set(source_task_keys) - source_specs = [ - candidate - for candidate in profile.sweep_specs - if candidate.key in source_key_set - ] - # Yaw zero is a same-view relationship between the pitch and roll - # axes. A retry must remain in the original top-camera PnP reference - # generation; otherwise the retry changes the datum it is intended to - # verify. Ordinary single-task retries still reset their camera to - # recover from a genuinely bad branch. - self.retry_preserve_pnp_continuity = bool(source_specs) - data_specs = source_specs or [spec] - retry_joint_names = set(getattr(self, "retry_joint_names", set())) - if source_specs: - retry_joint_names = { - joint_name - for candidate in source_specs - for joint_name in candidate.joints - } - else: - retry_joint_names = retry_joint_names or set(spec.joints) - retry_joint_names.intersection_update(spec.joints) - if not retry_joint_names: - retry_joint_names = set(spec.joints) - self.retry_joint_names = retry_joint_names - baseline_records_by_joint = getattr( - self, "baseline_records_by_joint", {} - ) - for joint_name in retry_joint_names: - self.records_by_joint[joint_name][:] = [ - record - for record in self.records_by_joint[joint_name] - if int(record.get("cycle", -1)) not in cycles - ] - if joint_name in baseline_records_by_joint: - baseline_records_by_joint[joint_name][:] = [ - record - for record in baseline_records_by_joint[joint_name] - if int(record.get("cycle", -1)) not in cycles - ] - command_records = getattr(self, "command_records_by_joint", {}) - if joint_name in command_records: - command_records[joint_name][:] = [ - record - for record in command_records[joint_name] - if int(record.get("cycle", -1)) not in cycles - ] - data_task_keys = {candidate.key for candidate in data_specs} - observers = [ - item - for item in profile.palm_axis_observers - if item.task_name in data_task_keys - ] - for observer in observers: - records = self.palm_axis_records_by_source.get( - observer.source_name, [] - ) - records[:] = [ - record - for record in records - if int(record.get("cycle", -1)) not in cycles - ] - all_items = getattr(self, "sweep_items", []) - self.retry_sweep_items = [ - item - for item in all_items - if item.spec in data_specs and item.cycle in cycles - ] - if not self.retry_sweep_items: - self.retry_sweep_items = [ - SweepItem(candidate, cycle, direction) - for candidate in data_specs - for cycle in sorted(cycles) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ] - # A planar Tag PnP tracker can occasionally remain on a continuous but - # biased pose branch for an otherwise clean sweep. Starting a full - # fit retry with that tracker state intact tends to reproduce the same - # orientation residual, so reset only the affected camera before the - # retry. Raw samples remain logged and all final quality gates stay - # unchanged. - reset_trackers = getattr(self, "_reset_view_trackers", None) - retry_views = { - profile.record_specs[name].view - for name in retry_joint_names - } - for observer in observers: - retry_views.add(observer.view) - for view in retry_views: - if view is None: - continue - runtime = getattr(self, "views", {}).get(view) - if runtime is None or reset_trackers is None: - continue - if self.retry_preserve_pnp_continuity: - # Frame-quality counters are attempt-local even though the - # geometric PnP state deliberately remains continuous. - runtime.task_valid_frames = 0 - runtime.task_total_frames = 0 - continue - if _node_profile(self).layout_id == G20_RIGHT_19_LAYOUT: - reset_trackers(runtime, preserve_task_reference=True) - else: - reset_trackers(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - # Detection quality is attempt-local. Carrying the previous - # attempt into this retry made its percentage describe two PnP - # branches instead of the data currently being admitted. - runtime.task_valid_frames = 0 - runtime.task_total_frames = 0 - storage_key = _sweep_storage_key(spec) - self.sweep_attempts[storage_key] = ( - self.sweep_attempts.get(storage_key, 1) + 1 - ) - source_attempts: dict[str, int] = {} - for source_spec in source_specs: - source_key = _sweep_storage_key(source_spec) - self.sweep_attempts[source_key] = ( - self.sweep_attempts.get(source_key, 1) + 1 - ) - source_attempts[str(source_spec.key)] = self.sweep_attempts[ - source_key - ] - invalidated_storage_keys = { - storage_key, - *(_sweep_storage_key(candidate) for candidate in source_specs), - } - for key in list(getattr(self, "sweep_retry_counts", {})): - if key[0] in invalidated_storage_keys: - self.sweep_retry_counts.pop(key, None) - append_jsonl( - self.raw_path, - { - "kind": "retry", - "view": spec.view, - **({"task_name": spec.key} if spec.task_name else {}), - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "joints_to_rescan": sorted(retry_joint_names), - **( - {"source_task_names": list(source_task_keys)} - if source_task_keys - else {} - ), - **( - {"source_attempts": source_attempts} - if source_attempts - else {} - ), - **( - {"pnp_reference_policy": "preserve_existing_generation"} - if self.retry_preserve_pnp_continuity - else {} - ), - "attempt": self.sweep_attempts[storage_key], - "reason": self.paused_reason, - "cycles": [cycle + 1 for cycle in sorted(cycles)], - }, - ) - self.fit_failure = {} - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) - getattr(self, "motion_stall_details", {}).clear() - return spec - - def _invalidate_fitted_calibration_state(self) -> None: - """Invalidate every artifact derived from the current motion records. - - A task rescan changes the observation set, so curves, zero offsets and - validation diagnostics must be treated as one generation. Keeping - this transition in one place prevents final publication from mixing a - newly collected task with state left by an earlier fit attempt. - """ - self.measured_fits = {} - self.axis_measurements = [] - self.palm_orientation_measurements = [] - self.palm_orientation_rejections = {} - self.zero_result = None - self.validated_endpoint_zero_offsets_rad = {} - self.corrected_urdf_path = None - self.fit_quality_passed = False - self.validation_errors_rad = [] - self.cross_view_roll_metrics = {} - self.validation_only_fits = {} - self.joint_dynamic_diagnostics = {} - - def _endpoint_zero_offsets_for_publication(self) -> dict[str, float]: - """Return the endpoint anchors belonging to the validated zero fit.""" - stored = { - str(name): float(value) - for name, value in getattr( - self, "validated_endpoint_zero_offsets_rad", {} - ).items() - } - if self.profile.layout_id != G20_RIGHT_19_LAYOUT: - if stored: - raise RuntimeError( - "validated_endpoint_zero_state_unexpected_for_layout" - ) - return {} - expected = set(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) - if getattr(self, "standalone_thumb_calibration", False): - expected = { - name for name in expected if name.startswith("thumb_") - } - actual = set(stored) - if actual != expected: - missing = ",".join(sorted(expected - actual)) or "-" - extra = ",".join(sorted(actual - expected)) or "-" - raise RuntimeError( - "validated_endpoint_zero_state_incomplete:" - f"missing={missing};extra={extra}" - ) - if not all(math.isfinite(value) for value in stored.values()): - raise RuntimeError("validated_endpoint_zero_state_non_finite") - if self.zero_result is None: - raise RuntimeError("URDF zero solution is missing") - solved = self.zero_result.direct_offsets_rad - diverged = sorted( - name - for name, value in stored.items() - if name not in solved - or not math.isclose( - value, - float(solved[name]), - rel_tol=0.0, - abs_tol=1.0e-12, - ) - ) - if diverged: - raise RuntimeError( - "validated_endpoint_zero_state_diverged:" - + ",".join(diverged) - ) - return stored - - def _restore_durable_task_checkpoint(self) -> int: - """Restore independently validated complete tasks from a failed session.""" - source = self.resume_raw_samples_path - if source is None: - return 0 - if not source.is_file(): - raise RuntimeError(f"resume raw samples do not exist: {source}") - if source.resolve() == self.raw_path.resolve(): - raise RuntimeError("resume raw samples must come from an older session") - rows: list[dict[str, Any]] = [] - source_size = max(1, int(source.stat().st_size)) - bytes_read = 0 - with source.open("rb") as stream: - for line_number, raw_line in enumerate(stream, start=1): - bytes_read += len(raw_line) - if not raw_line.strip(): - continue - try: - line = raw_line.decode("utf-8") - value = json.loads(line) - except (UnicodeDecodeError, json.JSONDecodeError) as error: - raise RuntimeError( - f"resume raw JSON is invalid at line {line_number}" - ) from error - if isinstance(value, dict): - rows.append(value) - if line_number % 1000 == 0: - self.base_import_progress = { - "phase": "reading", - "records_read": len(rows), - "bytes_read": bytes_read, - "total_bytes": source_size, - "fraction": min(1.0, bytes_read / source_size), - } - self.reason = "reading_base_session_records" - _publish_import_status(self) - starts = [row for row in rows if row.get("kind") == "session_start"] - 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( - start.get("tag_sizes_m_by_id", {}) - ).items() - } - current_tag_sizes = dict(getattr(self, "tag_sizes_m_by_id", {})) - try: - ( - palm_axis_schema_compatible, - palm_axis_invalidated_tasks, - ) = _palm_axis_resume_policy( - self.profile, - start, - getattr( - getattr( - getattr(self, "calibration_profile", None), - "artifacts", - None, - ), - "session_compatibility_tokens", - frozenset(self.profile.capabilities), - ), - ) - except ValueError as error: - raise RuntimeError( - "resume checkpoint algorithm capabilities differ" - ) from error - if ( - str(start.get("model", "G20")).upper() - != str(getattr(self, "model", "G20")).upper() - or str(start.get("hand_type")) != self.hand_type - or str(start.get("tag_layout")) != self.profile.layout_id - or start.get("view_tags") - != {view: dict(tags) for view, tags in self.profile.view_tags.items()} - or tuple(int(value) for value in start.get("baseline_command_u8", [])) - != tuple(self.baseline_command) - or str(start.get("source_urdf_sha256", "")) - != _file_sha256(self.source_urdf_path) - ): - raise RuntimeError( - "resume checkpoint geometry, algorithm capabilities, Tag " - "layout, baseline or source URDF differs" - ) - current_corners_by_view = { - str(view): getattr(runtime, "locked_base_corners_xy", None) - for view, runtime in getattr(self, "views", {}).items() - } - if not current_corners_by_view: - raise RuntimeError( - "resume checkpoint requires current fixed-base references" - ) - ( - position_drift_by_view_px, - position_changed_views, - position_unverifiable_views, - ) = _resume_fixed_base_position_compatibility( - rows, - current_corners_by_view, - float( - getattr(self, "fixed_base_maximum_corner_drift_px", 2.0) - ), - ) - self.resume_source_session = source.parent.name - self.resume_position_drift_by_view_px = dict( - position_drift_by_view_px - ) - self.resume_position_changed_views = position_changed_views - self.resume_position_unverifiable_views = ( - position_unverifiable_views - ) - start_position_invalidated_tasks: tuple[str, ...] = () - if position_changed_views or position_unverifiable_views: - if str(getattr(self, "recalibration_scope", "full")) != "full": - affected = sorted( - {*position_changed_views, *position_unverifiable_views} - ) - raise RuntimeError( - "resume checkpoint start pose differs or cannot be " - "verified for partial recalibration: " - + ",".join(affected) - ) - start_position_invalidated_tasks = tuple( - spec.key for spec in self.profile.sweep_specs - ) - self.resume_position_policy = ( - "discard_all_tasks_for_new_start_pose" - if position_changed_views - else "discard_all_tasks_for_unverified_start_pose" - ) - else: - self.resume_position_policy = "reuse_same_start_pose" - try: - changed_tag_size_ids, size_invalidated_tasks = ( - resume_tasks_invalidated_by_tag_size_changes( - self.profile, - previous_tag_sizes, - current_tag_sizes, - ) - ) - except ValueError as error: - raise RuntimeError( - "resume checkpoint Tag-size geometry differs incompatibly" - ) from error - invalidated_task_set = set(size_invalidated_tasks) - invalidated_task_set.update(palm_axis_invalidated_tasks) - invalidated_task_set.update(start_position_invalidated_tasks) - scope_invalidated_tasks = tuple( - getattr(self, "recalibration_task_keys", ()) or () - ) - invalidated_task_set.update(scope_invalidated_tasks) - if invalidated_task_set: - rows = [ - row - for row in rows - if str(row.get("task_name", "")) not in invalidated_task_set - ] - completed, reusable = resumable_completed_task_prefix( - self.profile, - self.repetitions, - self.baseline_command, - rows, - minimum_sweep_bins=self.minimum_sweep_bins, - allow_sparse=True, - ) - validate_sample_records( - row - for row in reusable - if str(row.get("kind", "")) in RESUMABLE_SAMPLE_KINDS - or row.get("kind") == "palm_axis_sample" - ) - self.state = STATE_REVALIDATING_INHERITED - self.reason = "revalidating_inherited_tasks" - self.base_import_progress = { - "phase": "revalidating", - "records_read": len(rows), - "completed_tasks": 0, - "total_tasks": len(completed), - "fraction": 0.0, - } - _publish_import_status(self) - for durable in reusable: - kind = str(durable["kind"]) - if kind == "synchronised_frame": - continue - if kind == "palm_axis_sample": - source_name = str(durable.get("source_joint", "")) - if source_name not in self.palm_axis_records_by_source: - raise RuntimeError( - "resume checkpoint contains unknown palm-axis source " - + source_name - ) - record = fitting_sample_record(durable) - self.palm_axis_records_by_source[source_name].append(record) - continue - joint_name = str(durable["joint"]) - if joint_name not in self.records_by_joint: - raise RuntimeError( - f"resume checkpoint contains unknown joint {joint_name}" - ) - if kind not in { - "sample", "baseline_hold_sample", "steady_command_sample" - }: - continue - record = fitting_sample_record(durable) - if kind == "sample": - self.records_by_joint[joint_name].append(record) - elif kind == "baseline_hold_sample": - self.baseline_records_by_joint[joint_name].append(record) - else: - self.command_records_by_joint[joint_name].append(record) - completed, dropped = G20ThreeCameraCalibrationNode._revalidate_imported_tasks( - self, completed, allow_sparse=True - ) - completed_set = set(completed) - reusable = tuple( - row - for row in reusable - if str(row.get("task_name", "")) in completed_set - ) - # Attempts are durable identities, not counters local to one process. - # Without this floor a resumed task whose accepted data used attempt 2 - # would label its next retry as attempt 2 again. The raw log then - # contained two different acquisitions with the same identity and an - # offline replay merged them into one invalid trajectory. - attempt_floor_by_task: dict[str, int] = {} - for row in reusable: - task_key = str(row.get("task_name", "")) - if task_key not in completed_set: - continue - try: - attempt = max(1, int(row.get("attempt", 1))) - except (TypeError, ValueError): - attempt = 1 - attempt_floor_by_task[task_key] = max( - attempt_floor_by_task.get(task_key, 1), attempt - ) - attempts = getattr(self, "sweep_attempts", None) - if attempts is None: - self.sweep_attempts = { - _sweep_storage_key(spec): 1 - for spec in self.profile.sweep_specs - } - attempts = self.sweep_attempts - for spec in self.profile.sweep_specs: - if spec.key not in completed_set: - continue - storage_key = _sweep_storage_key(spec) - attempts[storage_key] = max( - int(attempts.get(storage_key, 1)), - attempt_floor_by_task.get(spec.key, 1), - ) - self.resumed_task_keys = completed - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) - missing_tasks = [ - spec.key - for spec in self.profile.sweep_specs - if spec.key not in completed_set - ] - append_jsonl( - self.raw_path, - { - "kind": "resume_checkpoint_import", - "source_session": source.parent.name, - "completed_task_count": len(completed), - "completed_task_keys": list(completed), - "discarded_incomplete_task": ( - missing_tasks[0] if missing_tasks else None - ), - "pending_task_keys": missing_tasks, - "revalidation_dropped_tasks": dropped, - "tag_size_changed_ids": list(changed_tag_size_ids), - "tag_size_invalidated_task_keys": list(size_invalidated_tasks), - "palm_axis_schema_compatible": palm_axis_schema_compatible, - "palm_axis_invalidated_task_keys": list( - palm_axis_invalidated_tasks - ), - "recalibration_scope": str( - getattr(self, "recalibration_scope", "full") - ), - "scope_invalidated_task_keys": list( - scope_invalidated_tasks - ), - "start_position_policy": self.resume_position_policy, - "start_position_changed_views": list( - position_changed_views - ), - "start_position_unverifiable_views": list( - position_unverifiable_views - ), - "start_position_drift_by_view_px": { - view: round(float(value), 6) - for view, value in position_drift_by_view_px.items() - }, - "start_position_invalidated_task_keys": list( - start_position_invalidated_tasks - ), - "imported_record_count": len(reusable), - "imported_attempt_floor_by_task": attempt_floor_by_task, - "source_raw_samples_sha256": _file_sha256(source), - }, - ) - append_jsonl_many(self.raw_path, reusable) - return len(completed) - - def _revalidate_imported_tasks( - self, completed: Sequence[str], *, allow_sparse: bool = False - ) -> tuple[list[str], list[dict[str, Any]]]: - """Re-apply the final hard gates to imported tasks at import time. - - A task that only provisionally passed (warning band) in its source - session would otherwise survive until the final fit re-applies the - unmodified thresholds after every other task is collected, forcing - the session to go back and re-scan it at the very end. Failing - tasks are dropped here. Sparse product resume independently checks - later complete tasks so only the failed task has to be re-collected; - the compatibility mode still drops the remaining suffix. - """ - def discard_task_records(task_key: str) -> None: - spec = next( - ( - candidate - for candidate in self.profile.sweep_specs - if candidate.key == task_key - ), - None, - ) - if spec is None: - return - for joint_name in spec.joints: - for store in ( - self.records_by_joint, - self.baseline_records_by_joint, - self.command_records_by_joint, - ): - if joint_name in store: - store[joint_name].clear() - observer = next( - ( - item - for item in self.profile.palm_axis_observers - if item.task_name == task_key - ), - None, - ) - if observer is not None: - self.palm_axis_records_by_source[ - observer.source_name - ].clear() - - accepted: list[str] = [] - dropped: list[dict[str, Any]] = [] - for task_index, task_key in enumerate(completed, start=1): - if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED: - total = max(1, len(completed)) - self.base_import_progress.update( - { - "current_task": task_key, - "completed_tasks": task_index - 1, - "total_tasks": len(completed), - "fraction": (task_index - 1) / total, - } - ) - _publish_import_status(self) - spec = next( - ( - item.spec - for item in self.sweep_items - if item.spec.key == task_key - ), - None, - ) - failures = ( - G20ThreeCameraCalibrationNode._provisional_fit_failures( - self, spec, include_view_validity=False - ) - if spec is not None - else [{"metric": "missing_task_spec"}] - ) - if failures: - dropped.append( - { - "task": task_key, - "failures": [ - { - key: value - for key, value in failure.items() - if key in {"joint", "metric", "actual", "limit"} - } - for failure in failures[:6] - ], - } - ) - # Make the rejection visible before checking later tasks. - # Some fits intentionally depend on an earlier reference - # task (for example every non-reference finger roll uses the - # pinky roll direction). Delaying this clear until the end - # let a dependent task pass with records that were about to - # be removed, producing a checkpoint that failed only on the - # next restart. - discard_task_records(task_key) - if allow_sparse: - continue - break - accepted.append(task_key) - if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED: - self.base_import_progress.update( - { - "current_task": "", - "completed_tasks": len(completed), - "total_tasks": len(completed), - "fraction": 1.0, - } - ) - _publish_import_status(self) - if dropped and not allow_sparse: - accepted_set = set(accepted) - for task_key in completed: - if task_key not in accepted_set: - discard_task_records(task_key) - if isinstance(completed, tuple): - accepted = tuple(accepted) - return accepted, dropped - - def _start_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_WAIT_START: - response.success = False - response.message = f"not ready: state={self.state} reason={self.reason}" - return response - if not self.commands_enabled: - response.success = False - response.message = "commands_enabled=false; preview cannot move the hand" - return response - if self.command_publisher.get_subscription_count() < 1: - response.success = False - response.message = "hand SDK is not subscribed to the command topic" - return response - if self.setting_publisher.get_subscription_count() < 1: - response.success = False - response.message = "hand SDK is not subscribed to the setting topic" - return response - if len(self.get_publishers_info_by_topic(self.command_topic)) > 1: - response.success = False - response.message = "another node is publishing hand commands" - return response - self.started = True - self.startup_baseline_recovered = False - self.resumed_task_keys = () - self.resume_source_session = "" - self.resume_checkpoint_pending = bool( - self.resume_raw_samples_path is not None - ) - self.resume_position_policy = ( - "pending_new_start_pose_check" - if self.resume_checkpoint_pending - else "not_requested" - ) - self.resume_position_changed_views = () - self.resume_position_unverifiable_views = () - self.resume_position_drift_by_view_px = {} - self.sweep_items = [] - self.pnp_task_spec = None - selected_sweep_specs = list(self.profile.sweep_specs) - if self.standalone_thumb_calibration: - standalone_task_keys = set(self.recalibration_task_keys) - selected_sweep_specs = [ - spec - for spec in selected_sweep_specs - if spec.key in standalone_task_keys - ] - if self.cross_view_roll_diagnostic_finger: - selected_sweep_specs = [ - spec - for spec in selected_sweep_specs - if spec.key - == f"{self.cross_view_roll_diagnostic_finger}_roll_multiview" - ] - if len(selected_sweep_specs) != 1: - raise RuntimeError( - "cross-view diagnostic could not resolve multiview task" - ) - selected_profile = replace( - self.profile, - sweep_specs=tuple(selected_sweep_specs), - ) - self.sweep_items.extend( - _build_sweep_plan(selected_profile, self.repetitions) - ) - self.sweep_index = 0 - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_sweep_items.clear() - self.retry_joint_names.clear() - self.retry_source_failure_task_key = None - self.retry_source_task_keys = () - self.retry_cycle_override.clear() - self.retry_preserve_pnp_continuity = False - self.active_sweep_is_fit_retry = False - self.fit_failure = {} - self.fit_failure_history_by_task.clear() - self.cross_view_roll_diagnostic_result = {} - self.motion_stall_details.clear() - self.sweep_attempts = { - _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs - } - self.sweep_retry_counts.clear() - self.motion_retry_counts.clear() - self.validation_retry_counts.clear() - self.precheck_speed_metrics.clear() - self.formal_speed_scales.clear() - self.records_by_joint = { - name: [] for name in self.profile.record_joints - } - self.baseline_records_by_joint = { - name: [] for name in self.profile.record_joints - } - self.command_records_by_joint = { - name: [] for name in self.profile.record_joints - } - self.palm_axis_records_by_source = { - observer.source_name: [] - for observer in self.profile.palm_axis_observers - } - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) - self.combination_validation_items.clear() - self.combination_validation_index = 0 - self.active_combination_validation = None - self.combination_validation_completed = False - self.combination_tag_mounts.clear() - self.combination_tag_observation_counts.clear() - self.combination_tag_validation_counts.clear() - self.combination_position_errors_m.clear() - self.combination_orientation_errors_rad.clear() - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.completed_payload = None - self.sweep_frames.clear() - self.sweep_baseline_frames.clear() - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since = None - self.sweep_start_frames.clear() - self.carried_sweep_start_frames.clear() - append_jsonl( - 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, - "sample_schema_version": self.sample_schema_version, - "recalibration_scope": self.recalibration_scope, - "recalibration_task_keys": list( - self.recalibration_task_keys - ), - "command_names": list(_command_names(self)), - # Retained only as an old-session serialization token. Live - # decisions use the typed motion/measurement policies. - "capabilities": sorted( - getattr( - getattr( - getattr(self, "calibration_profile", None), - "artifacts", - None, - ), - "session_compatibility_tokens", - frozenset(self.profile.capabilities), - ) - ), - "reference_finger": self.profile.reference_finger, - "view_tags": { - view: dict(tags) - for view, tags in self.profile.view_tags.items() - }, - "palm_axis_observers": _palm_axis_observer_schema( - self.profile - ), - "tag_family": "36h11", - "tag_size_m": float(self.tag_size_m), - "tag_sizes_m_by_id": { - str(tag_id): float(self.tag_sizes_m_by_id[tag_id]) - for tag_id in sorted(self.tag_sizes_m_by_id) - }, - "baseline_command_u8": [ - int(value) for value in self.baseline_command - ], - "camera_extrinsics_file": str(self.camera_extrinsics_file), - "source_urdf_path": str(self.source_urdf_path), - "source_urdf_sha256": _file_sha256(self.source_urdf_path), - "curve_input_domain": ( - "requested_command_u8" - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else "command_u8" - ), - "baseline_hysteresis_source": ( - "dedicated_mid_sweep_hold" - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else "trajectory_endpoint" - ), - "directional_zero_policy": ( - "canonical_decreasing_255_to_127" - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else "independent_direction_rezero" - ), - "thumb_ip_pnp_coupling": { - "usage": "candidate_prior_with_visual_fallback", - }, - "cross_view_roll_diagnostic_finger": ( - self.cross_view_roll_diagnostic_finger - ), - "training_cycles": list(range(self.repetitions - 1)), - "validation_cycle": self.repetitions - 1, - "adaptive_formal_speed": { - "enabled": bool(self.adaptive_formal_speed_enabled), - "maximum_scale": float( - self.adaptive_formal_speed_max_scale - ), - "minimum_bins": int( - self.adaptive_formal_speed_minimum_bins - ), - "maximum_bin_gap": int( - self.adaptive_formal_speed_maximum_bin_gap - ), - }, - }, - ) - try: - if self.resume_raw_samples_path is not None: - if not self.resume_raw_samples_path.is_file(): - raise RuntimeError( - "resume raw samples do not exist: " - f"{self.resume_raw_samples_path}" - ) - self.resume_source_session = ( - self.resume_raw_samples_path.parent.name - ) - self.base_import_progress = { - "phase": "waiting_for_new_start_pose_reference", - "records_read": 0, - "bytes_read": 0, - "total_bytes": int( - self.resume_raw_samples_path.stat().st_size - ), - "fraction": 0.0, - } - except Exception as error: - self.started = False - self.resume_checkpoint_pending = False - response.success = False - response.message = f"CFG-RESUME-009:{error}" - return response - self._begin_return_baseline("startup_tag_preflight") - response.success = True - response.message = ( - "three-camera calibration started" - if not self.resume_checkpoint_pending - else ( - "three-camera calibration started; checkpoint will be " - "verified after the new start-pose reference is locked" - ) - ) - return response - - def _pause_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state in {STATE_COMPLETE, STATE_ABORTED, STATE_PREFLIGHT, STATE_WAIT_START}: - response.success = False - response.message = f"cannot pause from {self.state}" - return response - self._pause("operator_pause") - response.success = True - response.message = "calibration paused and current pose held" - return response - - def _resume_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_PAUSED: - response.success = False - response.message = "calibration is not paused" - return response - if self.paused_reason == "zero_model_validation_failed": - response.success = False - response.message = ( - "zero/URDF model validation failed; samples are retained but " - "resume cannot repair this result by rescanning. Inspect the " - "reported zero guards and start a new session after correcting " - "the camera/tag/URDF geometry" - ) - return response - if self.paused_reason == "fixed_base_reference_moved": - response.success = False - response.message = ( - "the locked top base reference moved during acquisition; " - "start a new session so preflight can establish a new " - "reference before any motion" - ) - return response - if self.paused_reason == "cross_view_roll_diagnostic_complete": - response.success = False - response.message = ( - "cross-view roll diagnostic is complete and URDF publication " - "is locked; inspect the front/side result and start a formal " - "session only after choosing the mechanical or vision remedy" - ) - return response - now = time.monotonic() - if not self._resume_preflight_ready(now): - response.success = False - required = ",".join(self._active_views_for_resume()) - response.message = ( - f"resume preflight is not ready; required_views={required}" - ) - return response - for view in self._active_views_for_resume(): - runtime = self.views[view] - self._reset_view_trackers(runtime) - runtime.pnp_invalid_since = None - self.sweep_frames.clear() - self.sweep_start_frames.clear() - self.validation_frames_buffer.clear() - if self.retry_sweep_spec is not None: - self._prepare_failed_sweep_retry() - self._begin_return_baseline("resume_sweep") - response.success = True - response.message = ( - "calibration resumed; only the failed six-direction joint " - "sweep will restart" - ) - return response - if self.active_sweep is not None and self.sweep_index < len(self.sweep_items): - self._begin_return_baseline("resume_sweep") - elif self.active_combination_validation is not None: - item = self.active_combination_validation - active = { - "kind": "combination_validation", - "pose_name": item.name, - "label_zh": item.label_zh, - "pose_index": self.combination_validation_index + 1, - "pose_total": len(self.combination_validation_items), - "requested_command_u8": list(item.command_u8), - "valid_frames_by_view": { - view: len(frames) - for view, frames in self.combination_validation_frames_buffer.items() - }, - "automatic_retry_count": self.validation_retry_counts.get( - ("combination", self.combination_validation_index), 0 - ), - "automatic_retry_limit": self.automatic_sweep_retry_limit, - } - elif self.active_validation is not None: - self._begin_return_baseline("resume_validation") - else: - self._begin_return_baseline("next_sweep") - response.success = True - response.message = "calibration resumed; active step will restart" - return response - - def _abort_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - self._publish_speed_profile(self._normal_speed_profile()) - unsafe_to_return = bool( - self.motion_stall_details - or "stall" in str(self.reason) - or len(self.latest_state_u8) != 20 - or time.monotonic() - self.last_state_at > 1.0 - ) - if unsafe_to_return: - self._publish_hold_current() - self.state = STATE_ABORTED - self.reason = "operator_abort_held_current_for_safety" - else: - self.abort_original_reason = str(self.reason or "operator_abort") - self._begin_return_baseline("abort") - response.success = True - response.message = ( - "calibration aborted and current pose held" - if unsafe_to_return - else "calibration stopping after safe baseline return" - ) - return response - - def _publish_command(self, values: list[int]) -> None: - if not self.commands_enabled: - return - message = JointState() - message.header.stamp = self.get_clock().now().to_msg() - message.name = list(_command_names(self)) - message.position = [float(value) for value in values] - self.command_publisher.publish(message) - - def _publish_speed_profile(self, speeds: list[int]) -> None: - if not self.commands_enabled: - return - profile = tuple(int(value) for value in speeds) - if profile == self.commanded_speed_profile: - return - message = String() - message.data = json.dumps( - { - "setting_cmd": "set_speed", - "params": { - "hand_type": self.hand_type, - "speed": list(profile), - }, - } - ) - self.setting_publisher.publish(message) - self.commanded_speed_profile = profile - self.speed_commanded_at = time.monotonic() - - def _normal_speed_profile(self) -> list[int]: - return [self.normal_calibration_speed] * 5 - - def _transition_speed_profile( - self, target_command: Sequence[int] - ) -> list[int]: - """Use conservative joint-class speeds for one safe pose waypoint.""" - speeds = list(self._normal_speed_profile()) - current = getattr(self, "latest_state_u8", ()) - if len(current) != 20 or len(target_command) != 20: - return speeds - roll_speed = int( - getattr(self, "index_roll_calibration_speed", min(speeds)) - ) - flex_speed = int( - getattr(self, "index_flex_calibration_speed", min(speeds)) - ) - for motor, (actual, target) in enumerate( - zip(current, target_command) - ): - if abs(float(actual) - float(target)) <= 0.5: - continue - if motor in range(6, 10): - slot = motor - 5 - speeds[slot] = min(speeds[slot], roll_speed) - elif motor in range(1, 5): - slot = motor - speeds[slot] = min(speeds[slot], flex_speed) - elif motor in range(16, 20): - slot = motor - 15 - speeds[slot] = min(speeds[slot], flex_speed) - return speeds - - @staticmethod - def _speed_slot_for_spec(spec: SweepSpec) -> int: - motor = int(spec.motor_index) - if motor in {0, 5, 10, 15}: - return 0 - if motor in range(1, 5): - return motor - if motor in range(6, 10): - return motor - 5 - if motor in range(16, 20): - return motor - 15 - raise ValueError(f"unsupported G20 calibration motor: {motor}") - - def _base_speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return build_calibration_speed_profile( - spec, - normal_speed=self.normal_calibration_speed, - index_roll_speed=self.index_roll_calibration_speed, - index_flex_speed=self.index_flex_calibration_speed, - profile=profile, - ) - - def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: - speeds = G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( - self, spec - ) - active = getattr(self, "active_sweep", None) - precheck = bool(active is not None and active.precheck) - fit_retry = bool(getattr(self, "active_sweep_is_fit_retry", False)) - if not precheck and not fit_retry: - scale = float( - getattr(self, "formal_speed_scales", {}).get(spec.key, 1.0) - ) - slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(spec) - speeds[slot] = min( - 255, int(math.floor(float(speeds[slot]) * scale + 1e-9)) - ) - retry = 0 - if active is not None: - key = ( - _sweep_storage_key(active.spec), - active.cycle, - active.direction, - ) - retry = self.sweep_retry_counts.get(key, 0) - if retry: - scale = self.calibration_engine.retry_speed(1.0, 2) - speeds = [ - max(self.retry_minimum_speed, int(round(speed * scale))) - for speed in speeds - ] - return speeds - - def _record_precheck_speed_metric( - self, - item: SweepItem, - *, - bin_count: int, - maximum_bin_gap: int, - valid_frames: int, - ) -> None: - """Use the accepted low-speed precheck to bound formal-scan speed.""" - key = ( - _sweep_storage_key(item.spec), item.cycle, item.direction - ) - retry = int(getattr(self, "sweep_retry_counts", {}).get(key, 0)) - base_speeds = ( - G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( - self, item.spec - ) - ) - actual_speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec( - self, item.spec - ) - slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(item.spec) - base_speed = int(base_speeds[slot]) - actual_speed = int(actual_speeds[slot]) - acquisition_scale = ( - 1.0 - if base_speed <= 0 - else float(actual_speed) / float(base_speed) - ) - metrics = getattr(self, "precheck_speed_metrics", None) - if metrics is None: - self.precheck_speed_metrics = {} - metrics = self.precheck_speed_metrics - by_direction = metrics.setdefault(item.spec.key, {}) - by_direction[item.direction] = { - "bin_count": int(bin_count), - "maximum_bin_gap": int(maximum_bin_gap), - "valid_frames": int(valid_frames), - "retry": retry, - "acquisition_speed": actual_speed, - "acquisition_speed_scale": acquisition_scale, - } - required = { - DIRECTION_DECREASING, - DIRECTION_INCREASING, - } - if not required.issubset(by_direction): - return - - profile = _node_profile(self) - configured_enabled = bool( - getattr(self, "adaptive_formal_speed_enabled", False) - ) - # G20 right repeatability is evaluated across independent sessions. - # Letting camera-dependent precheck density alter the physical scan - # speed changes velocity lag and backlash, so two runs of the same hand - # no longer measure the same process. Keep the reviewed base speed for - # every product task; adaptive speed remains available to legacy tools. - stability_speed_lock = profile.layout_id == G20_RIGHT_19_LAYOUT - enabled = configured_enabled and not stability_speed_lock - # MCP roll is both the fastest native mechanism and the one guarded by - # the strict 0.5-degree baseline backlash limit. Field data at speed - # 7 exceeded that limit after a clean speed-5 precheck, so sampling - # density alone is not sufficient evidence to accelerate roll. - eligible = int(item.spec.motor_index) not in range(6, 10) - selected_scale = 1.0 - if enabled and eligible and base_speed > 0: - caps: list[float] = [] - target_bins = int(self.adaptive_formal_speed_minimum_bins) - target_gap = int(self.adaptive_formal_speed_maximum_bin_gap) - for direction in sorted(required): - metric = by_direction[direction] - observed_scale = float(metric["acquisition_speed_scale"]) - caps.append( - observed_scale - * float(metric["bin_count"]) - / float(target_bins) - ) - caps.append( - observed_scale - * float(target_gap) - / float(max(1, int(metric["maximum_bin_gap"]))) - ) - selected_scale = max( - 1.0, - min(float(self.adaptive_formal_speed_max_scale), *caps), - ) - selected_speed = min( - 255, - int(math.floor(float(base_speed) * selected_scale + 1e-9)), - ) - if selected_speed <= base_speed or base_speed <= 0: - selected_speed = base_speed - selected_scale = 1.0 - else: - selected_scale = float(selected_speed) / float(base_speed) - scales = getattr(self, "formal_speed_scales", None) - if scales is None: - self.formal_speed_scales = {} - scales = self.formal_speed_scales - scales[item.spec.key] = selected_scale - raw_path = getattr(self, "raw_path", None) - if raw_path is not None: - append_jsonl( - raw_path, - { - "kind": "task_formal_speed_selected", - "task_name": item.spec.key, - "motor_index": int(item.spec.motor_index), - "enabled": enabled, - "configured_enabled": configured_enabled, - "stability_speed_lock": stability_speed_lock, - "eligible": eligible, - "ineligible_reason": ( - "g20_right_deterministic_acquisition_speed" - if stability_speed_lock - else "roll_baseline_hysteresis_sensitive" - if configured_enabled and not eligible - else "" - ), - "base_speed": base_speed, - "formal_speed": selected_speed, - "formal_speed_scale": round(selected_scale, 6), - "minimum_retained_bins": int( - self.adaptive_formal_speed_minimum_bins - ), - "maximum_retained_bin_gap": int( - self.adaptive_formal_speed_maximum_bin_gap - ), - "precheck": { - direction: dict(by_direction[direction]) - for direction in sorted(required) - }, - }, - ) - - def _active_endpoint_hold_seconds(self) -> float: - if self.active_sweep is None: - return float(self.endpoint_hold_seconds) - if self.active_sweep.precheck: - return max( - float(self.endpoint_hold_seconds), - float(self.task_precheck_hold_seconds), - ) - key = ( - _sweep_storage_key(self.active_sweep.spec), - self.active_sweep.cycle, - self.active_sweep.direction, - ) - retry = self.sweep_retry_counts.get(key, 0) - if retry <= 0: - return float(self.endpoint_hold_seconds) - return float( - self.retry_endpoint_hold_seconds[ - min(retry, len(self.retry_endpoint_hold_seconds)) - 1 - ] - ) - - def _publish_hold_current(self) -> None: - if len(self.latest_state_u8) != 20: - return - values = [ - int(np.clip(np.rint(value), 0, 255)) - for value in self.latest_state_u8 - ] - self._publish_command(values) - - def _snap_reached_state_to_command( - self, - current_state: Sequence[float], - target_command: Sequence[int], - ) -> tuple[int, ...]: - """Remove no-op transition steps already inside feedback tolerance.""" - if len(current_state) != 20 or len(target_command) != 20: - raise ValueError("state and target command must contain 20 values") - result = [ - int(np.clip(np.rint(value), 0, 255)) for value in current_state - ] - controlled = { - int(spec.motor_index) for spec in self.profile.joint_specs.values() - } - for index in controlled: - target = int(target_command[index]) - if abs(float(current_state[index]) - target) <= ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, target - ) - ): - result[index] = target - return tuple(result) - - def _begin_return_baseline(self, after: str) -> None: - self.baseline_after = str(after) - if str(after) not in {"next_cycle", "resume_sweep"}: - # A mechanical task boundary is also the PnP ownership boundary. - # Same-task cycle resets and localized fit recovery retain it. - self.pnp_task_spec = None - target_command = ( - G20ThreeCameraCalibrationNode._return_command_for_transition( - self, after - ) - ) - current_state = getattr(self, "latest_state_u8", ()) - if len(current_state) != 20: - current_state = target_command - else: - current_state = self._snap_reached_state_to_command( - current_state, target_command - ) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - self.return_waypoints = deque( - build_calibration_return_waypoints( - target_command, - current_command=current_state, - profile=profile, - anchor_roll_motor=( - G20ThreeCameraCalibrationNode._return_anchor_roll_motor( - self - ) - ), - parallel=self.parallel_pose_transitions, - ) - ) - self.return_command_u8 = self.return_waypoints.popleft() - self.position_hold_since = None - self.motion_stage_started_at = time.monotonic() - getattr(self, "motion_stall_details", {}).clear() - self._reset_motion_progress( - self.motion_stage_started_at, self._baseline_error_u8() - ) - self.state = STATE_RETURN_BASELINE - self.reason = ( - "holding_same_finger_clearance_before_next_task" - if str(after) == "next_task_same_finger" - else f"return_baseline_before_{after}" - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.return_command_u8 - ) - ) - self._publish_command(list(self.return_command_u8)) - - def _return_anchor_roll_motor(self) -> int | None: - """Resolve the finger that must leave a fan pose first.""" - retry_items = getattr(self, "retry_sweep_items", []) - item = retry_items[0] if retry_items else None - if item is None: - items = getattr(self, "sweep_items", []) - index = int(getattr(self, "sweep_index", len(items))) - if index < len(items): - item = items[index] - if item is None: - item = getattr(self, "active_sweep", None) - if item is None: - return None - motor = int(item.spec.motor_index) - if motor in range(6, 10): - return motor - if motor in range(1, 5): - return motor + 5 - if motor in range(16, 20): - return motor - 10 - return None - - @staticmethod - def _four_finger_task_group(spec: SweepSpec) -> str | None: - """Return the finger whose consecutive tasks share one avoidance pose.""" - for joint_name in spec.joints: - finger = str(joint_name).split("_", 1)[0] - if finger in {"pinky", "ring", "middle", "index"}: - return finger - return None - - def _next_pending_sweep_item(self) -> SweepItem | None: - retry_items = getattr(self, "retry_sweep_items", []) - if retry_items: - return retry_items[0] - items = getattr(self, "sweep_items", []) - index = int(getattr(self, "sweep_index", len(items))) - return items[index] if index < len(items) else None - - def _advance_past_resumed_sweeps(self) -> None: - """Skip every independently revalidated task restored from disk.""" - completed = set(getattr(self, "resumed_task_keys", ()) or ()) - items = getattr(self, "sweep_items", []) - while ( - self.sweep_index < len(items) - and items[self.sweep_index].spec.key in completed - ): - self.sweep_index += 1 - - def _transition_after_completed_spec(self, spec: SweepSpec) -> str: - """Keep clearance parked between consecutive tasks of one finger.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item(self) - current_group = ( - G20ThreeCameraCalibrationNode._four_finger_task_group(spec) - ) - next_group = ( - None - if next_item is None - else G20ThreeCameraCalibrationNode._four_finger_task_group( - next_item.spec - ) - ) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and current_group is not None - and current_group == next_group - ): - return "next_task_same_finger" - return "next_sweep" - - def _return_command_for_transition(self, after: str) -> tuple[int, ...]: - """Choose a global or task-local safe transition target.""" - if str(after) in {"next_cycle", "next_task_same_finger"}: - next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( - self - ) - if next_item is None: - return tuple(self.baseline_command) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return tuple( - build_calibration_motion_command( - next_item.spec, - int(self.baseline_command[next_item.spec.motor_index]), - baseline=self.baseline_command, - profile=profile, - ) - ) - if str(after) == "next_sweep": - merged = ( - G20ThreeCameraCalibrationNode._cross_group_return_command( - self - ) - ) - if merged is not None: - return merged - if str(after) not in {"retry_sweep", "resume_sweep"}: - return tuple(self.baseline_command) - retry_items = getattr(self, "retry_sweep_items", []) - item = retry_items[0] if retry_items else getattr(self, "active_sweep", None) - if item is None: - return tuple(self.baseline_command) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - # A four-finger retry immediately repeats the same task. Preserve its - # reviewed occlusion-clearance pose and return only to the queued - # direction start; a global baseline would unfold the parked fingers - # and make the next preparation flex them again. The retry setup has - # already reset every contributing PnP tracker. - keep_task_clearance = bool( - item.spec.motor_index == 10 - or ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and G20ThreeCameraCalibrationNode._four_finger_task_group( - item.spec - ) - is not None - ) - ) - if not keep_task_clearance: - return tuple(self.baseline_command) - return tuple( - build_calibration_motion_command( - item.spec, - item.start_u8, - baseline=self.baseline_command, - profile=profile, - ) - ) - - def _cross_group_return_command(self) -> tuple[int, ...] | None: - """Keep already-positioned clearance motors through a finger change. - - Crossing from one finger's task group to the next used to unfold every - auxiliary motor back to baseline and then re-flex it for the next - avoidance pose. Motors the next group still wants at a non-baseline - value and that are already in place (within their endpoint deadband) - now simply stay put; everything else still returns to baseline, so the - reviewed roll-before-unfold and open-before-flex orderings keep - holding and only genuinely unused clearance unfolds. - """ - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return None - next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( - self - ) - if next_item is None: - return None - current_state = getattr(self, "latest_state_u8", ()) - if len(current_state) != 20: - return None - baseline = [int(value) for value in self.baseline_command] - # Match what the next preparation will actually command: the measured - # motor enters at its sweep start, not at the baseline value. - next_start = int( - getattr( - next_item, - "start_u8", - baseline[int(next_item.spec.motor_index)], - ) - ) - next_final = [ - int(value) - for value in build_calibration_motion_command( - next_item.spec, - next_start, - baseline=baseline, - profile=profile, - ) - ] - controlled = { - int(spec.motor_index) for spec in profile.joint_specs.values() - } - target = list(baseline) - for motor in sorted(controlled): - next_value = next_final[motor] - if next_value == baseline[motor]: - continue - actual = float(current_state[motor]) - tolerance = ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, motor, next_value - ) - ) - if abs(actual - next_value) <= tolerance: - target[motor] = next_value - return tuple(target) - - def _current_return_command(self) -> tuple[int, ...]: - command = getattr(self, "return_command_u8", self.baseline_command) - return tuple(int(value) for value in command) - - def _baseline_reached(self) -> bool: - if len(self.latest_state_u8) != 20: - return False - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return_command = G20ThreeCameraCalibrationNode._current_return_command( - self - ) - indices = sorted( - {spec.motor_index for spec in profile.joint_specs.values()} - ) - return all( - abs(float(self.latest_state_u8[index]) - return_command[index]) - <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(return_command[index]) - ) - for index in indices - ) - - def _baseline_error_details(self) -> dict[str, float | int | str]: - if len(self.latest_state_u8) != 20: - return { - "stage": "return_baseline", - "motor_index": -1, - "target_u8": float("nan"), - "actual_u8": float("nan"), - "error_u8": float("inf"), - "tolerance_u8": float(self.endpoint_tolerance_u8), - } - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return_command = G20ThreeCameraCalibrationNode._current_return_command( - self - ) - indices = sorted({spec.motor_index for spec in profile.joint_specs.values()}) - values: list[dict[str, float | int | str]] = [] - for index in indices: - target = float(return_command[index]) - actual = float(self.latest_state_u8[index]) - tolerance = ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(target) - ) - ) - values.append( - { - "stage": "return_baseline", - "motor_index": int(index), - "target_u8": target, - "actual_u8": actual, - "error_u8": abs(actual - target), - "tolerance_u8": tolerance, - } - ) - outside = [ - item - for item in values - if float(item["error_u8"]) > float(item["tolerance_u8"]) - ] - candidates = outside or values - return max( - candidates, - key=lambda item: ( - float(item["error_u8"]) - float(item["tolerance_u8"]), - float(item["error_u8"]), - ), - ) - - def _baseline_error_u8(self) -> float: - if len(self.latest_state_u8) != 20: - return float("inf") - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return_command = G20ThreeCameraCalibrationNode._current_return_command( - self - ) - indices = {spec.motor_index for spec in profile.joint_specs.values()} - return max( - abs( - float(self.latest_state_u8[index]) - - float(return_command[index]) - ) - for index in indices - ) - - def _command_vector_reached(self, command: Sequence[int]) -> bool: - if len(self.latest_state_u8) != 20 or len(command) != 20: - return False - controlled = { - int(spec.motor_index) for spec in self.profile.joint_specs.values() - } - return all( - abs(float(self.latest_state_u8[index]) - float(command[index])) - <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(command[index]) - ) - for index in controlled - ) - - def _command_vector_error_u8(self, command: Sequence[int]) -> float: - if len(self.latest_state_u8) != 20 or len(command) != 20: - return float("inf") - controlled = { - int(spec.motor_index) for spec in self.profile.joint_specs.values() - } - return max( - abs(float(self.latest_state_u8[index]) - float(command[index])) - for index in controlled - ) - - def _command_vector_error_details( - self, command: Sequence[int], context: str - ) -> dict[str, float | int | str]: - """Identify the actual controlled motor blocking one safe waypoint.""" - if len(self.latest_state_u8) != 20 or len(command) != 20: - return { - "stage": str(context), - "motor_index": -1, - "target_u8": float("nan"), - "actual_u8": float("nan"), - "error_u8": float("inf"), - "tolerance_u8": float(self.endpoint_tolerance_u8), - } - controlled = sorted( - {int(spec.motor_index) for spec in self.profile.joint_specs.values()} - ) - values: list[dict[str, float | int | str]] = [] - for index in controlled: - target = float(command[index]) - actual = float(self.latest_state_u8[index]) - tolerance = G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(target) - ) - values.append( - { - "stage": str(context), - "motor_index": index, - "target_u8": target, - "actual_u8": actual, - "error_u8": abs(actual - target), - "tolerance_u8": float(tolerance), - } - ) - outside = [ - item - for item in values - if float(item["error_u8"]) > float(item["tolerance_u8"]) - ] - return max( - outside or values, - key=lambda item: ( - float(item["error_u8"]) - float(item["tolerance_u8"]), - float(item["error_u8"]), - ), - ) - - def _motion_command_reached( - self, - spec: SweepSpec, - command_u8: int, - state_u8: tuple[float, ...] | None = None, - ) -> bool: - """Check the swept motor and all clearance motors together.""" - state = self.latest_state_u8 if state_u8 is None else state_u8 - if len(state) != 20: - return False - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=profile, - ) - indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec, profile=profile)) - return all( - abs(float(state[index]) - expected[index]) - <= ( - self._endpoint_tolerance_for_spec(spec, command_u8) - if index == spec.motor_index - else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, expected[index] - ) - ) - for index in indices - ) - - def _steady_checkpoint_reached( - self, - spec: SweepSpec, - command_u8: int, - state_u8: tuple[float, ...] | None = None, - ) -> bool: - """Accept a settled command/feedback offset without hiding a stall. - - The checkpoint exists specifically to identify requested-command to - feedback/angle offsets, so applying the ordinary ±2-u8 equality gate - here is circular. Only the swept motor gets the bounded calibration - deadband; every clearance motor keeps its normal strict tolerance. - """ - state = self.latest_state_u8 if state_u8 is None else state_u8 - if len(state) != 20: - return False - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - swept_tolerance = max( - float(self.steady_checkpoint_command_feedback_tolerance_u8), - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - self, spec, command_u8 - ), - ) - if ( - abs(float(state[spec.motor_index]) - expected[spec.motor_index]) - > swept_tolerance - ): - return False - return all( - abs(float(state[index]) - expected[index]) - <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, expected[index] - ) - for index in calibration_auxiliary_commands( - spec, profile=self.profile - ) - ) - - def _steady_checkpoint_feedback_is_stable( - self, - spec: SweepSpec, - frames: Sequence[FrameObservation], - ) -> bool: - """Require the held feedback range to be small before recording it.""" - if len(frames) < 3: - return False - values = [ - float(frame.state_u8[spec.motor_index]) for frame in frames - ] - return bool( - max(values) - min(values) - <= float(self.steady_checkpoint_maximum_feedback_range_u8) - ) - - def _motion_command_error_u8( - self, - spec: SweepSpec, - command_u8: int, - state_u8: tuple[float, ...] | None = None, - ) -> float: - """Return the worst swept/auxiliary motor error for stall detection.""" - state = self.latest_state_u8 if state_u8 is None else state_u8 - if len(state) != 20: - return float("inf") - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=profile, - ) - indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec, profile=profile)) - return max( - abs(float(state[index]) - float(expected[index])) - for index in indices - ) - - def _motion_command_error_details( - self, - spec: SweepSpec, - command_u8: int, - context: str, - ) -> dict[str, float | int | str]: - """Identify the swept or clearance motor blocking a motion.""" - if len(self.latest_state_u8) != 20: - return { - "stage": str(context), - "motor_index": -1, - "target_u8": float(command_u8), - "actual_u8": float("nan"), - "error_u8": float("inf"), - "tolerance_u8": float(self.endpoint_tolerance_u8), - } - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=profile, - ) - indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec, profile=profile)) - values: list[dict[str, float | int | str]] = [] - for index in indices: - target = float(expected[index]) - actual = float(self.latest_state_u8[index]) - tolerance = ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - self, spec, command_u8 - ) - if index == spec.motor_index - else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(target) - ) - ) - values.append( - { - "stage": str(context), - "motor_index": int(index), - "target_u8": target, - "actual_u8": actual, - "error_u8": abs(actual - target), - "tolerance_u8": float(tolerance), - } - ) - outside = [ - item - for item in values - if float(item["error_u8"]) > float(item["tolerance_u8"]) - ] - return max( - outside or values, - key=lambda item: ( - float(item["error_u8"]) - float(item["tolerance_u8"]), - float(item["error_u8"]), - ), - ) - - def _reset_motion_progress(self, now: float, error_u8: float) -> None: - self.motion_progress_reference_error_u8 = float(error_u8) - self.motion_last_progress_at = float(now) - - def _pause_if_motion_stalled( - self, - *, - now: float, - error_u8: float, - context: str, - details: Mapping[str, Any] | None = None, - ) -> bool: - """Pause immediately when feedback stops moving toward its target.""" - error = float(error_u8) - if not math.isfinite(error): - return False - reference = float( - getattr(self, "motion_progress_reference_error_u8", float("inf")) - ) - last_progress = float(getattr(self, "motion_last_progress_at", now)) - minimum_progress = float( - getattr(self, "motor_stall_minimum_progress_u8", 1.0) - ) - if not math.isfinite(reference): - self._reset_motion_progress(now, error) - return False - if reference - error >= minimum_progress: - self._reset_motion_progress(now, error) - return False - motion_started_value = getattr(self, "motion_stage_started_at", None) - startup_grace = float( - getattr(self, "motor_stall_startup_grace_seconds", 1.0) - ) - if ( - motion_started_value is not None - and now - float(motion_started_value) < startup_grace - ): - return False - timeout = float(getattr(self, "motor_stall_timeout_seconds", 8.0)) - if now - last_progress < timeout: - return False - detail_payload = {} if details is None else dict(details) - reason_parts = ["motor_state_stalled", str(context)] - for key in ("motor_index", "target_u8", "actual_u8", "tolerance_u8"): - if key in detail_payload: - reason_parts.append(f"{key}={detail_payload[key]}") - reason_parts.append(f"timeout_seconds={timeout:.3f}") - reason_parts.append(f"error_u8={error:.3f}") - reason = ":".join(reason_parts) - self.motion_stall_details = { - "kind": "motion_stall", - "stage": str(context), - "error_u8": error, - "timeout_seconds": timeout, - **detail_payload, - } - append_jsonl( - self.raw_path, - { - "kind": "mechanical_motion_stall", - "context": context, - "error_u8": error, - "timeout_seconds": timeout, - "minimum_progress_u8": minimum_progress, - "state_u8": [float(value) for value in self.latest_state_u8], - "details": detail_payload, - }, - ) - self._pause(reason) - return True - - def _active_sweep_timeout_seconds(self) -> float: - """Scale retry timeout so a deliberately slower retry can finish.""" - if self.active_sweep is None: - return float(self.sweep_timeout_seconds) - key = ( - _sweep_storage_key(self.active_sweep.spec), - self.active_sweep.cycle, - self.active_sweep.direction, - ) - retry = self.sweep_retry_counts.get(key, 0) - if retry <= 0: - return float(self.sweep_timeout_seconds) - # 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 - ) -> float: - """Return the measured feedback deadband for one sweep endpoint.""" - if spec.motor_index == 10 and int(endpoint_u8) == 0: - return float( - getattr( - self, - "thumb_yaw_zero_endpoint_tolerance_u8", - self.endpoint_tolerance_u8, - ) - ) - if spec.motor_index == 19 and int(endpoint_u8) == 0: - return float( - getattr( - self, - "pinky_pip_zero_endpoint_tolerance_u8", - self.endpoint_tolerance_u8, - ) - ) - return G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, spec.motor_index, int(endpoint_u8) - ) - - def _synchronised_endpoint_tolerance_for_spec( - self, spec: SweepSpec, endpoint_u8: int - ) -> float: - """Return the endpoint bin deadband for image-timestamped feedback. - - Motion completion deliberately keeps the physical endpoint deadband. - A synchronised image observation uses interpolated feedback at the - camera timestamp and can trail that latest feedback by a fraction of - one u8 count. One additional count prevents a reached endpoint from - being discarded solely at this continuous-to-integer bin boundary. - """ - mechanical_tolerance = self._endpoint_tolerance_for_spec( - spec, endpoint_u8 - ) - return mechanical_tolerance + float( - getattr(self, "synchronised_endpoint_tolerance_margin_u8", 1.0) - ) - - def _motor_endpoint_tolerance( - self, motor_index: int, endpoint_u8: int - ) -> float: - """Return a side/motor-specific feedback deadband when established.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if int(motor_index) == 10 and int(endpoint_u8) == 0: - return float( - getattr( - self, - "thumb_yaw_zero_endpoint_tolerance_u8", - 4.0, - ) - ) - if ( - profile.side == "right" - and int(motor_index) == 10 - and int(endpoint_u8) == 255 - ): - return float( - getattr( - self, - "right_thumb_yaw_255_endpoint_tolerance_u8", - 5.0, - ) - ) - if ( - profile.side == "right" - and int(motor_index) == 19 - and int(endpoint_u8) == 0 - ): - return float( - getattr( - self, - "pinky_pip_zero_endpoint_tolerance_u8", - 5.0, - ) - ) - return float(self.endpoint_tolerance_u8) - - def _requires_mid_sweep_baseline_hold(self, item: SweepItem) -> bool: - """Return whether this formal sweep crosses a non-endpoint zero. - - The four MCP-roll motors use command 127 as zero. Their zero-pose - hysteresis must be measured after settling at 127 from each direction, - not inferred from frames captured while the motor is moving through - 127. Endpoint-zero joints already have static start/end holds. - """ - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if ( - profile.layout_id != G20_RIGHT_19_LAYOUT - or item.precheck - or item.cycle < 0 - ): - return False - baseline = int(self.baseline_command[item.spec.motor_index]) - return baseline not in {item.start_u8, item.target_u8} - - def _start_next_sweep(self) -> None: - carried_start_frames = list( - getattr(self, "carried_sweep_start_frames", ()) - ) - if hasattr(self, "carried_sweep_start_frames"): - self.carried_sweep_start_frames.clear() - retry_items = getattr(self, "retry_sweep_items", []) - if retry_items: - self.active_sweep = retry_items.pop(0) - self.active_sweep_is_fit_retry = True - else: - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) - if self.sweep_index >= len(self.sweep_items): - self.active_sweep = None - self._begin_return_baseline("fit") - return - self.active_sweep = self.sweep_items[self.sweep_index] - self.active_sweep_is_fit_retry = False - item = self.active_sweep - profile = _node_profile(self) - if profile.layout_id == G20_RIGHT_19_LAYOUT: - self.pnp_task_spec = item.spec - is_fit_retry = bool( - getattr(self, "active_sweep_is_fit_retry", False) - ) - if _requires_pnp_tracker_reset_for_sweep( - profile, - item, - is_fit_retry=is_fit_retry, - preserve_retry_continuity=bool( - getattr(self, "retry_preserve_pnp_continuity", False) - ), - ): - reset_trackers = getattr(self, "_reset_view_trackers", None) - preserve_task_reference = _preserve_pnp_task_reference_for_sweep( - item, - is_fit_retry=is_fit_retry, - has_precheck_anchor=( - profile.layout_id == G20_RIGHT_19_LAYOUT - ), - ) - reset_views = set(_sweep_views(profile, item.spec)) - observer = next( - ( - candidate - for candidate in profile.palm_axis_observers - if candidate.task_name == item.spec.key - ), - None, - ) - if observer is not None: - reset_views.add(observer.view) - if is_fit_retry: - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - reset_views = { - _node_profile(self).record_specs[name].view - for name in scoped - } - reset_views.discard(None) - if observer is not None: - reset_views.add(observer.view) - for view in reset_views: - runtime = getattr(self, "views", {}).get(view) - if runtime is None or reset_trackers is None: - continue - # A normal G20-right task enters here only for its first - # decreasing precheck. All formal cycles keep this exact - # frame-to-frame branch instead of independently selecting a - # new stable IPPE solution. A fit retry deliberately starts - # fresh but retains the task-relative anchor. - if preserve_task_reference: - reset_trackers( - runtime, preserve_task_reference=True - ) - else: - reset_trackers(runtime) - self._reset_view_pnp_diagnostics(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - raw_path = getattr(self, "raw_path", None) - if raw_path is not None: - append_jsonl( - raw_path, - { - "kind": "pnp_task_initialization", - "view": view, - "task_name": item.spec.key, - "motor_index": item.spec.motor_index, - "cycle": item.cycle, - "fit_retry": is_fit_retry, - "task_reference_preserved": ( - preserve_task_reference - ), - }, - ) - getattr(self, "sweep_frames", []).clear() - getattr(self, "sweep_baseline_frames", []).clear() - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since = None - getattr(self, "sweep_start_frames", []).clear() - getattr(self, "sweep_start_frames", []).extend(carried_start_frames) - self.sweep_detection_total_frames = 0 - self.sweep_detection_valid_frames = 0 - self.sweep_detection_total_by_view = {} - self.sweep_detection_valid_by_view = {} - self.position_hold_since = None - self.motion_stage_started_at = time.monotonic() - current_state = getattr(self, "latest_state_u8", ()) - if len(current_state) != 20: - current_state = self.baseline_command - else: - preparation_target = build_calibration_motion_command( - item.spec, - item.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - current_state = self._snap_reached_state_to_command( - current_state, preparation_target - ) - self.preparation_waypoints = deque( - build_calibration_preparation_waypoints( - item.spec, - item.start_u8, - current_command=current_state, - baseline=self.baseline_command, - profile=self.profile, - parallel=self.parallel_pose_transitions, - ) - ) - self.preparation_command_u8 = self.preparation_waypoints.popleft() - self._reset_motion_progress( - self.motion_stage_started_at, - self._command_vector_error_u8(self.preparation_command_u8), - ) - self.state = STATE_PREPARE_SWEEP - self.reason = ( - f"prepare_{item.spec.view}_motor_{item.spec.motor_index}_" - f"cycle_{item.cycle}_{item.direction}" - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.preparation_command_u8 - ) - ) - self._publish_command(list(self.preparation_command_u8)) - - def _stage_immediate_reverse_start_frames( - self, completed: SweepItem, following: SweepItem - ) -> int: - """Carry a proven terminal endpoint into an immediate reverse pass.""" - carried = getattr(self, "carried_sweep_start_frames", None) - if carried is None: - self.carried_sweep_start_frames = [] - carried = self.carried_sweep_start_frames - carried.clear() - if ( - completed.spec != following.spec - or completed.target_u8 != following.start_u8 - or completed.direction == following.direction - ): - return 0 - tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, following.spec, following.start_u8 - ) - ) - limit_per_view = max(1, int(getattr(self, "preflight_frames", 30))) - selected: list[FrameObservation] = [] - for view in _sweep_views(_node_profile(self), following.spec): - view_frames = [ - frame - for frame in self.sweep_frames - if frame.view == view - and abs( - float(frame.state_u8[following.spec.motor_index]) - - following.start_u8 - ) - <= tolerance - ] - selected.extend(view_frames[-limit_per_view:]) - selected.sort(key=lambda frame: frame.stamp_ns) - carried.extend(selected) - if selected and getattr(self, "raw_path", None) is not None: - append_jsonl( - self.raw_path, - { - "kind": "sweep_endpoint_frames_carried", - "task_name": completed.spec.key, - "from_cycle": completed.cycle, - "from_direction": completed.direction, - "to_cycle": following.cycle, - "to_direction": following.direction, - "endpoint_u8": following.start_u8, - "valid_frames": len(selected), - "valid_frames_by_joint": { - name: len(_frames_for_joint(selected, name)) - for name in following.spec.joints - }, - }, - ) - return len(selected) - - def _begin_active_sweep(self, now: float) -> None: - assert self.active_sweep is not None - self.sweep_frames.clear() - self.sweep_frames.extend(self.sweep_start_frames) - self.sweep_start_frames.clear() - if not hasattr(self, "sweep_baseline_frames"): - self.sweep_baseline_frames = [] - self.sweep_baseline_frames.clear() - self.sweep_baseline_pending = ( - G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( - self, self.active_sweep - ) - ) - self.sweep_baseline_hold_since = None - self.sweep_started_at = now - self.sweep_last_valid_at = now - capture_views = _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - self.sweep_last_valid_at_by_view = { - view: now for view in capture_views - } - self.sweep_endpoint_since = None - self.sweep_detection_total_frames = 0 - self.sweep_detection_valid_frames = 0 - self.sweep_detection_total_by_view = { - view: 0 for view in capture_views - } - self.sweep_detection_valid_by_view = { - view: 0 for view in capture_views - } - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - self.sweep_checkpoint_commands = deque( - _steady_checkpoint_commands(profile, self.active_sweep) - ) - self.sweep_checkpoint_mode = bool(self.sweep_checkpoint_commands) - self.sweep_checkpoint_target_u8 = None - self.sweep_checkpoint_hold_since = None - if not hasattr(self, "sweep_checkpoint_frames"): - self.sweep_checkpoint_frames = [] - self.sweep_checkpoint_frames.clear() - if self.sweep_checkpoint_commands: - # PREPARE_SWEEP already held the start endpoint long enough to be - # a steady command-domain observation. - if hasattr(self, "command_records_by_joint"): - self._record_command_checkpoint( - self.active_sweep, - self.active_sweep.start_u8, - list(self.sweep_frames), - ) - self._reset_motion_progress( - now, - self._motion_command_error_u8( - self.active_sweep.spec, self.active_sweep.target_u8 - ), - ) - self.state = STATE_SWEEP - self.reason = ( - "collecting_dedicated_baseline_hold" - if self.sweep_baseline_pending - else "collecting_timestamp_synchronised_tag_centres" - ) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - target_u8 = ( - int(self.baseline_command[self.active_sweep.spec.motor_index]) - if self.sweep_baseline_pending - else ( - self.sweep_checkpoint_commands.popleft() - if self.sweep_checkpoint_commands - else self.active_sweep.target_u8 - ) - ) - if not self.sweep_baseline_pending and self.sweep_checkpoint_mode: - self.sweep_checkpoint_target_u8 = int(target_u8) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - target_u8, - baseline=self.baseline_command, - profile=profile, - ) - ) - - def _record_command_checkpoint( - self, - item: SweepItem, - requested_command_u8: int, - frames: Sequence[FrameObservation], - ) -> None: - """Persist one steady requested-command/feedback/vision observation.""" - motor = int(item.spec.motor_index) - record_joint_names = tuple(item.spec.joints) - if getattr(self, "active_sweep_is_fit_retry", False): - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - record_joint_names = tuple( - name for name in item.spec.joints if name in scoped - ) - for joint_name in record_joint_names: - selected = _frames_for_joint(frames, joint_name) - if len(selected) < 3: - raise RuntimeError( - f"steady command checkpoint {joint_name} has fewer than 3 frames" - ) - feedback = float( - np.median([float(frame.state_u8[motor]) for frame in selected]) - ) - state = np.median( - np.asarray( - [frame.state_u8 for frame in selected], dtype=float - ), - axis=0, - ) - durable = canonical_sample_record({ - "kind": "steady_command_sample", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": self.profile.record_specs[joint_name].view, - "joint": joint_name, - "motor_index": motor, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": int(requested_command_u8), - "feedback_u8": round(feedback, 6), - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in selected]) - ), - "relative_quaternion_xyzw": [ - float(value) - for value in robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[joint_name] - for frame in selected - ] - )[0] - ], - "relative_translation_xyz_m": [ - float(value) - for value in np.median( - np.asarray( - [ - frame.joint_vectors_xyz_m[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - ], - "image_relative_xy_px": [ - float(value) - for value in np.median( - np.asarray( - [ - frame.image_vectors_xy_px[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - ], - "parent_pose_common": _robust_pose_payload( - [frame.parent_poses_common[joint_name] for frame in selected] - ), - "child_pose_common": _robust_pose_payload( - [frame.child_poses_common[joint_name] for frame in selected] - ), - "state_u8": [float(value) for value in state], - "valid_frames": len(selected), - }) - record = fitting_sample_record(durable) - self.command_records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, durable) - - def _publish_next_checkpoint(self, now: float) -> bool: - """Advance a first-round steady scan; return False at its endpoint.""" - if not self.sweep_checkpoint_commands: - self.sweep_checkpoint_target_u8 = None - # The final checkpoint is also the normal sweep endpoint. Leave - # checkpoint mode here so _advance can run its common endpoint - # hold/completion path on this and subsequent timer ticks. Keeping - # the mode latched made the next tick treat the intentionally - # cleared target as an internal error. - self.sweep_checkpoint_mode = False - return False - assert self.active_sweep is not None - target = int(self.sweep_checkpoint_commands.popleft()) - self.sweep_checkpoint_target_u8 = target - self.sweep_checkpoint_hold_since = None - self.sweep_checkpoint_frames.clear() - self.sweep_endpoint_since = None - self.motion_stage_started_at = now - self._reset_motion_progress( - now, self._motion_command_error_u8(self.active_sweep.spec, target) - ) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - target, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - return True - - def _sweep_spec_start_index(self, spec: SweepSpec) -> int: - return next( - index - for index, item in enumerate(self.sweep_items) - if item.spec == spec - ) - - def _fit_joint_records( - self, - joint_name: str, - records: list[dict[str, Any]], - *, - relaxed: bool = False, - ) -> JointCurveFit: - del relaxed - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_command = int( - self.baseline_command[ - profile.record_specs[joint_name].motor_index - ] - ) - model_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, joint_name, records - ) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and ( - joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS - or ( - joint_name.endswith("_mcp_roll") - and not joint_name.startswith("thumb_") - ) - ) - ): - return fit_joint_image_curve( - model_records, - maximum_radial_rms_px=( - self.image_trajectory_maximum_radial_rms_px - ), - maximum_radial_p95_px=( - self.image_trajectory_maximum_radial_p95_px - ), - minimum_radius_px=self.image_trajectory_minimum_radius_px, - minimum_arc_rad=self.trajectory_minimum_arc_rad, - ) - return fit_rotation_joint_curve( - model_records, - zero_command_u8=zero_command, - canonical_zero_direction=canonical_zero_direction( - profile, joint_name - ), - ) - - def _hysteresis_axis_for_fit( - self, - joint_name: str, - records: Sequence[Mapping[str, Any]], - fit: JointCurveFit, - ) -> Sequence[float]: - """Return a physical rotation axis for directional zero checks. - - End-on finger flexion uses its much more stable projected circle for - the dynamic curve. Baseline hysteresis is still a pose-domain check, - so fit a separate diagnostic rotation axis instead of accidentally - treating the image circle as a 3-D pose model. - """ - axis = fit.circle.get("axis_xyz") - if axis is not None: - return axis - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_command = int( - self.baseline_command[ - profile.record_specs[joint_name].motor_index - ] - ) - diagnostic_fit = fit_rotation_joint_curve( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, joint_name, records - ), - zero_command_u8=zero_command, - canonical_zero_direction=canonical_zero_direction( - profile, joint_name - ), - ) - return diagnostic_fit.circle["axis_xyz"] - - def _records_with_baseline_holds( - self, - joint_name: str, - records: Sequence[Mapping[str, Any]], - ) -> list[dict[str, Any]]: - """Add settled mid-range zero poses to directional model fitting.""" - samples = [dict(record) for record in records] - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if canonical_zero_direction(profile, joint_name) is None: - return samples - included_groups = { - (int(record["cycle"]), str(record["direction"])) - for record in samples - } - holds = getattr(self, "baseline_records_by_joint", {}).get( - joint_name, () - ) - samples.extend( - dict(record) - for record in holds - if ( - int(record["cycle"]), str(record["direction"]) - ) in included_groups - ) - return samples - - def _fit_axis_measurement_raw( - self, joint_name: str, cycle: int - ) -> JointAxisMeasurement: - constraint: Sequence[float] | None = None - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - upstream_joint = { - # These neighbouring axes are parallel in the fixed source URDF. - # A small planar Tag's monocular PnP orientation can have a stable - # field-dependent bias, so use the well-observed upstream axis - # direction and let each Tag-centre trajectory independently fit - # its physical axis line. This changes no URDF geometry and is - # valid only because the source-URDF axes are deliberately locked. - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - **{ - f"{finger}_pip": f"{finger}_mcp_pitch" - for finger in ("index", "middle", "ring", "pinky") - }, - **{ - f"{finger}_dip": f"{finger}_pip" - for finger in ("index", "middle", "ring", "pinky") - }, - }.get(joint_name) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and joint_name.endswith("_mcp_roll") - and not joint_name.endswith("_mcp_roll_side") - and not joint_name.startswith("thumb_") - ): - reference_roll = f"{profile.reference_finger}_mcp_roll" - if joint_name != reference_roll: - # The four finger roll axes are exactly parallel in the - # product URDF. The reference-finger task is acquired first, - # so reuse its measured common-frame direction for the other - # three fingers. This removes session-to-session planar-PnP - # branch tilt without changing any fitted axis-line position. - upstream_joint = reference_roll - if upstream_joint is not None: - # Resolve recursively so thumb_ip receives the already constrained - # thumb_mcp direction (and index_dip the constrained index_pip - # direction), rather than reintroducing the raw PnP orientation at - # the last passive joint. - upstream = ( - G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, upstream_joint, cycle - ) - ) - constraint = upstream.axis_common_xyz - spec = profile.record_specs[joint_name] - extrinsics = getattr(self, "extrinsics", None) - view_normal = None - if extrinsics is not None: - view_transform = extrinsics.transform(spec.view) - view_normal = view_transform[:3, :3] @ np.asarray( - [0.0, 0.0, 1.0], dtype=float - ) - measurement_records = ( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, - joint_name, - self.records_by_joint[joint_name], - ) - ) - measurement = fit_joint_axis_measurement( - joint_name, - measurement_records, - cycle=cycle, - zero_command_u8=int( - self.baseline_command[spec.motor_index] - ), - axis_common_constraint=constraint, - constrained_circle_joints=( - zero_profile.constrained_circle_joints - | ({joint_name} if joint_name.endswith("_side") else set()) - ), - view_normal_common_xyz=view_normal, - canonical_zero_direction=canonical_zero_direction( - profile, joint_name - ), - ) - sweep_spec = next( - sweep - for sweep in profile.sweep_specs - if joint_name in sweep.joints - ) - condition_command = build_calibration_motion_command( - sweep_spec, - int(self.baseline_command[spec.motor_index]), - baseline=self.baseline_command, - profile=profile, - ) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - measurement = replace( - measurement, - condition_command_u8=tuple( - float(value) for value in condition_command - ), - ) - if extrinsics is None: - return measurement - return with_depth_free_axis_projection( - replace( - measurement, - view_normal_common_xyz=tuple( - float(value) for value in view_normal - ), - ), - extrinsics.transform(spec.view)[:3, 3], - ) - - def _fit_axis_measurement( - self, joint_name: str, cycle: int - ) -> JointAxisMeasurement: - """Fit one axis, fusing a separately accepted cross-view estimate.""" - primary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, joint_name, cycle - ) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - validation_sources = profile.axis_validation_sources or {} - validation_name = validation_sources.get(joint_name) - if validation_name is None: - return primary - validation_records = self.records_by_joint.get(validation_name, []) - if not any( - int(record.get("cycle", -1)) == int(cycle) - for record in validation_records - ): - # Front roll is provisionally checked before the independent side - # task has been acquired. Fusion is allowed only in the final fit - # after both complete raw datasets have independently passed. - return primary - secondary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, validation_name, cycle - ) - primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) - secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) - if float(primary_axis @ secondary_axis) < 0.0: - secondary_axis = -secondary_axis - axis_difference = math.acos( - float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) - ) - primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) - secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) - line_distance = float( - np.linalg.norm( - np.cross(secondary_point - primary_point, primary_axis) - ) - ) - validation_spec = profile.record_specs[validation_name] - if validation_spec.zero_kind == "axis_cross_view_validation": - # This Tag is attached to the downstream PIP link. During MCP - # roll its orientation contains linkage/pitch coupling and is not - # an independent observation of the published MCP angle or axis - # direction. It remains the physical axis-line-position source; - # its own radial, line-repeatability, visibility and trajectory - # quality gates are all applied before reaching this point. - decision = ( - "diagnostic_only_gross_pose_disagreement_use_primary" - if axis_difference - > getattr( - self, - "cross_view_roll_maximum_axis_difference_rad", - math.radians(15.0), - ) - else "diagnostic_only_pose_disagreement_use_primary" - ) - G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( - self, - joint_name, - cycle, - primary=primary, - secondary=secondary, - axis_difference_rad=axis_difference, - line_distance_m=line_distance, - decision=decision, - ) - extrinsics = getattr(self, "extrinsics", None) - if extrinsics is None or validation_spec.view is None: - raise ValueError( - f"{validation_name}: cross-view camera geometry is unavailable" - ) - primary_view = profile.record_specs[joint_name].view - if primary_view is None: - raise ValueError( - f"{joint_name}: primary point source has no view" - ) - point_camera_center = extrinsics.transform(primary_view)[:3, 3] - point_ray = primary_point - point_camera_center - interpretation_plane_normal = np.cross( - point_ray, primary_axis - ) - interpretation_plane_normal /= np.linalg.norm( - interpretation_plane_normal - ) - return replace( - primary, - point_common_xyz_m=tuple( - float(value) for value in primary.point_common_xyz_m - ), - axis_point_source=( - "front_interpretation_plane_cross_view_validated" - ), - pose_axis_line_rms_m=primary.pose_axis_line_rms_m, - pose_axis_line_source_joints=(joint_name,), - axis_point_camera_center_common_xyz_m=tuple( - float(value) - for value in point_camera_center - ), - axis_point_interpretation_plane_normal_common_xyz=tuple( - float(value) - for value in interpretation_plane_normal - ), - ) - # Monocular planar-tag orientation on the side view carries a - # systematic line-of-sight IPPE bias of several degrees through roll - # sweeps (tags tilted ~13-20 deg from the ray), which sub-pixel - # reprojection cannot expose. Session 20260820_105535 showed a - # stable 11.4 deg front/side disagreement from exactly this effect, - # so sub-degree cross-view agreement is not achievable at this - # geometry. Disagreement above the fusion gate now falls back to the - # trusted front-only axis with a recorded diagnostic instead of - # failing the joint; only a gross direction error (wrong-link or a - # loose Tag that changes the observed motion axis) still fails here. - gross_axis_limit = getattr( - self, - "cross_view_roll_maximum_axis_difference_rad", - math.radians(15.0), - ) - # Do not apply a universal front/side line-distance gate here. The - # front roll link rides the splay screw, so its pure-revolute pose fit - # is a displaced pseudo-line whose offset varies by finger (about - # 15 mm for pinky and 37 mm for ring on the same fixed setup). It is - # not the physical line ultimately published. Retain the distance in - # diagnostics; validate the selected side physical line through its - # own radial quality, cycle RMS and final URDF geometry instead. - if axis_difference > gross_axis_limit: - raise ValueError( - "cross_view_roll_axis_gross_disagreement:" - f"{math.degrees(axis_difference):.6f}deg," - f"{1000.0 * line_distance:.6f}mm" - ) - if ( - axis_difference > self.zero_maximum_axis_cycle_difference_rad - or line_distance > self.axis_maximum_pose_line_rms_m - ): - selected_direction = primary - decision = "skip_fusion_use_primary" - zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - observer_name = next( - ( - observer - for observer, parent in zero_profile.axis_parent_joint.items() - if parent == joint_name - ), - None, - ) - observer_records = ( - [] - if observer_name is None - else self.records_by_joint.get(observer_name, []) - ) - if observer_records: - model = UrdfKinematicModel(self.source_urdf_path) - parent_axis, _ = model.axis_line( - joint_name, zero_offsets={}, joint_angles={} - ) - observer_axis, _ = model.axis_line( - observer_name, zero_offsets={}, joint_angles={} - ) - expected_cone = math.acos( - abs( - float( - np.clip(parent_axis @ observer_axis, -1.0, 1.0) - ) - ) - ) - - def cone_residual( - candidate_axis: np.ndarray, - measured_observer_axis: np.ndarray, - ) -> float: - measured_cone = math.acos( - abs( - float( - np.clip( - candidate_axis @ measured_observer_axis, - -1.0, - 1.0, - ) - ) - ) - ) - return abs(measured_cone - expected_cone) - - # A zero rotates the downstream axis around this parent and - # cannot change their mutual cone angle. Select the camera - # once for the complete repeated-sweep group. Per-cycle - # selection is invalid because a residual close to the gate - # can alternate views and turn their fixed systematic bias - # into a false cycle-axis spread. - group_cycles = tuple(range(int(self.repetitions))) - required_names = ( - joint_name, - validation_name, - observer_name, - ) - group_is_complete = all( - any( - int(record.get("cycle", -1)) == group_cycle - for record in self.records_by_joint.get(name, ()) - ) - for name in required_names - for group_cycle in group_cycles - ) - group_source = "primary" - if group_is_complete: - group_signature = tuple( - ( - name, - group_cycle, - len(cycle_records), - max( - ( - int(record.get("attempt", 0)) - for record in cycle_records - ), - default=0, - ), - max( - ( - int(record.get("image_stamp_ns", 0)) - for record in cycle_records - ), - default=0, - ), - ) - for name in required_names - for group_cycle in group_cycles - for cycle_records in ( - [ - record - for record in self.records_by_joint.get( - name, () - ) - if int(record.get("cycle", -1)) - == group_cycle - ], - ) - ) - cache_key = ( - joint_name, - validation_name, - observer_name, - group_signature, - ) - source_cache = getattr( - self, "_cross_view_roll_group_source_cache", {} - ) - cached_source = source_cache.get(cache_key) - if cached_source is not None: - group_source = str(cached_source) - else: - primary_residuals: list[float] = [] - secondary_residuals: list[float] = [] - fit_raw = ( - G20ThreeCameraCalibrationNode - ._fit_axis_measurement_raw - ) - try: - for group_cycle in group_cycles: - group_primary = fit_raw( - self, joint_name, group_cycle - ) - group_secondary = fit_raw( - self, validation_name, group_cycle - ) - group_observer = fit_raw( - self, observer_name, group_cycle - ) - observer_axis_common = np.asarray( - group_observer.axis_common_xyz, - dtype=float, - ) - primary_residuals.append( - cone_residual( - np.asarray( - group_primary.axis_common_xyz, - dtype=float, - ), - observer_axis_common, - ) - ) - secondary_residuals.append( - cone_residual( - np.asarray( - group_secondary.axis_common_xyz, - dtype=float, - ), - observer_axis_common, - ) - ) - except (KeyError, ValueError): - primary_residuals = [] - secondary_residuals = [] - group_source = ( - select_cross_view_roll_direction_source( - primary_residuals, - secondary_residuals, - self.zero_maximum_axis_cone_mismatch_rad, - ) - ) - source_cache[cache_key] = group_source - if len(source_cache) > 32: - source_cache.pop(next(iter(source_cache))) - self._cross_view_roll_group_source_cache = source_cache - if group_source == "secondary": - selected_direction = replace( - primary, - axis_common_xyz=tuple( - float(value) for value in secondary_axis - ), - axis_direction_source=( - "cross_view_cone_selected_secondary" - ), - ) - decision = ( - "use_secondary_zero_invariant_cone_consensus" - ) - G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( - self, - joint_name, - cycle, - primary=primary, - secondary=secondary, - axis_difference_rad=axis_difference, - line_distance_m=line_distance, - decision=decision, - ) - # The front roll link rides the splay screw: its orientation - # tracks the rotation faithfully, but its centre trajectory - # carries the screw translation, displacing the fitted axis - # line by ~21 mm from the finger's physical MCP axis. The side - # PIP-link circle recovers that physical line. Keep the one - # direction selected consistently for all cycles and take the - # line position from the side view. - return replace( - selected_direction, - point_common_xyz_m=tuple( - float(value) - for value in secondary.point_common_xyz_m - ), - axis_point_source="side_circle_cross_view", - pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, - pose_axis_line_source_joints=(validation_name,), - ) - primary_variance = max( - primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, - 1.0e-12, - ) - secondary_variance = max( - secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, - 1.0e-12, - ) - primary_weight = 1.0 / primary_variance - secondary_weight = 1.0 / secondary_variance - total_weight = primary_weight + secondary_weight - fused_axis = ( - primary_weight * primary_axis - + secondary_weight * secondary_axis - ) - fused_axis /= np.linalg.norm(fused_axis) - # Axis-line points have a free coordinate along the axis. Fuse only - # the observable perpendicular displacement and retain the primary - # point's along-axis gauge. - delta = secondary_point - primary_point - perpendicular_delta = delta - fused_axis * float(delta @ fused_axis) - fused_point = ( - primary_point - + secondary_weight / total_weight * perpendicular_delta - ) - return replace( - primary, - axis_common_xyz=tuple(float(value) for value in fused_axis), - point_common_xyz_m=tuple(float(value) for value in fused_point), - plane_rms_m=max(primary.plane_rms_m, secondary.plane_rms_m), - radial_rms_m=max(primary.radial_rms_m, secondary.radial_rms_m), - pose_axis_line_rms_m=max( - primary.pose_axis_line_rms_m, - secondary.pose_axis_line_rms_m, - line_distance, - ), - pose_axis_line_source_joints=(joint_name, validation_name), - axis_direction_source="cross_view_weighted_fusion", - ) - - def _refit_cross_view_axis_line_group( - self, - joint_name: str, - measurements: Sequence[JointAxisMeasurement], - ) -> list[JointAxisMeasurement]: - """Apply the repeated-cycle physical-radius constraint when eligible.""" - group = list(measurements) - sources = { - source - for measurement in group - if (source := cross_view_side_line_source(measurement)) is not None - } - if ( - len(group) != int(self.repetitions) - or len(sources) != 1 - or not all( - cross_view_side_line_source(measurement) is not None - for measurement in group - ) - or any( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in group - ) - ): - return group - source = next(iter(sources)) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - source_spec = profile.record_specs[source] - source_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, - source, - self.records_by_joint[source], - ) - return list( - refit_axis_line_group_with_shared_radius( - group, - source_records, - zero_command_u8=int( - self.baseline_command[source_spec.motor_index] - ), - canonical_zero_direction=canonical_zero_direction( - profile, source - ), - ) - ) - - def _record_cross_view_roll_axis_diagnostic( - self, - joint_name: str, - cycle: int, - *, - primary: Any, - secondary: Any, - axis_difference_rad: float, - line_distance_m: float, - decision: str = "skip_fusion_use_primary", - ) -> None: - """Log the deterministic cross-view direction decision.""" - record = { - "kind": "cross_view_roll_axis_diagnostic", - "joint": joint_name, - "cycle": int(cycle), - "decision": str(decision), - "axis_difference_deg": round( - math.degrees(axis_difference_rad), 6 - ), - "line_distance_mm": round(1000.0 * line_distance_m, 6), - "primary_axis_common_xyz": [ - round(float(value), 9) for value in primary.axis_common_xyz - ], - "secondary_axis_common_xyz": [ - round(float(value), 9) for value in secondary.axis_common_xyz - ], - "primary_axis_direction_source": primary.axis_direction_source, - } - try: - append_jsonl(self.raw_path, record) - except Exception as error: - logger_factory = getattr(self, "get_logger", None) - if callable(logger_factory): - logger_factory().warning( - f"failed to record cross-view roll diagnostic: {error}" - ) - logger_factory = getattr(self, "get_logger", None) - if callable(logger_factory): - selected_text = ( - "using the side axis selected by the zero-invariant cone" - if decision.startswith( - "use_secondary_zero_invariant_cone" - ) - else "keeping the front-only axis" - ) - logger_factory().warning( - f"{joint_name} cycle {cycle}: front/side roll axes disagree " - f"by {math.degrees(axis_difference_rad):.2f} deg; skipping " - f"fusion and {selected_text}" - ) - - def _provisional_fit_failures( - self, spec: SweepSpec, *, include_view_validity: bool = True - ) -> list[dict[str, Any]]: - """Check full-pose curve and 3-D axis after one six-way task.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - failures: list[dict[str, Any]] = [] - if include_view_validity and hasattr(self, "views"): - for view in _sweep_views(profile, spec): - runtime = self.views[view] - view_joints = _sweep_joints_for_view(profile, spec, view) - validation_only_view = bool(view_joints) and all( - profile.record_specs[name].zero_kind - == "axis_cross_view_validation" - for name in view_joints - ) - retry_scope = set( - getattr(self, "retry_joint_names", set()) - if getattr(self, "retry_sweep_spec", None) == spec - else () - ) - if retry_scope and not retry_scope.intersection(view_joints): - # This view's accepted records were intentionally kept; - # only the independently failed view is being measured. - continue - valid_rate = float(runtime.valid_rate) - # Prefer the task-scoped counters: after the last sweep the - # required-role set flips back to the full preflight set and - # the rolling window restarts on idle frames, which is not - # what a per-task capture-quality gate should measure. - total = int(getattr(runtime, "task_total_frames", 0)) - if total > 0: - valid_rate = ( - float(getattr(runtime, "task_valid_frames", 0)) - / total - ) - imported_without_capture = ( - total == 0 - and spec.key - in set(getattr(self, "resumed_task_keys", ()) or ()) - ) - dense_trajectory_coverage = bool( - profile.layout_id == G20_RIGHT_19_LAYOUT - and _records_have_formal_sweep_coverage( - self.records_by_joint, - view_joints, - repetitions=self.repetitions, - minimum_bins=int( - getattr(self, "minimum_sweep_bins", 32) - ), - maximum_bin_gap=int( - getattr(self, "maximum_bin_gap", 16) - ), - ) - ) - if ( - not imported_without_capture - and valid_rate < self.minimum_detection_rate - and not validation_only_view - and not dense_trajectory_coverage - ): - failures.append( - { - "joint": view_joints[0], - "view": view, - "metric": "tag_valid_rate_percent", - "actual": round(100.0 * valid_rate, 3), - "limit": round( - 100.0 * self.minimum_detection_rate, 3 - ), - "comparison": "minimum", - "task_valid_frames": int( - getattr(runtime, "task_valid_frames", 0) - ), - "task_total_frames": total, - } - ) - for joint_name in spec.joints: - records = self.records_by_joint[joint_name] - sync_p95 = float( - np.percentile( - [ - float(record.get("state_image_sync_error_ms", 0.0)) - for record in records - ], - 95.0, - ) - ) - sync_limit_ms = ( - getattr(self, "maximum_state_image_skew_ns", 50_000_000) - / 1_000_000.0 - ) - if sync_p95 > sync_limit_ms: - failures.append( - { - "joint": joint_name, - "metric": "state_image_sync_p95_ms", - "actual": round(sync_p95, 6), - "limit": round(sync_limit_ms, 6), - "comparison": "maximum", - } - ) - try: - fit = self._fit_joint_records( - joint_name, - records, - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "trajectory_fit", - "reason": str(error), - } - ) - continue - if profile.steady_command_checkpoints: - command_store = getattr( - self, "command_records_by_joint", None - ) - if command_store is not None: - command_records = [ - dict(record) - for record in command_store.get(joint_name, ()) - if int(record.get("cycle", 0)) == 0 - ] - if len(command_records) < 18: - failures.append( - { - "joint": joint_name, - "metric": "steady_command_checkpoints", - "actual": len(command_records), - "limit": 18, - "comparison": "minimum", - } - ) - else: - try: - self._fit_joint_records( - joint_name, command_records - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "command_trajectory_fit", - "reason": str(error), - } - ) - else: - feedback_fit = self._fit_joint_records( - joint_name, - _steady_records_in_feedback_domain( - command_records - ), - ) - command_gap = float( - feedback_fit.maximum_hysteresis_rad - ) - command_gap_limit = float( - self.command_maximum_direction_gap_rad - ) - if command_gap > command_gap_limit: - failures.append( - { - "joint": joint_name, - "metric": ( - "feedback_direction_gap_deg" - ), - "actual": round( - math.degrees(command_gap), 6 - ), - "limit": round( - math.degrees(command_gap_limit), 6 - ), - "comparison": "maximum", - } - ) - try: - baseline_hysteresis = baseline_hysteresis_by_cycle_rad( - G20ThreeCameraCalibrationNode._baseline_hysteresis_records( - self, joint_name, records - ), - zero_command_u8=int( - self.baseline_command[ - profile.record_specs[joint_name].motor_index - ] - ), - axis_xyz=( - G20ThreeCameraCalibrationNode - ._hysteresis_axis_for_fit( - self, joint_name, records, fit - ) - ), - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "baseline_hysteresis", - "reason": str(error), - } - ) - continue - maximum_baseline_hysteresis = max(baseline_hysteresis) - canonical_direction = canonical_zero_direction( - profile, joint_name - ) - if canonical_direction is not None: - validation_only_pose = ( - profile.record_specs[joint_name].zero_kind - == "axis_cross_view_validation" - ) - branch_gap_range = ( - maximum_baseline_hysteresis - - min(baseline_hysteresis) - ) - branch_gap_limit = getattr( - self, - "directional_zero_maximum_branch_gap_rad", - math.radians(1.5), - ) - branch_gap_range_limit = getattr( - self, - "directional_zero_maximum_branch_gap_range_rad", - math.radians(0.3), - ) - if maximum_baseline_hysteresis > branch_gap_limit: - failures.append( - { - "joint": joint_name, - "metric": "baseline_directional_gap_deg", - "actual": round( - math.degrees( - maximum_baseline_hysteresis - ), - 6, - ), - "limit": round( - math.degrees(branch_gap_limit), 6 - ), - "comparison": "maximum", - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - } - ) - if ( - branch_gap_range > branch_gap_range_limit - and not validation_only_pose - ): - failures.append( - { - "joint": joint_name, - "metric": ( - "baseline_directional_gap_range_deg" - ), - "actual": round( - math.degrees(branch_gap_range), 6 - ), - "limit": round( - math.degrees(branch_gap_range_limit), 6 - ), - "comparison": "maximum", - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - } - ) - elif ( - branch_gap_range > branch_gap_range_limit - and validation_only_pose - ): - # This side-view alias is never published as a joint - # angle. Its orientation is reconstructed from a - # near-grazing planar Tag, so IPPE pose drift can move - # the apparent branch gap between rounds even when - # the image trajectory and the authoritative front - # axis are repeatable. Keep the production threshold - # as an audit reference, but do not rescan identical - # motion based on a non-published pose component. - append_jsonl( - self.raw_path, - { - "kind": "validation_only_quality_diagnostic", - "joint": joint_name, - "metric": ( - "baseline_directional_gap_range_deg" - ), - "actual": round( - math.degrees(branch_gap_range), 6 - ), - "reference_limit": round( - math.degrees(branch_gap_range_limit), 6 - ), - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - "decision": "diagnostic_only", - }, - ) - elif ( - maximum_baseline_hysteresis - > self.baseline_maximum_hysteresis_rad - ): - failures.append( - { - "joint": joint_name, - "metric": "baseline_hysteresis_deg", - "actual": round( - math.degrees(maximum_baseline_hysteresis), 6 - ), - "limit": round( - math.degrees( - self.baseline_maximum_hysteresis_rad - ), - 6, - ), - "comparison": "maximum", - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - } - ) - if fit.circle.get("space") == "image_2d": - checks = ( - ( - "image_radial_rms_px", - float(fit.quality["radial_rms_px"]), - self.image_trajectory_maximum_radial_rms_px, - "maximum", - ), - ( - "image_radial_p95_px", - float(fit.quality["radial_p95_px"]), - self.image_trajectory_maximum_radial_p95_px, - "maximum", - ), - ( - "image_radius_px", - float(fit.quality["radius_px"]), - self.image_trajectory_minimum_radius_px, - "minimum", - ), - ( - "arc_deg", - math.degrees(float(fit.quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) - else: - checks = ( - ( - "rotation_orthogonal_rms_deg", - math.degrees( - float( - fit.quality["rotation_orthogonal_rms_rad"] - ) - ), - math.degrees( - self.active_maximum_rotation_orthogonal_rms_rad - if profile.record_specs[joint_name].active - else self.passive_maximum_rotation_orthogonal_rms_rad - ), - "maximum", - ), - ( - "arc_deg", - math.degrees(float(fit.quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) - joint_spec = profile.record_specs[joint_name] - monotonic_limit = ( - self.maximum_monotonic_correction_rad - if joint_spec.active - else self.passive_maximum_monotonic_correction_rad - ) - hysteresis_limit = ( - self.maximum_hysteresis_rad - if joint_spec.active - else self.passive_maximum_hysteresis_rad - ) - checks = checks + ( - ( - "monotonic_correction_deg", - math.degrees(fit.maximum_monotonic_correction_rad), - math.degrees(monotonic_limit), - "maximum", - ), - ) - if not profile.directional_zero: - checks = checks + ( - ( - "hysteresis_deg", - math.degrees(fit.maximum_hysteresis_rad), - math.degrees(hysteresis_limit), - "maximum", - ), - ) - for metric, actual, limit, comparison in checks: - failed = ( - actual > limit - if comparison == "maximum" - else actual < limit - ) - if failed: - failures.append( - { - "joint": joint_name, - "metric": metric, - "actual": round(float(actual), 6), - "limit": round(float(limit), 6), - "comparison": comparison, - } - ) - - cycle_travels: list[float] = [] - cycle_axes: list[np.ndarray] = [] - cycle_axis_sources: list[str] = [] - cycle_axis_measurements: list[JointAxisMeasurement] = [] - for cycle in range(self.repetitions): - cycle_records = [ - record - for record in self.records_by_joint[joint_name] - if int(record["cycle"]) == cycle - ] - try: - cycle_fit = self._fit_joint_records( - joint_name, cycle_records - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "cycle_fit", - "cycle": cycle + 1, - "reason": str(error), - } - ) - continue - cycle_travels.append( - abs( - float(cycle_fit.angle_rad[0]) - - float(cycle_fit.angle_rad[255]) - ) - ) - try: - axis = self._fit_axis_measurement(joint_name, cycle) - except Exception as error: - reason = str(error) - failure: dict[str, Any] = { - "joint": joint_name, - "metric": "axis_fit", - "cycle": cycle + 1, - "reason": reason, - } - if reason.startswith( - "cross_view_roll_axis_gross_disagreement:" - ): - validation_name = ( - profile.axis_validation_sources or {} - ).get(joint_name) - if validation_name in spec.joints: - # A gross direction disagreement does not identify - # which independently observed view is wrong. A - # retry must therefore reacquire both sources. - failure["quality_source_joints"] = [ - joint_name, - validation_name, - ] - failures.append(failure) - continue - cycle_axes.append(np.asarray(axis.axis_common_xyz, dtype=float)) - cycle_axis_sources.append(axis.axis_direction_source) - cycle_axis_measurements.append(axis) - plane_limit = ( - self.axis_maximum_plane_rms_m - if joint_spec.active - else self.passive_axis_maximum_plane_rms_m - ) - axis_checks = [ - ( - "axis_radial_rms_mm", - 1000.0 * axis.radial_rms_m, - 1000.0 * self.axis_maximum_radial_rms_m, - ), - ] - if ( - joint_spec.zero_kind != "axis_cross_view_validation" - and cross_view_side_line_source(axis) is None - ): - # A cross-view fallback deliberately takes its direction - # from the front and its line position from the side. The - # side Tag's monocular orientation is not the published - # direction, so its per-frame ideal-revolute residual is - # not an accuracy measure for the combined line. Those - # lines are checked for independent-cycle position - # repeatability below, at this same threshold. - axis_checks.append( - ( - "axis_pose_line_rms_mm", - 1000.0 * axis.pose_axis_line_rms_m, - 1000.0 * self.axis_maximum_pose_line_rms_m, - ) - ) - circle_is_constrained = circle_direction_is_constrained( - joint_name, zero_profile.constrained_circle_joints - ) - if not circle_is_constrained: - axis_checks.extend( - [ - ( - "axis_plane_rms_mm", - 1000.0 * axis.plane_rms_m, - 1000.0 * plane_limit, - ), - ] - ) - if not circle_is_constrained: - axis_checks.append( - ( - "rotation_circle_axis_difference_deg", - math.degrees( - axis.rotation_circle_axis_difference_rad - ), - math.degrees( - self.axis_maximum_rotation_circle_difference_rad - ), - ) - ) - # With a trusted orientation-constrained axis, plane_rms is - # scatter *along* that infinite axis. It cannot change the - # axis line or joint zero, so only the perpendicular/radial - # error is an admissibility check for constrained circles. - for metric, actual, limit in axis_checks: - if actual > limit: - if ( - metric == "axis_pose_line_rms_mm" - and not joint_spec.pose_axis_line_required - ): - append_jsonl( - self.raw_path, - { - "kind": ( - "position_invariant_quality_diagnostic" - ), - "task_name": spec.key, - "joint": joint_name, - "cycle": cycle + 1, - "metric": metric, - "actual": round(float(actual), 6), - "reference_limit": round( - float(limit), 6 - ), - "decision": "diagnostic_only", - "authoritative_quality": [ - "tag_pnp", - "image_trajectory", - "relative_rotation", - "synchronisation", - "isolated_holdout", - ], - }, - ) - continue - failure_joint = joint_name - quality_sources: tuple[str, ...] = () - if metric == "axis_pose_line_rms_mm": - quality_sources = tuple( - str(source) - for source in getattr( - axis, - "pose_axis_line_source_joints", - (), - ) - if str(source) in spec.joints - ) - if len(quality_sources) == 1: - failure_joint = quality_sources[0] - failure = { - "joint": failure_joint, - "metric": metric, - "cycle": cycle + 1, - "actual": round(float(actual), 6), - "limit": round(float(limit), 6), - "comparison": "maximum", - } - if quality_sources: - failure["quality_source_joints"] = list( - quality_sources - ) - if failure_joint != joint_name: - failure["model_joint"] = joint_name - failures.append(failure) - side_line_sources = { - source - for measurement in cycle_axis_measurements - if ( - source := cross_view_side_line_source(measurement) - ) is not None - } - if side_line_sources: - try: - cycle_axis_measurements = ( - G20ThreeCameraCalibrationNode - ._refit_cross_view_axis_line_group( - self, joint_name, cycle_axis_measurements - ) - ) - except Exception as error: - source = next(iter(side_line_sources)) - failures.append( - { - "joint": source, - "model_joint": joint_name, - "quality_source_joints": [source], - "metric": "axis_line_shared_radius_fit", - "reason": str(error), - } - ) - side_line_sources = { - source - for measurement in cycle_axis_measurements - if ( - source := cross_view_side_line_source(measurement) - ) is not None - } - if ( - len(cycle_axis_measurements) == self.repetitions - and len(side_line_sources) == 1 - and all( - cross_view_side_line_source(measurement) is not None - for measurement in cycle_axis_measurements - ) - and not any( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in cycle_axis_measurements - ) - ): - line_rms = axis_line_cycle_rms_m( - cycle_axis_measurements - ) - if line_rms > self.axis_maximum_pose_line_rms_m: - source = next(iter(side_line_sources)) - outliers = _isolated_axis_line_cycle_outliers( - cycle_axis_measurements, - self.axis_maximum_pose_line_rms_m, - ) - failure = { - "joint": source, - "model_joint": joint_name, - "quality_source_joints": [source], - "metric": "axis_line_cycle_rms_mm", - "actual": round(1000.0 * line_rms, 6), - "limit": round( - 1000.0 * self.axis_maximum_pose_line_rms_m, - 6, - ), - "maximum_pairwise_mm": round( - 1000.0 - * maximum_axis_line_cycle_spread_m( - cycle_axis_measurements - ), - 6, - ), - "comparison": "maximum", - } - if outliers: - outlier = next(iter(outliers)) - failure["cycle"] = outlier + 1 - failure["inlier_cycles"] = [ - index + 1 - for index in range(len(cycle_axis_measurements)) - if index != outlier - ] - failures.append(failure) - if len(cycle_travels) == self.repetitions: - travel_range = max(cycle_travels) - min(cycle_travels) - cycle_limit = ( - self.trajectory_maximum_cycle_travel_difference_rad - if joint_spec.active - else self.passive_maximum_cycle_travel_difference_rad - ) - if travel_range > cycle_limit: - failures.append( - { - "joint": joint_name, - "metric": "cycle_travel_range_deg", - "actual": round(math.degrees(travel_range), 6), - "limit": round(math.degrees(cycle_limit), 6), - "comparison": "maximum", - "cycle_travel_deg": [ - round(math.degrees(value), 6) - for value in cycle_travels - ], - } - ) - if ( - len(cycle_axes) == self.repetitions - and not all( - source == "upstream_constraint" - for source in cycle_axis_sources - ) - ): - maximum_difference = 0.0 - for left in cycle_axes: - for right in cycle_axes: - maximum_difference = max( - maximum_difference, - math.acos( - abs(float(np.clip(left @ right, -1.0, 1.0))) - ), - ) - if ( - maximum_difference - > self.zero_maximum_axis_cycle_difference_rad - and joint_spec.zero_kind - == "axis_cross_view_validation" - ): - # As above, the side alias' independently reconstructed - # pose axis is not the axis written to the URDF. The - # published front direction and the side translation - # circle retain their own strict cross-round gates. - append_jsonl( - self.raw_path, - { - "kind": "validation_only_quality_diagnostic", - "joint": joint_name, - "metric": "axis_cycle_difference_deg", - "actual": round( - math.degrees(maximum_difference), 6 - ), - "reference_limit": round( - math.degrees( - self.zero_maximum_axis_cycle_difference_rad - ), - 6, - ), - "decision": "diagnostic_only", - }, - ) - elif ( - maximum_difference - > self.zero_maximum_axis_cycle_difference_rad - ): - outliers = _isolated_axis_cycle_outliers( - cycle_axes, - self.zero_maximum_axis_cycle_difference_rad, - ) - failure = { - "joint": joint_name, - "metric": "axis_cycle_difference_deg", - "actual": round(math.degrees(maximum_difference), 6), - "limit": round( - math.degrees( - self.zero_maximum_axis_cycle_difference_rad - ), - 6, - ), - "comparison": "maximum", - } - if outliers: - outlier = next(iter(outliers)) - failure["cycle"] = outlier + 1 - failure["inlier_cycles"] = [ - index + 1 - for index in range(len(cycle_axes)) - if index != outlier - ] - failures.append(failure) - if profile.cross_view_roll_curve: - for primary_name, validation_name in ( - profile.axis_validation_sources or {} - ).items(): - if not {primary_name, validation_name}.issubset(spec.joints): - continue - try: - cycle_fits = [ - ( - self._fit_joint_records( - primary_name, - [ - record - for record in self.records_by_joint[ - primary_name - ] - if int(record.get("cycle", -1)) == cycle - ], - ), - self._fit_joint_records( - validation_name, - [ - record - for record in self.records_by_joint[ - validation_name - ] - if int(record.get("cycle", -1)) == cycle - ], - ), - ) - for cycle in range(self.repetitions) - ] - training_cycles = set(range(max(1, self.repetitions - 1))) - scopes = ( - ( - "training", - [ - record - for record in self.records_by_joint[primary_name] - if int(record.get("cycle", -1)) - in training_cycles - ], - [ - record - for record in self.records_by_joint[ - validation_name - ] - if int(record.get("cycle", -1)) - in training_cycles - ], - ), - ( - "all_cycles", - self.records_by_joint[primary_name], - self.records_by_joint[validation_name], - ), - ) - for scope, primary_records, validation_records in scopes: - failure = _cross_view_curve_failure( - primary_name, - validation_name, - self._fit_joint_records( - primary_name, primary_records - ), - self._fit_joint_records( - validation_name, validation_records - ), - scope=scope, - maximum_rms_difference_rad=( - self.cross_view_roll_maximum_shape_rms_rad - ), - maximum_branch_gap_difference_rad=( - self.cross_view_roll_maximum_branch_gap_difference_rad - ), - allow_projection_scale=True, - maximum_projection_scale_ratio=( - self.cross_view_roll_maximum_projection_scale_ratio - ), - cycle_fits=cycle_fits, - ) - if failure is not None: - append_jsonl( - self.raw_path, - { - "kind": ( - "validation_only_quality_diagnostic" - ), - **failure, - "decision": "diagnostic_only", - }, - ) - # One structured result contains all per-cycle - # evidence; avoid duplicate training/final errors. - break - except Exception as error: - append_jsonl( - self.raw_path, - { - "kind": "validation_only_quality_diagnostic", - "joint": primary_name, - "metric": "cross_view_roll_curve", - "reason": str(error), - "quality_source_joints": [validation_name], - "decision": "diagnostic_only", - }, - ) - return failures - - def _cross_view_roll_diagnostic_role( - self, spec: SweepSpec - ) -> str | None: - finger = str( - getattr(self, "cross_view_roll_diagnostic_finger", "") - ) - if not finger: - return None - if spec.key == f"{finger}_roll_multiview": - return "multiview" - return None - - @staticmethod - def _only_baseline_hysteresis_failures( - failures: Sequence[Mapping[str, Any]], - ) -> bool: - return bool(failures) and all( - str(item.get("metric", "")) - in { - "baseline_hysteresis_deg", - "baseline_directional_gap_deg", - "baseline_directional_gap_range_deg", - } - for item in failures - ) - - def _roll_baseline_hysteresis_degrees( - self, joint_name: str - ) -> list[float]: - records = self.records_by_joint[joint_name] - fit = self._fit_joint_records(joint_name, records) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - spec = profile.record_specs[joint_name] - values = baseline_hysteresis_by_cycle_rad( - G20ThreeCameraCalibrationNode._baseline_hysteresis_records( - self, joint_name, records - ), - zero_command_u8=int( - self.baseline_command[spec.motor_index] - ), - axis_xyz=fit.circle["axis_xyz"], - ) - return [round(math.degrees(value), 6) for value in values] - - def _complete_cross_view_roll_diagnostic( - self, - side_spec: SweepSpec, - side_quality_failures: Sequence[Mapping[str, Any]] = (), - ) -> None: - finger = self.cross_view_roll_diagnostic_finger - front_joint = f"{finger}_mcp_roll" - side_joint = f"{front_joint}_side" - front_values = self._roll_baseline_hysteresis_degrees(front_joint) - side_values = self._roll_baseline_hysteresis_degrees(side_joint) - limit_deg = math.degrees(self.baseline_maximum_hysteresis_rad) - interpretation = _classify_cross_view_roll_hysteresis( - front_values, side_values, limit_deg=limit_deg - ) - result = { - "kind": "cross_view_roll_diagnostic", - "finger": finger, - "motor_index": side_spec.motor_index, - "front_joint": front_joint, - "side_joint": side_joint, - "front_by_cycle_deg": front_values, - "side_by_cycle_deg": side_values, - "front_maximum_deg": max(front_values), - "side_maximum_deg": max(side_values), - "formal_limit_deg": limit_deg, - "interpretation": interpretation, - "side_quality_failures": [ - dict(item) for item in side_quality_failures - ], - "publication_locked": True, - } - self.cross_view_roll_diagnostic_result = result - self.fit_failure = result - append_jsonl(self.raw_path, result) - self._pause("cross_view_roll_diagnostic_complete") - - def _pause_for_provisional_fit_failure( - self, - spec: SweepSpec, - failures: list[dict[str, Any]], - *, - allow_warning: bool = False, - ) -> bool: - diagnostic_role = ( - G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - self, spec - ) - ) - if ( - diagnostic_role == "front" - and G20ThreeCameraCalibrationNode._only_baseline_hysteresis_failures( - failures - ) - ): - append_jsonl( - self.raw_path, - { - "kind": "cross_view_roll_front_failure_deferred", - "finger": self.cross_view_roll_diagnostic_finger, - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "failures": failures, - "publication_locked": True, - }, - ) - self.reason = "cross_view_roll_front_failure_deferred" - return False - - def is_warning(item: Mapping[str, Any]) -> bool: - if "actual" not in item or "limit" not in item: - return False - actual = float(item["actual"]) - limit = float(item["limit"]) - ratio = float(getattr(self, "provisional_warning_ratio", 1.0)) - if limit <= 0.0: - return False - if item.get("comparison") == "minimum": - return actual >= limit / ratio - return actual <= limit * ratio - - if allow_warning and failures and all(is_warning(item) for item in failures): - band_attempt = int( - self.sweep_attempts.get(_sweep_storage_key(spec), 1) - ) - 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 - # collected. Consume every remaining retry here; when the budget - # is exhausted, the common failure path below pauses in place. - append_jsonl( - self.raw_path, - { - "kind": ( - "provisional_fit_warning_rescan" - if band_attempt <= retry_limit - else "provisional_fit_warning_retry_exhausted" - ), - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "attempt": band_attempt, - "attempt_limit": retry_limit + 1, - "warning_ratio": float( - getattr( - self, "provisional_warning_ratio", 1.0 - ), - ), - "failures": failures, - }, - ) - - attempt = self.sweep_attempts.get(_sweep_storage_key(spec), 1) - self.retry_resume_index = self.sweep_index - self.retry_sweep_spec = spec - self.retry_joint_names = _fit_retry_joint_names( - _node_profile(self), spec, failures - ) - source_task_keys = ( - tuple(getattr(self, "retry_source_task_keys", ())) - if getattr(self, "retry_source_failure_task_key", None) == spec.key - else () - ) - source_key_set = set(source_task_keys) - source_specs = [ - candidate - for candidate in _node_profile(self).sweep_specs - if candidate.key in source_key_set - ] - if source_specs: - self.retry_joint_names = { - joint_name - for candidate in source_specs - for joint_name in candidate.joints - } - localized_cycles = { - int(item["cycle"]) - 1 - for item in failures - if item.get("cycle") is not None - } - cycle_override = set( - getattr(self, "retry_cycle_override", set()) - ) - if source_specs and cycle_override: - # These source tasks jointly define one PnP-relative zero. Mixing - # cycles acquired before and after a tracker/reference reset can - # make a clean branch change look like a physical zero change. - # Reacquire the whole source holdout set as one generation. - self.retry_cycles = cycle_override - elif failures and all( - item.get("cycle") is not None - or str(item.get("metric", "")).startswith("third_cycle_") - for item in failures - ): - if any( - str(item.get("metric", "")).startswith("third_cycle_") - for item in failures - ): - localized_cycles.add(self.repetitions - 1) - self.retry_cycles = localized_cycles - else: - self.retry_cycles = set(range(self.repetitions)) - history_by_task = getattr( - self, "fit_failure_history_by_task", None - ) - if history_by_task is None: - self.fit_failure_history_by_task = {} - history_by_task = self.fit_failure_history_by_task - task_history = history_by_task.setdefault(spec.key, []) - repeated_branch_clusters = bool( - task_history - and _fit_failure_repeats_branch_clusters( - task_history[-1], failures - ) - ) - task_history.append([dict(item) for item in failures]) - systematic = bool( - _fit_failure_is_systematic(failures, self.repetitions) - or repeated_branch_clusters - ) - fit_retry_limit = 0 - self.fit_failure = { - "kind": "fit_failure", - "view": spec.view, - "task_name": spec.key, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "joints_to_rescan": sorted(self.retry_joint_names), - **( - {"source_task_names": list(source_task_keys)} - if source_task_keys - else {} - ), - "attempt": attempt, - "fit_attempt": attempt, - "fit_attempt_limit": fit_retry_limit + 1, - "automatic_retry_count": max(0, attempt - 1), - "automatic_retry_limit": fit_retry_limit, - "cycles_to_rescan": [ - cycle + 1 for cycle in sorted(self.retry_cycles) - ], - "directions_to_rescan": ( - 0 - if systematic - else 2 - * len(self.retry_cycles) - * max(1, len(source_specs)) - ), - "recoverable_by_rescan": not systematic, - "repeated_branch_clusters": repeated_branch_clusters, - "failures": failures, - } - append_jsonl( - self.raw_path, - { - "kind": "fit_failure", - **self.fit_failure, - }, - ) - if systematic: - self._pause( - "joint_fit_repeated_branch_failure" - if repeated_branch_clusters - else "joint_fit_systematic_failure" - ) - return True - if attempt <= fit_retry_limit: - self.paused_reason = "joint_fit_check_failed" - self.reason = "automatic_retry_joint_fit_check_failed" - self._prepare_failed_sweep_retry() - self._begin_return_baseline("resume_sweep") - return True - self._pause("joint_fit_check_failed") - return True - - def _pause_for_zero_model_failure( - self, zero_result: ZeroSolveResult - ) -> None: - """Stop once for a model/zero failure; repeated motion cannot fix it.""" - line_errors_m = dict( - getattr(zero_result, "validation_line_error_by_joint_m", {}) - ) - axis_line_rms_m = float(getattr(zero_result, "axis_line_rms_m", 0.0)) - axis_line_limit_m = float( - getattr(self, "axis_maximum_pose_line_rms_m", 0.0) - ) - observability_rank = int(getattr(zero_result, "observability_rank", 0)) - observability_parameter_count = int( - getattr(zero_result, "observability_parameter_count", 0) - ) - observability_condition_number = float( - getattr(zero_result, "observability_condition_number", 0.0) - ) - failures: list[dict[str, Any]] = [] - for name, reason in zero_result.failure_reasons.items(): - failure: dict[str, Any] = { - "joint": name, - "metric": "zero_guard", - "reason": reason, - } - if reason == "zero_offset_exceeds_configured_limit": - configured_limit = self.zero_joint_maximum_offsets_rad.get( - name, - ( - self.zero_finger_maximum_offset_rad - if name.startswith( - ("index_", "middle_", "ring_", "pinky_") - ) - else self.zero_maximum_offset_rad - ), - ) - failure["actual_deg"] = round( - math.degrees(zero_result.direct_offsets_rad[name]), 6 - ) - failure["limit_deg"] = round( - math.degrees(configured_limit), 6 - ) - failures.append(failure) - failures.extend( - { - "joint": observer, - "metric": "third_cycle_axis_holdout_deg", - "actual": round(math.degrees(error), 6), - "limit": math.degrees(self.maximum_validation_p95_rad), - "comparison": "maximum", - } - for observer, error in ( - zero_result.validation_error_by_joint_rad.items() - ) - if error > self.maximum_validation_p95_rad - ) - failures.extend( - { - "joint": observer, - "metric": "validation_axis_line_residual_mm", - "actual": round(1000.0 * float(error), 6), - "limit": round( - 1000.0 * axis_line_limit_m, 6 - ), - "comparison": "maximum", - } - for observer, error in ( - line_errors_m.items() - ) - if error > axis_line_limit_m - ) - direct_joint = next( - iter(zero_result.failure_reasons), - next( - iter(zero_result.validation_error_by_joint_rad), - "thumb_cmc_pitch", - ), - ) - spec = next( - ( - item - for item in self.profile.sweep_specs - if direct_joint in item.joints - ), - self.profile.sweep_specs[0], - ) - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_cycles = set() - self.retry_joint_names = set() - self.fit_failure = { - "kind": "zero_model_failure", - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "directions_to_rescan": 0, - "recoverable_by_rescan": False, - "failures": failures, - "zero_geometry": { - "axis_line_rms_mm": round( - 1000.0 * axis_line_rms_m, 6 - ), - "axis_line_rms_limit_mm": round( - 1000.0 * axis_line_limit_m, 6 - ), - "validation_line_error_by_joint_mm": { - name: round(1000.0 * float(value), 6) - for name, value in ( - line_errors_m.items() - ) - }, - "observability_rank": observability_rank, - "observability_parameter_count": observability_parameter_count, - "observability_condition_number": observability_condition_number, - }, - } - append_jsonl( - self.raw_path, - {"kind": "zero_model_failure", **self.fit_failure}, - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - atomic_write_json( - self.session_dir / "g20_right_19_failed_diagnostics.json", - { - "schema_version": 5, - "layout_id": G20_RIGHT_19_LAYOUT, - "passed": False, - "reason": "zero_model_validation_failed", - "failure_reasons": dict(zero_result.failure_reasons), - "axis_line_rms_m": axis_line_rms_m, - "axis_line_rms_limit_m": axis_line_limit_m, - "validation_line_error_by_joint_m": dict( - line_errors_m - ), - "validation_error_by_joint_rad": dict( - zero_result.validation_error_by_joint_rad - ), - "direct_offsets_rad": dict(zero_result.direct_offsets_rad), - "offset_confidence_95_half_width_rad": dict( - zero_result.offset_confidence_half_width_rad - ), - "observability": { - "rank": observability_rank, - "parameter_count": observability_parameter_count, - "condition_number": observability_condition_number, - }, - "raw_samples": str(self.raw_path), - "source_urdf": str(self.source_urdf_path), - "camera_extrinsics": str( - self.camera_extrinsics_file - ), - "urdf_published": False, - }, - ) - self._pause("zero_model_validation_failed") - - def _retry_active_sweep_or_pause(self, reason: str) -> None: - """Retry a recoverable acquisition failure without weakening gates.""" - if self.active_sweep is None: - self._pause(reason) - return - item = self.active_sweep - key = ( - _sweep_storage_key(item.spec), item.cycle, item.direction - ) - retries = self.sweep_retry_counts.get(key, 0) - retry_limit = 1 - if not CalibrationEngine.permits_retry("sweep_acquisition", retries): - self._pause(reason) - return - retries += 1 - self.sweep_retry_counts[key] = retries - observer = next( - ( - candidate - for candidate in _node_profile(self).palm_axis_observers - if candidate.task_name == item.spec.key - ), - None, - ) - retry_views = set(_sweep_views(_node_profile(self), item.spec)) - if observer is not None: - retry_views.add(observer.view) - for view in retry_views: - runtime = getattr(self, "views", {}).get(view) - if runtime is None: - continue - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 - speed_scales = getattr( - self, "retry_speed_scales", (1.0,) - ) - endpoint_holds = getattr( - 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 - # neither resurrect the rejected sweep nor merge it with its - # replacement acquisition. - append_jsonl( - self.raw_path, - { - "kind": "automatic_sweep_retry", - "task_name": item.spec.key, - "attempt": int( - getattr(self, "sweep_attempts", {}).get( - _sweep_storage_key(item.spec), 1 - ) - ), - "view": item.spec.view, - "motor_index": item.spec.motor_index, - "joints": list(item.spec.joints), - "cycle": item.cycle, - "direction": item.direction, - "retry": retries, - "retry_limit": retry_limit, - "reason": str(reason), - "speed_scale": float( - speed_scales[ - min(retries, len(speed_scales)) - 1 - ] - ), - "endpoint_hold_seconds": float( - endpoint_holds[ - min(retries, len(endpoint_holds)) - 1 - ] - ), - }, - ) - command_records = getattr(self, "command_records_by_joint", {}) - for joint_name in item.spec.joints: - if joint_name in command_records: - command_records[joint_name] = [ - record - for record in command_records[joint_name] - if not ( - str(record.get("task_name")) == item.spec.key - and int(record.get("cycle", -999)) == item.cycle - and str(record.get("direction")) == item.direction - ) - ] - if observer is not None: - records = self.palm_axis_records_by_source.get( - observer.source_name, [] - ) - records[:] = [ - record - for record in records - if not ( - int(record.get("cycle", -999)) == item.cycle - and str(record.get("direction", "")) == item.direction - ) - ] - getattr(self, "sweep_frames", []).clear() - getattr(self, "sweep_start_frames", []).clear() - self.reason = f"automatic_retry_{reason}" - self._begin_return_baseline("retry_sweep") - - def _record_dedicated_baseline_hold(self, item: SweepItem) -> None: - """Persist one settled baseline observation for one approach path.""" - if not G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( - self, item - ): - return - frames = list(getattr(self, "sweep_baseline_frames", [])) - minimum_frames = int(getattr(self, "minimum_baseline_hold_frames", 10)) - motor = item.spec.motor_index - command = int(self.baseline_command[motor]) - attempt = self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) - record_joint_names = tuple(item.spec.joints) - if getattr(self, "active_sweep_is_fit_retry", False): - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - record_joint_names = tuple( - name for name in item.spec.joints if name in scoped - ) - for joint_name in record_joint_names: - selected = _frames_for_joint(frames, joint_name) - if len(selected) < minimum_frames: - raise RuntimeError( - "dedicated baseline hold for " - f"{joint_name} has {len(selected)} valid frames; " - f"at least {minimum_frames} required" - ) - state = np.median( - np.asarray( - [frame.state_u8 for frame in selected], dtype=float - ), - axis=0, - ) - quaternion = robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[joint_name] - for frame in selected - ] - )[0] - relative_translation = np.median( - np.asarray( - [ - frame.joint_vectors_xyz_m[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - image_relative = np.median( - np.asarray( - [ - frame.image_vectors_xy_px[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - durable = canonical_sample_record({ - "kind": "baseline_hold_sample", - "attempt": attempt, - "task_name": item.spec.key, - "view": self.profile.record_specs[joint_name].view, - "joint": joint_name, - "motor_index": motor, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": command, - "feedback_u8": round(float(state[motor]), 6), - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in selected]) - ), - "relative_quaternion_xyzw": [ - float(value) for value in quaternion - ], - "relative_translation_xyz_m": [ - float(value) for value in relative_translation - ], - "image_relative_xy_px": [ - float(value) for value in image_relative - ], - "parent_pose_common": _robust_pose_payload( - [ - frame.parent_poses_common[joint_name] - for frame in selected - ] - ), - "child_pose_common": _robust_pose_payload( - [ - frame.child_poses_common[joint_name] - for frame in selected - ] - ), - "state_u8": [float(value) for value in state], - "state_image_sync_error_ms": round( - float( - np.percentile( - [ - abs(frame.state_sync_error_ns) - for frame in selected - ], - 95.0, - ) - / 1_000_000.0 - ), - 6, - ), - "pnp_reprojection_error_px": round( - float( - np.percentile( - [ - frame.joint_reprojection_error_px[joint_name] - for frame in selected - ], - 95.0, - ) - ), - 6, - ), - "valid_frames": len(selected), - "hold_seconds": float(self.baseline_hold_seconds), - }) - record = fitting_sample_record(durable) - self.baseline_records_by_joint[joint_name].append(record) - if item.cycle == 0: - self.command_records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, durable) - - def _baseline_hysteresis_records( - self, - joint_name: str, - trajectory_records: Sequence[Mapping[str, Any]], - ) -> Sequence[Mapping[str, Any]]: - """Use dedicated settled records for non-endpoint baseline joints.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - spec = profile.record_specs[joint_name] - baseline = int(self.baseline_command[spec.motor_index]) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and baseline not in {0, 255} - ): - return self.baseline_records_by_joint.get(joint_name, []) - return trajectory_records - - def _finish_active_sweep(self) -> None: - assert self.active_sweep is not None - item = self.active_sweep - motor = item.spec.motor_index - start_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, item.spec, item.start_u8 - ) - ) - target_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, item.spec, item.target_u8 - ) - ) - record_joint_names = tuple(item.spec.joints) - if getattr(self, "active_sweep_is_fit_retry", False): - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - record_joint_names = tuple( - name for name in item.spec.joints if name in scoped - ) - - def observation_bins( - joint_name: str, - ) -> dict[int, list[FrameObservation]]: - bins: dict[int, list[FrameObservation]] = {} - frames = _frames_for_joint(self.sweep_frames, joint_name) - for frame in frames: - state = float(frame.state_u8[motor]) - if abs(state - item.start_u8) <= start_tolerance: - command = item.start_u8 - elif abs(state - item.target_u8) <= target_tolerance: - command = item.target_u8 - else: - command = int(np.clip(np.rint(state), 0, 255)) - bins.setdefault(command, []).append(frame) - return bins - - joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} - for joint_name in record_joint_names: - bins = observation_bins(joint_name) - commands = sorted(bins) - if not commands or commands[0] != 0 or commands[-1] != 255: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "sweep_missing_endpoint_bin", item.spec, joint_name - ) - ) - return - joint_bins[joint_name] = bins - if item.precheck: - # This low-speed pass is a visibility/safety check at 0, 127 and - # 255, not a source for the command-angle curve. Its full-sweep - # detection rate is counted directly in the camera callback. A - # raw-frame rate below the nominal threshold is not itself a data - # defect when synchronised valid poses still cover the complete - # trajectory densely: detector flicker then changes only how many - # duplicate observations exist, not what motion is observable. - for joint_name, bins in joint_bins.items(): - commands = sorted(bins) - if min(abs(command - 127) for command in commands) > 2: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "task_precheck_missing_command_127", - item.spec, - joint_name, - ) - ) - return - detection_rates: dict[str, float] = {} - total_by_view = getattr( - self, "sweep_detection_total_by_view", {} - ) - valid_by_view = getattr( - self, "sweep_detection_valid_by_view", {} - ) - capture_views = _sweep_views(_node_profile(self), item.spec) - profile = _node_profile(self) - trajectory_coverage_by_view: dict[str, bool] = {} - for view in capture_views: - view_joint_bins = [ - joint_bins[name] - for name in _sweep_joints_for_view( - profile, item.spec, view - ) - if name in joint_bins - ] - trajectory_coverage_by_view[view] = bool(view_joint_bins) and all( - len(bins) >= self.minimum_sweep_bins - and max(np.diff(sorted(bins)), default=0) - <= self.maximum_bin_gap - for bins in view_joint_bins - ) - detection_rate_below_threshold_views: list[str] = [] - trajectory_coverage_override_views: list[str] = [] - for view in capture_views: - if view in total_by_view: - total = int(total_by_view.get(view, 0)) - valid = int(valid_by_view.get(view, 0)) - elif len(capture_views) == 1: - # Compatibility for legacy sessions and focused unit - # harnesses created before per-camera counters existed. - total = int( - getattr(self, "sweep_detection_total_frames", 0) - ) - valid = int( - getattr(self, "sweep_detection_valid_frames", 0) - ) - else: - total = 0 - valid = 0 - rate = 0.0 if total <= 0 else valid / total - detection_rates[view] = rate - if rate < self.minimum_detection_rate: - detection_rate_below_threshold_views.append(view) - if trajectory_coverage_by_view.get(view, False): - trajectory_coverage_override_views.append(view) - else: - self._retry_active_sweep_or_pause( - f"task_precheck_detection_rate_too_low:{view}" - ) - return - minimum_bin_count = min( - len(bins) for bins in joint_bins.values() - ) - precheck_maximum_bin_gap = max( - int(max(np.diff(sorted(bins)), default=0)) - for bins in joint_bins.values() - ) - minimum_valid_frames = min( - sum(len(frames) for frames in bins.values()) - for bins in joint_bins.values() - ) - G20ThreeCameraCalibrationNode._record_precheck_speed_metric( - self, - item, - bin_count=minimum_bin_count, - maximum_bin_gap=precheck_maximum_bin_gap, - valid_frames=minimum_valid_frames, - ) - append_jsonl( - self.raw_path, - { - "kind": "task_visibility_precheck", - "task_name": item.spec.key, - "view": item.spec.view, - "capture_views": list( - _sweep_views(_node_profile(self), item.spec) - ), - "motor_index": motor, - "direction": item.direction, - "checked_commands_u8": [0, 127, 255], - "bin_count": minimum_bin_count, - "maximum_bin_gap": precheck_maximum_bin_gap, - "detection_rate": round(min(detection_rates.values()), 6), - "detection_rate_by_view": { - view: round(rate, 6) - for view, rate in detection_rates.items() - }, - "minimum_detection_rate": round( - float(self.minimum_detection_rate), 6 - ), - "detection_rate_below_threshold_views": ( - detection_rate_below_threshold_views - ), - "trajectory_coverage_by_view": ( - trajectory_coverage_by_view - ), - "trajectory_coverage_override_views": ( - trajectory_coverage_override_views - ), - "passed": True, - }, - ) - if not getattr(self, "active_sweep_is_fit_retry", False): - self.sweep_index += 1 - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps( - self - ) - self.active_sweep = None - self.active_sweep_is_fit_retry = False - if self.sweep_index >= len(self.sweep_items): - self._begin_return_baseline("fit") - elif self.sweep_items[self.sweep_index].spec == item.spec: - following = self.sweep_items[self.sweep_index] - transition = _sweep_plan_transition( - self.profile, item, following - ) - if transition == "cycle_reset": - self._begin_return_baseline("next_cycle") - else: - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - self, - item, - following, - ) - self._start_next_sweep() - else: - self._begin_return_baseline( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - self, item.spec - ) - ) - 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( - _joint_failure_reason( - "sweep_bins_too_few", item.spec, joint_name - ) - ) - return - if max(np.diff(commands), default=0) > self.maximum_bin_gap: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "sweep_bin_gap_too_large", item.spec, joint_name - ) - ) - return - - stored_joint_names = tuple(joint_bins) - - def synchronised_frame_payload( - frame: FrameObservation, - ) -> dict[str, Any]: - names = [ - name - for name in stored_joint_names - if name in frame.joint_quaternions_xyzw - ] - return { - "kind": "synchronised_frame", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": frame.view, - "motor_index": motor, - "joints": names, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": int( - self.sweep_checkpoint_target_u8 - if self.sweep_checkpoint_target_u8 is not None - else item.target_u8 - ), - "feedback_u8": round(float(frame.state_u8[motor]), 6), - "image_stamp_ns": int(frame.stamp_ns), - "actual_state_u8": [ - float(value) for value in frame.state_u8 - ], - "state_image_sync_error_ms": round( - abs(frame.state_sync_error_ns) / 1_000_000.0, 6 - ), - "relative_quaternion_xyzw": { - name: list(frame.joint_quaternions_xyzw[name]) - for name in names - }, - "parent_pose_common": { - name: frame.parent_poses_common[name] - for name in names - }, - "child_pose_common": { - name: frame.child_poses_common[name] - for name in names - }, - "pnp_reprojection_error_px": { - name: frame.joint_reprojection_error_px[name] - for name in names - }, - } - append_jsonl_many( - self.raw_path, - ( - synchronised_frame_payload(frame) - for frame in self.sweep_frames - if any( - name in frame.joint_quaternions_xyzw - for name in stored_joint_names - ) - ), - ) - - for joint_name in stored_joint_names: - bins = joint_bins[joint_name] - commands = sorted(bins) - for command in commands: - frames = bins[command] - vector = np.median( - np.asarray( - [frame.joint_vectors_xyz_m[joint_name] for frame in frames] - ), - axis=0, - ) - image_vector = np.median( - np.asarray( - [frame.image_vectors_xy_px[joint_name] for frame in frames] - ), - axis=0, - ) - relative_quaternion = robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[joint_name] - for frame in frames - ] - )[0] - parent_pose = _robust_pose_payload( - [ - frame.parent_poses_common[joint_name] - for frame in frames - ] - ) - child_pose = _robust_pose_payload( - [ - frame.child_poses_common[joint_name] - for frame in frames - ] - ) - state = np.median( - np.asarray([frame.state_u8 for frame in frames], dtype=float), - axis=0, - ) - durable = canonical_sample_record({ - "kind": "sample", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": self.profile.record_specs[joint_name].view, - "joint": joint_name, - "motor_index": motor, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": int( - self.sweep_checkpoint_target_u8 - if self.sweep_checkpoint_target_u8 is not None - else item.target_u8 - ), - "feedback_u8": int(command), - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in frames]) - ), - "relative_translation_xyz_m": [ - float(value) for value in vector - ], - "image_relative_xy_px": [ - float(value) for value in image_vector - ], - "relative_quaternion_xyzw": [ - float(value) for value in relative_quaternion - ], - "parent_pose_common": parent_pose, - "child_pose_common": child_pose, - "state_u8": [float(value) for value in state], - "state_image_sync_error_ms": round( - float( - np.percentile( - [ - abs(frame.state_sync_error_ns) - for frame in frames - ], - 95.0, - ) - / 1_000_000.0 - ), - 6, - ), - "pnp_reprojection_error_px": round( - float( - np.percentile( - [ - frame.joint_reprojection_error_px[joint_name] - for frame in frames - ], - 95.0, - ) - ), - 6, - ), - "valid_frames": len(frames), - }) - record = fitting_sample_record(durable) - self.records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, durable) - - G20ThreeCameraCalibrationNode._persist_palm_axis_samples(self, item) - G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold( - self, item - ) - previous_spec = item.spec - was_fit_retry = bool( - getattr(self, "active_sweep_is_fit_retry", False) - ) - if not was_fit_retry: - self.sweep_index += 1 - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) - self.active_sweep = None - self.active_sweep_is_fit_retry = False - if was_fit_retry: - if self.retry_sweep_items: - following = self.retry_sweep_items[0] - transition = _sweep_plan_transition( - self.profile, item, following - ) - if transition == "cycle_reset": - self._begin_return_baseline("next_cycle") - else: - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - self, - item, - following, - ) - self._start_next_sweep() - return - # All physical source items for a dependent retry are complete. - # Clear the routing before checking the final source task itself, - # so an ordinary source-task failure retries only that task. - self.retry_source_failure_task_key = None - self.retry_source_task_keys = () - self.retry_cycle_override.clear() - self.retry_preserve_pnp_continuity = False - failures = self._provisional_fit_failures(previous_spec) - diagnostic_role = ( - G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - self, previous_spec - ) - ) - if diagnostic_role in {"side", "multiview"}: - G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( - self, previous_spec, failures - ) - return - if failures and self._pause_for_provisional_fit_failure( - previous_spec, failures, allow_warning=True - ): - return - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_cycles = set() - self.retry_joint_names = set() - self.fit_failure = {} - if self.sweep_index >= len(self.sweep_items): - self._begin_return_baseline("fit") - else: - self._begin_return_baseline( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - self, previous_spec - ) - ) - return - spec_complete = bool( - self.sweep_index >= len(self.sweep_items) - or self.sweep_items[self.sweep_index].spec != previous_spec - ) - if spec_complete: - failures = self._provisional_fit_failures(previous_spec) - diagnostic_role = ( - G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - self, previous_spec - ) - ) - if diagnostic_role in {"side", "multiview"}: - G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( - self, previous_spec, failures - ) - return - if failures and self._pause_for_provisional_fit_failure( - previous_spec, failures, allow_warning=True - ): - return - if self.retry_sweep_spec == previous_spec: - resume_index = self.retry_resume_index - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_cycles = set() - self.retry_joint_names = set() - if resume_index is not None: - self.sweep_index = resume_index - self.fit_failure = {} - if self.sweep_index >= len(self.sweep_items): - self._begin_return_baseline("fit") - elif self.sweep_items[self.sweep_index].spec != previous_spec: - self._begin_return_baseline( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - self, previous_spec - ) - ) - elif _sweep_plan_transition( - self.profile, - item, - self.sweep_items[self.sweep_index], - ) == "cycle_reset": - # Keep the task's complete avoidance pose between its three - # repetitions. Only the active joint returns to its standard-side - # baseline (roll: 255->127); global unfolding is deferred until - # the task really changes. - self._begin_return_baseline("next_cycle") - else: - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - self, - item, - self.sweep_items[self.sweep_index], - ) - self._start_next_sweep() - - def _fit_all_curves(self) -> None: - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) - self.state = STATE_FITTING - self.reason = "fitting_3d_axes_and_urdf_zero_offsets" - # Fitting is deliberately synchronous so no capture callback can - # mutate the observation set midway through a model fit. Announce - # the state before entering the CPU-bound section: the outer product - # runner then applies its fitting-specific watchdog instead of - # mistaking a blocked ROS timer for a motion communication failure. - fitting_started_at = time.monotonic() - self.last_status_publish = fitting_started_at - self._publish_status(fitting_started_at) - if int(getattr(self, "sample_schema_version", 0)) >= 1: - for store_name, store in ( - ("trajectory", self.records_by_joint), - ("baseline", self.baseline_records_by_joint), - ("steady", self.command_records_by_joint), - ("palm_axis", self.palm_axis_records_by_source), - ): - try: - validate_sample_records( - record - for records in store.values() - for record in records - ) - except SampleDataContractError as error: - raise SampleDataContractError( - f"{error};store={store_name}" - ) from error - # Provisional checks have a warning band so collection can continue, - # but the final fit always re-applies the unmodified hard thresholds. - fresh_task_keys = set( - getattr(self, "recalibration_task_keys", ()) - ) - for spec in self.profile.sweep_specs: - if fresh_task_keys and spec.key not in fresh_task_keys: - continue - failures = self._provisional_fit_failures(spec) - if failures: - self._pause_for_provisional_fit_failure( - spec, failures, allow_warning=False - ) - return - training_fits: dict[str, JointCurveFit] = {} - holdout_by_joint: dict[str, tuple[float, ...]] = {} - measured: dict[str, JointCurveFit] = {} - validation_cycle = self.repetitions - 1 - training_cycles = tuple(range(validation_cycle)) - training_cycle_set = set(training_cycles) - quality_joint_names = set( - recalibration_quality_joints( - self.profile, - getattr(self, "recalibration_scope", "full"), - ) - ) - standalone_thumb_calibration = bool( - getattr(self, "standalone_thumb_calibration", False) - ) - fit_joint_names = tuple( - name - for name in self.profile.measured_joints - if ( - not standalone_thumb_calibration - or name in quality_joint_names - ) - ) - for name in fit_joint_names: - training_records = [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) in training_cycle_set - ] - holdout_records = [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) == validation_cycle - ] - holdout_model_records = ( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, name, holdout_records - ) - ) - zero_command = int( - self.baseline_command[ - self.profile.joint_specs[name].motor_index - ] - ) - training_fits[name] = self._fit_joint_records( - name, training_records - ) - holdout_by_joint[name] = joint_curve_holdout_errors( - training_fits[name], - holdout_model_records, - zero_command_u8=zero_command, - ) - measured[name] = self._fit_joint_records( - name, self.records_by_joint[name] - ) - command_fits: dict[str, JointCurveFit] = dict(training_fits) - command_feedback_fits: dict[str, JointCurveFit] = dict( - training_fits - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - command_fits = {} - command_feedback_fits = {} - for name in fit_joint_names: - if name in G20_REFERENCE_THUMB_CMC_JOINTS: - # a609d521's dense feedback-indexed relative-rotation - # curve is the validated transfer function for the three - # CMC axes. Do not replace it with nine planar PnP - # checkpoints; those points were the source of the later - # thumb regression and combination-pose false failures. - command_fits[name] = measured[name] - continue - command_records = [ - dict(record) - for record in self.command_records_by_joint[name] - if int(record["cycle"]) == 0 - ] - if len(command_records) < 18: - failed_spec = next( - spec - for spec in self.profile.sweep_specs - if name in spec.joints - ) - self._pause_for_provisional_fit_failure( - failed_spec, - [ - { - "joint": name, - "metric": "steady_command_checkpoints", - "actual": len(command_records), - "limit": 18, - "comparison": "minimum", - } - ], - ) - return - zero_command = int( - self.baseline_command[ - self.profile.joint_specs[name].motor_index - ] - ) - if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS: - command_fits[name] = fit_joint_image_curve( - command_records, - maximum_radial_rms_px=( - self.image_trajectory_maximum_radial_rms_px - ), - maximum_radial_p95_px=( - self.image_trajectory_maximum_radial_p95_px - ), - minimum_radius_px=( - self.image_trajectory_minimum_radius_px - ), - minimum_arc_rad=self.trajectory_minimum_arc_rad, - ) - else: - command_fits[name] = fit_rotation_joint_curve( - command_records, - zero_command_u8=zero_command, - canonical_zero_direction=canonical_zero_direction( - self.profile, name - ), - ) - command_feedback_fits[name] = self._fit_joint_records( - name, - _steady_records_in_feedback_domain(command_records), - ) - self.cross_view_roll_metrics: dict[str, dict[str, float]] = {} - self.validation_only_fits: dict[str, JointCurveFit] = {} - for name, validation_name in ( - self.profile.axis_validation_sources or {} - ).items(): - if ( - name not in training_fits - or validation_name not in quality_joint_names - ): - continue - validation_training_records = [ - record - for record in self.records_by_joint[validation_name] - if int(record["cycle"]) in training_cycle_set - ] - validation_holdout_records = [ - record - for record in self.records_by_joint[validation_name] - if int(record["cycle"]) == validation_cycle - ] - validation_holdout_model_records = ( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, - validation_name, - validation_holdout_records, - ) - ) - validation_training_fit = self._fit_joint_records( - validation_name, validation_training_records - ) - validation_fit = self._fit_joint_records( - validation_name, self.records_by_joint[validation_name] - ) - # Any optional post-fit random validation must query the same - # frozen training model that passed the isolated holdout. - self.validation_only_fits[validation_name] = ( - validation_training_fit - ) - # The structured task-level check above records threshold - # overruns with per-cycle evidence. At publication time this - # secondary view remains diagnostic: the primary front curve and - # the side physical axis line retain their independent hard - # quality gates. Reuse the same policy as offline replay instead - # of reintroducing an unlocalized retry from a second code path. - training_metrics = cross_view_roll_diagnostic_metrics( - training_fits[name], validation_training_fit - ) - final_metrics = cross_view_roll_diagnostic_metrics( - measured[name], validation_fit - ) - validation_errors = joint_curve_holdout_errors( - validation_training_fit, - validation_holdout_model_records, - zero_command_u8=int( - self.baseline_command[ - self.profile.record_specs[validation_name].motor_index - ] - ), - ) - holdout_by_joint[validation_name] = validation_errors - self.cross_view_roll_metrics[name] = { - **{ - f"training_{key}": float(value) - for key, value in training_metrics.items() - }, - **{ - f"final_{key}": float(value) - for key, value in final_metrics.items() - }, - } - self.joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} - for name in fit_joint_names: - cycle_travel: list[float] = [] - for cycle in range(self.repetitions): - cycle_fit = self._fit_joint_records( - name, - [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) == cycle - ], - ) - cycle_travel.append( - abs( - float(cycle_fit.angle_rad[0]) - - float(cycle_fit.angle_rad[255]) - ) - ) - holdout = np.abs( - np.asarray(holdout_by_joint[name], dtype=float) - ) - baseline_hysteresis = baseline_hysteresis_by_cycle_rad( - G20ThreeCameraCalibrationNode._baseline_hysteresis_records( - self, name, self.records_by_joint[name] - ), - zero_command_u8=int( - self.baseline_command[ - self.profile.joint_specs[name].motor_index - ] - ), - axis_xyz=( - G20ThreeCameraCalibrationNode._hysteresis_axis_for_fit( - self, - name, - self.records_by_joint[name], - measured[name], - ) - ), - ) - self.joint_dynamic_diagnostics[name] = { - "cycle_travel_rad": [ - round(float(value), 8) for value in cycle_travel - ], - "cycle_travel_range_rad": round( - max(cycle_travel) - min(cycle_travel), 8 - ), - "baseline_hysteresis_by_cycle_rad": [ - round(float(value), 8) - for value in baseline_hysteresis - ], - "holdout_cycle": validation_cycle, - "holdout_cycle_mae_rad": round(float(np.mean(holdout)), 8), - "holdout_cycle_max_rad": round(float(np.max(holdout)), 8), - } - axes: list[JointAxisMeasurement] = [] - axis_profile = ( - get_right_19_thumb_zero_profile() - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and self.recalibration_scope == "thumb" - ) - else self.zero_profile - ) - for name in axis_profile.axis_joints: - group = [ - self._fit_axis_measurement(name, cycle) - for cycle in range(self.repetitions) - ] - axes.extend(self._refit_cross_view_axis_line_group(name, group)) - - try: - palm_orientation_measurements, palm_orientation_rejections = ( - fit_partial_palm_orientation_measurements( - sources=self.profile.palm_orientation_sources, - records_by_joint=self.palm_axis_records_by_source, - motor_by_source=self.profile.palm_axis_motor_by_source, - baseline_command_u8=self.baseline_command, - cycles=range(self.repetitions), - minimum_sources=int( - self.profile.minimum_palm_orientation_sources - ), - minimum_arc_rad=( - self.profile.palm_orientation_minimum_arc_rad - ), - maximum_rotation_orthogonal_rms_rad=( - self.active_maximum_rotation_orthogonal_rms_rad - ), - maximum_command_distance_u8=( - self.profile - .palm_orientation_maximum_command_distance_u8 - ), - ) - ) - except ValueError as error: - failure = str(error) - self.palm_orientation_rejections = {"coverage": failure} - self.fit_failure = { - "task": "palm_orientation_side_channel", - "failures": [ - { - "joint": "palm_orientation", - "metric": "palm_orientation_coverage", - "reason": failure, - } - ], - "directions_to_rescan": 0, - } - append_jsonl( - self.raw_path, - { - "kind": "palm_orientation_optional_source_diagnostic", - "rejections": {"coverage": failure}, - "decision": "reject_unobservable_palm_phase", - }, - ) - self._pause("palm_orientation_quality_failed") - return - if palm_orientation_rejections: - append_jsonl( - self.raw_path, - { - "kind": "palm_orientation_optional_source_diagnostic", - "rejections": dict(palm_orientation_rejections), - "decision": "use_qualified_redundant_sources", - }, - ) - - motor_by_joint = { - name: int(spec.motor_index) - for name, spec in self.profile.joint_specs.items() - } - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - if self.recalibration_scope == "full": - scoped_endpoint_joints = frozenset( - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - ) - else: - want_thumb_endpoints = self.recalibration_scope == "thumb" - scoped_endpoint_joints = frozenset( - name - for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - if name.startswith("thumb_") == want_thumb_endpoints - ) - command_fits = anchor_right_19_mechanical_endpoint_curves( - command_fits, - { - name: ( - [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) in training_cycle_set - ] - if name == "thumb_cmc_roll" - else self.command_records_by_joint[name] - ) - for name in scoped_endpoint_joints - }, - maximum_direction_difference_rad=( - self.command_maximum_direction_gap_rad - ), - feedback_endpoint_joints=( - frozenset({"thumb_cmc_roll"}) - & scoped_endpoint_joints - ), - endpoint_joints=scoped_endpoint_joints, - ) - measured_endpoint_zero_offsets = ( - derive_right_19_mechanical_endpoint_offsets( - self.source_urdf_path, - command_fits, - maximum_offset_rad=( - self.mechanical_endpoint_maximum_offset_rad - ), - endpoint_joints=scoped_endpoint_joints, - ) - ) - frozen_offsets = dict( - getattr( - self, - "partial_scope_fixed_zero_offsets_rad", - {}, - ) - ) - # Endpoint-derived zeros also move URDF limits or mimic offsets. - # A partial run must preserve those coupled fields together with - # the certified non-target origin, not merely freeze the solver's - # scalar offset after endpoint derivation. - measured_endpoint_zero_offsets.update( - { - name: value - for name, value in frozen_offsets.items() - if name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - } - ) - endpoint_zero_offsets = { - name: value - for name, value in measured_endpoint_zero_offsets.items() - if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - } - post_solve_endpoint_offsets = { - name: value - for name, value in measured_endpoint_zero_offsets.items() - if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS - } - else: - endpoint_zero_offsets = {} - post_solve_endpoint_offsets = {} - zero_solve_arguments = dict( - source_urdf=self.source_urdf_path, - palm_orientation_measurements=palm_orientation_measurements, - curves=training_fits, - motor_by_joint=motor_by_joint, - maximum_offset_rad=self.zero_maximum_offset_rad, - joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, - maximum_validation_mae_rad=self.maximum_validation_mae_rad, - maximum_validation_p95_rad=self.maximum_validation_p95_rad, - maximum_validation_error_rad=( - self.maximum_validation_error_rad - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - maximum_confidence_half_width_rad=( - self.zero_maximum_confidence_half_width_rad - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - maximum_axis_cone_mismatch_rad=( - self.zero_maximum_axis_cone_mismatch_rad - ), - maximum_systematic_axis_cone_bias_rad=( - self.cross_view_roll_maximum_axis_difference_rad - ), - maximum_observability_condition_number=( - self.zero_maximum_observability_condition_number - ), - maximum_pose_axis_line_rms_m=( - self.axis_maximum_pose_line_rms_m - ), - finger_maximum_offset_rad=self.zero_finger_maximum_offset_rad, - maximum_cycle_difference_rad=( - self.zero_maximum_axis_cycle_difference_rad - ), - hand_type=self.hand_type, - tag_layout=self.profile.layout_id, - training_cycles=training_cycles, - validation_cycle=validation_cycle, - ) - frozen_offsets = dict( - getattr(self, "partial_scope_fixed_zero_offsets_rad", {}) - ) - independent_thumb_scope = bool( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and self.recalibration_scope in {"full", "thumb"} - ) - if independent_thumb_scope: - thumb_profile = get_right_19_thumb_zero_profile() - thumb_names = set(thumb_profile.direct_zero_joints) - thumb_result = solve_urdf_zero_offsets( - **zero_solve_arguments, - measurements=[ - item for item in axes if item.joint in thumb_profile.axis_joints - ], - fixed_direct_zero_offsets_rad={ - name: value - for name, value in endpoint_zero_offsets.items() - if name in thumb_names - }, - static_output_zero_offsets_rad={ - name: value - for name, value in post_solve_endpoint_offsets.items() - if name in thumb_names - }, - zero_profile=thumb_profile, - ) - if self.recalibration_scope == "thumb": - if self.standalone_thumb_calibration: - holdout_zero_result = ( - expand_right_19_thumb_zero_result_with_cad_fingers( - thumb_result - ) - ) - else: - holdout_zero_result = merge_right_19_thumb_zero_result( - thumb_result=thumb_result, - preserved_offsets_rad=frozen_offsets, - ) - else: - # The full-hand solve still determines the 12 finger zeros and - # palm nuisance pose, but all four thumb values are immutable. - # Consequently no finger observation can pull a thumb zero. - companion_result = solve_urdf_zero_offsets( - **zero_solve_arguments, - measurements=axes, - fixed_direct_zero_offsets_rad={ - **self.zero_profile.fixed_direct_zero_offsets_rad, - **endpoint_zero_offsets, - **thumb_result.direct_offsets_rad, - }, - static_output_zero_offsets_rad={ - **self.zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - }, - ) - holdout_zero_result = merge_right_19_thumb_zero_result( - thumb_result=thumb_result, - companion_result=companion_result, - ) - else: - holdout_zero_result = solve_urdf_zero_offsets( - **zero_solve_arguments, - measurements=axes, - fixed_direct_zero_offsets_rad={ - **self.zero_profile.fixed_direct_zero_offsets_rad, - **endpoint_zero_offsets, - **frozen_offsets, - }, - static_output_zero_offsets_rad={ - **self.zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - **{ - name: value - for name, value in frozen_offsets.items() - if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS - }, - }, - ) - thumb_yaw_zero_failures = ( - _thumb_yaw_zero_repeatability_failures( - holdout_zero_result, - maximum_cycle_range_rad=( - self.thumb_yaw_maximum_zero_cycle_difference_rad - ), - maximum_confidence_half_width_rad=( - self.thumb_yaw_maximum_confidence_half_width_rad - ), - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else [] - ) - append_jsonl( - self.raw_path, - { - "kind": "zero_holdout_diagnostics", - "hand_type": self.hand_type, - "passed": holdout_zero_result.passed, - "direct_offsets_rad": dict( - holdout_zero_result.direct_offsets_rad - ), - "partial_scope_fixed_zero_offsets_rad": dict( - getattr( - self, - "partial_scope_fixed_zero_offsets_rad", - {}, - ) - ), - "model_base_translation_xyz_m": list( - holdout_zero_result.base_translation_xyz_m - ), - "model_base_quaternion_xyzw": list( - holdout_zero_result.base_quaternion_xyzw - ), - "cycle_offsets_rad": { - name: list(values) - for name, values in ( - holdout_zero_result.cycle_offsets_rad.items() - ) - }, - "offset_uncertainty_rad": dict( - holdout_zero_result.offset_uncertainty_rad - ), - "validation_error_by_joint_rad": dict( - holdout_zero_result.validation_error_by_joint_rad - ), - "validation_original_error_by_joint_rad": dict( - holdout_zero_result.validation_original_error_by_joint_rad - ), - "validation_improvement_by_joint_rad": dict( - holdout_zero_result.validation_improvement_by_joint_rad - ), - "validation_improvement_confidence_lower_rad": dict( - holdout_zero_result - .validation_improvement_confidence_lower_rad - ), - "axis_line_rms_m": holdout_zero_result.axis_line_rms_m, - "validation_line_error_by_joint_m": dict( - holdout_zero_result.validation_line_error_by_joint_m - ), - "offset_confidence_95_half_width_rad": dict( - holdout_zero_result.offset_confidence_half_width_rad - ), - "observability_rank": holdout_zero_result.observability_rank, - "observability_parameter_count": ( - holdout_zero_result.observability_parameter_count - ), - "observability_condition_number": ( - holdout_zero_result.observability_condition_number - ), - "failure_reasons": dict( - holdout_zero_result.failure_reasons - ), - "thumb_yaw_repeatability_failures": ( - thumb_yaw_zero_failures - ), - "axis_cone_mismatch_by_joint_rad": dict( - holdout_zero_result.axis_cone_mismatch_by_joint_rad - ), - "axis_cone_bias_classification_by_joint": dict( - holdout_zero_result - .axis_cone_bias_classification_by_joint - ), - }, - ) - holdout_errors = np.abs( - np.asarray( - [ - value - for name, values in holdout_by_joint.items() - if name in quality_joint_names - for value in values - ], - dtype=float, - ) - ) - trajectory_holdout_passed = bool( - holdout_errors.size - and float(np.mean(holdout_errors)) - <= self.maximum_validation_mae_rad - and float(np.percentile(holdout_errors, 95.0)) - <= self.maximum_validation_p95_rad - and ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - or float(np.max(holdout_errors)) - <= self.maximum_validation_error_rad - ) - and ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - or all( - float(np.mean(np.abs(np.asarray(values, dtype=float)))) - <= self.maximum_validation_mae_rad - and float(np.max(np.abs(np.asarray(values, dtype=float)))) - <= self.maximum_validation_error_rad - for name, values in holdout_by_joint.items() - if name in quality_joint_names - ) - ) - ) - self.fit_quality_passed = all( - fit.maximum_monotonic_correction_rad - <= ( - self.maximum_monotonic_correction_rad - if self.profile.joint_specs[name].active - else self.passive_maximum_monotonic_correction_rad - ) - and ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - or fit.maximum_hysteresis_rad - <= ( - self.maximum_hysteresis_rad - if self.profile.joint_specs[name].active - else self.passive_maximum_hysteresis_rad - ) - ) - for name, fit in training_fits.items() - if name in quality_joint_names - ) and all( - fit.maximum_hysteresis_rad - <= self.command_maximum_direction_gap_rad - for name, fit in command_feedback_fits.items() - if name in quality_joint_names - ) - if not trajectory_holdout_passed: - trajectory_score = { - name: float(np.percentile(np.abs(values), 95.0)) - for name, values in holdout_by_joint.items() - if name in quality_joint_names - } - worst_joint = max(trajectory_score, key=trajectory_score.get) - failed_spec = next( - spec - for spec in self.profile.sweep_specs - if worst_joint in spec.joints - ) - failures: list[dict[str, Any]] = [ - { - "joint": name, - "metric": "third_cycle_trajectory_p95_deg", - "actual": round(math.degrees(error), 6), - "limit": math.degrees( - self.maximum_validation_p95_rad - ), - "comparison": "maximum", - } - for name, error in trajectory_score.items() - if error > self.maximum_validation_p95_rad - ] - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - for name, values in holdout_by_joint.items(): - if name not in quality_joint_names: - continue - absolute = np.abs(np.asarray(values, dtype=float)) - mean_error = float(np.mean(absolute)) - maximum_error = float(np.max(absolute)) - if mean_error > self.maximum_validation_mae_rad: - failures.append( - { - "joint": name, - "metric": "third_cycle_trajectory_mae_deg", - "actual": round( - math.degrees(mean_error), 6 - ), - "limit": math.degrees( - self.maximum_validation_mae_rad - ), - "comparison": "maximum", - } - ) - if maximum_error > self.maximum_validation_error_rad: - failures.append( - { - "joint": name, - "metric": "third_cycle_trajectory_max_deg", - "actual": round( - math.degrees(maximum_error), 6 - ), - "limit": math.degrees( - self.maximum_validation_error_rad - ), - "comparison": "maximum", - } - ) - self._pause_for_provisional_fit_failure(failed_spec, failures) - return - if thumb_yaw_zero_failures: - thumb_yaw_spec = next( - spec - for spec in self.profile.sweep_specs - if "thumb_cmc_yaw" in spec.joints - ) - yaw_pair = self.zero_profile.same_view_axis_pair_by_offset.get( - "thumb_cmc_yaw", () - ) - source_task_key_set = { - observer.task_name - for observer in self.profile.palm_axis_observers - if observer.model_joint in set(yaw_pair) - } - self.retry_source_failure_task_key = thumb_yaw_spec.key - self.retry_source_task_keys = tuple( - candidate.key - for candidate in self.profile.sweep_specs - if candidate.key in source_task_key_set - ) - # A localized scalar outlier is replaced in place while the live - # PnP tracker/reference remains continuous. Non-localized yaw - # failures still select all cycles in the generic retry policy, - # but no dependent retry is allowed to create a new yaw datum. - self.retry_cycle_override.clear() - self._pause_for_provisional_fit_failure( - thumb_yaw_spec, thumb_yaw_zero_failures - ) - return - if not holdout_zero_result.passed: - self._pause_for_zero_model_failure(holdout_zero_result) - return - # Publish the exact frozen training model that was evaluated on the - # isolated final cycle. Re-solving with the holdout would invalidate - # the acceptance result. - zero_result = holdout_zero_result - self.thumb_yaw_cross_session_diagnostic = {} - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - previous_yaw = _previous_passed_joint_zero_offset( - self.session_dir, - self.serial_number, - "thumb_cmc_yaw", - ) - current_yaw = zero_result.all_active_offsets_rad.get( - "thumb_cmc_yaw" - ) - if previous_yaw is not None and current_yaw is not None: - previous_session, previous_offset = previous_yaw - delta = float(current_yaw) - float(previous_offset) - self.thumb_yaw_cross_session_diagnostic = { - "kind": "thumb_yaw_cross_session_diagnostic", - "joint": "thumb_cmc_yaw", - "previous_session": str(previous_session), - "previous_offset_deg": round( - math.degrees(previous_offset), 6 - ), - "current_offset_deg": round( - math.degrees(float(current_yaw)), 6 - ), - "absolute_difference_deg": round( - abs(math.degrees(delta)), 6 - ), - "reference_limit_deg": round( - math.degrees( - self.thumb_yaw_cross_session_diagnostic_rad - ), - 6, - ), - "classification": ( - "review_position_or_tag_installation" - if abs(delta) - > self.thumb_yaw_cross_session_diagnostic_rad - else "within_reference_band" - ), - "decision": "diagnostic_only", - } - append_jsonl( - self.raw_path, - self.thumb_yaw_cross_session_diagnostic, - ) - append_jsonl( - self.raw_path, - { - "kind": "zero_final_diagnostics", - "hand_type": self.hand_type, - "direct_offsets_rad": dict(zero_result.direct_offsets_rad), - "partial_scope_fixed_zero_offsets_rad": dict( - getattr( - self, - "partial_scope_fixed_zero_offsets_rad", - {}, - ) - ), - "model_base_translation_xyz_m": list( - zero_result.base_translation_xyz_m - ), - "model_base_quaternion_xyzw": list( - zero_result.base_quaternion_xyzw - ), - "offset_uncertainty_rad": dict( - zero_result.offset_uncertainty_rad - ), - "offset_confidence_95_half_width_rad": dict( - zero_result.offset_confidence_half_width_rad - ), - "training_cycles": list(zero_result.training_cycles), - "validation_cycle": zero_result.validation_cycle, - "axis_line_rms_m": zero_result.axis_line_rms_m, - "validation_line_error_by_joint_m": dict( - zero_result.validation_line_error_by_joint_m - ), - "observability_rank": zero_result.observability_rank, - "observability_parameter_count": ( - zero_result.observability_parameter_count - ), - "observability_condition_number": ( - zero_result.observability_condition_number - ), - "offset_covariance_rad2": dict( - zero_result.offset_covariance_rad2 - ), - "axis_cone_mismatch_by_joint_rad": dict( - zero_result.axis_cone_mismatch_by_joint_rad - ), - "axis_cone_bias_classification_by_joint": dict( - zero_result.axis_cone_bias_classification_by_joint - ), - }, - ) - # Runtime/MuJoCo consumes requested commands, never feedback bins. - # Static geometry and holdout above intentionally continue to use the - # dense feedback-domain training fit. - if standalone_thumb_calibration: - self.measured_fits = dict(command_fits) - else: - self.measured_fits = clamp_runtime_fits_to_urdf_limits( - self.source_urdf_path, - derive_mimic_passive_fits( - self.source_urdf_path, - command_fits, - profile=self.profile, - ), - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - ) - self.axis_measurements = axes - self.palm_orientation_measurements = list( - palm_orientation_measurements - ) - self.palm_orientation_rejections = dict( - palm_orientation_rejections - ) - self.zero_result = zero_result - self.validated_endpoint_zero_offsets_rad = dict(endpoint_zero_offsets) - self.validation_errors_rad = [ - float(value) - for name, values in holdout_by_joint.items() - if name in quality_joint_names - for value in values - ] - self.validation_errors_rad.extend( - float(value) - for value in holdout_zero_result.validation_errors_rad - ) - if self.combination_validation_enabled: - self._build_combination_validation_items() - self._start_next_combination_validation() - elif self.validation_enabled: - self._build_validation_items() - self._start_next_validation() - else: - self.validation_items.clear() - self.validation_index = 0 - self._begin_return_baseline("finalize") - - def _build_validation_items(self) -> None: - generator = random.Random(self.validation_seed) - self.validation_items = [] - for spec in self.profile.sweep_specs: - commands = generator.sample( - list(range(16, 240)), self.validation_command_count - ) - self.validation_items.extend( - ValidationItem(spec, command) for command in commands - ) - self.validation_index = 0 - - def _build_combination_validation_items(self) -> None: - self.combination_validation_items = list( - _combination_validation_items(self.baseline_command) - ) - self.combination_validation_index = 0 - self.combination_validation_completed = False - - def _start_next_combination_validation(self) -> None: - if self.combination_validation_index >= len( - self.combination_validation_items - ): - self.active_combination_validation = None - self.combination_validation_completed = True - self._begin_return_baseline("finalize") - return - item = self.combination_validation_items[ - self.combination_validation_index - ] - self.active_combination_validation = item - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.position_hold_since = None - self.validation_stage_started_at = time.monotonic() - self.motion_stage_started_at = self.validation_stage_started_at - self._reset_motion_progress( - self.validation_stage_started_at, - self._command_vector_error_u8(item.command_u8), - ) - self.state = STATE_VALIDATION_MOVE - self.reason = f"combination_validation_move_{item.name}" - self._publish_speed_profile(self._normal_speed_profile()) - self._publish_command(list(item.command_u8)) - - def _finish_combination_validation_capture(self) -> None: - item = self.active_combination_validation - if item is None: - raise RuntimeError("combination validation item is missing") - observations: dict[str, Any] = {} - all_frames = [ - frame - for frames in self.combination_validation_frames_buffer.values() - for frame in frames - ] - feedback = np.median( - np.asarray([frame.state_u8 for frame in all_frames], dtype=float), - axis=0, - ) - motor_directions = _combination_motor_directions( - self.combination_validation_items, - self.combination_validation_index, - self.baseline_command, - ) - angles = _combination_joint_angles( - self.profile, - self.measured_fits, - item.command_u8, - motor_directions, - ) - model = UrdfKinematicModel(self.source_urdf_path) - model_base_common = transform_matrix( - self.zero_result.base_translation_xyz_m, - self.zero_result.base_quaternion_xyzw, - ) - pose_position_errors: list[float] = [] - pose_orientation_errors: list[float] = [] - target_errors: list[dict[str, Any]] = [] - observed_target_keys: list[str] = [] - evaluated_target_keys: list[str] = [] - pending_mounts: dict[str, np.ndarray] = {} - for view, frames in self.combination_validation_frames_buffer.items(): - by_joint: dict[str, Any] = {} - joint_names = sorted( - set.intersection( - *(set(frame.joint_quaternions_xyzw) for frame in frames) - ) - ) - for name in joint_names: - by_joint[name] = { - "relative_quaternion_xyzw": [ - float(value) - for value in robust_rotation_summary( - [frame.joint_quaternions_xyzw[name] for frame in frames] - )[0] - ], - "parent_pose_common": _robust_pose_payload( - [frame.parent_poses_common[name] for frame in frames] - ), - "child_pose_common": _robust_pose_payload( - [frame.child_poses_common[name] for frame in frames] - ), - } - observations[view] = by_joint - base_role = COMBINATION_BASE_ROLE_BY_VIEW[view] - base_candidates = [ - name - for name in joint_names - if self.profile.record_specs[name].parent_role == base_role - ] - preferred_base = COMBINATION_BASE_OBSERVER_BY_VIEW[view] - if preferred_base in base_candidates: - base_name = preferred_base - elif base_candidates: - base_name = base_candidates[0] - else: - self._retry_combination_validation_or_pause( - f"combination_{view}_has_no_visible_base_target", - time.monotonic(), - ) - return - base_pose = _robust_pose_payload( - [frame.parent_poses_common[base_name] for frame in frames] - ) - base_matrix = transform_matrix( - base_pose["translation_xyz_m"], base_pose["quaternion_xyzw"] - ) - for observation_name, model_joint in COMBINATION_TAG_TARGETS_BY_VIEW[view]: - if observation_name not in by_joint: - continue - child_pose = by_joint[observation_name]["child_pose_common"] - child_matrix = transform_matrix( - child_pose["translation_xyz_m"], - child_pose["quaternion_xyzw"], - ) - observed = np.linalg.inv(base_matrix) @ child_matrix - # ``observed`` is expressed in this view's fixed palm-Tag - # frame, while UrdfKinematicModel returns hand_base->link. - # The zero solve already estimates hand_base->common; carry - # the model through common and into the observer base frame - # before solving or applying the moving Tag mount. - link = _model_link_in_observer_base( - base_matrix, - model_base_common, - model.link_transform( - model_joint, - zero_offsets=( - self.zero_result.all_active_offsets_rad - ), - joint_angles=angles, - independent_mimic_angles=True, - ), - ) - key = f"{view}:{observation_name}" - quality_gated = key in G20_COMBINATION_REQUIRED_TARGET_KEYS - observed_target_keys.append(key) - if key not in self.combination_tag_mounts: - pending_mounts[key] = np.linalg.inv(link) @ observed - continue - else: - mount = self.combination_tag_mounts[key] - predicted = link @ mount - if quality_gated: - evaluated_target_keys.append(key) - position_error = float( - np.linalg.norm(predicted[:3, 3] - observed[:3, 3]) - ) - orientation_error = float( - ( - Rotation.from_matrix(predicted[:3, :3]).inv() - * Rotation.from_matrix(observed[:3, :3]) - ).magnitude() - ) - if quality_gated: - pose_position_errors.append(position_error) - pose_orientation_errors.append(orientation_error) - target_errors.append( - { - "target": key, - "model_joint": model_joint, - "quality_gated": quality_gated, - "position_error_m": round(position_error, 8), - "orientation_error_rad": round( - orientation_error, 8 - ), - "orientation_error_deg": round( - math.degrees(orientation_error), 6 - ), - "observed_pose_in_base": matrix_payload(observed), - "predicted_pose_in_base": matrix_payload(predicted), - } - ) - if self.combination_validation_index > 0 and not pose_position_errors: - self._retry_combination_validation_or_pause( - "combination_pose_has_no_previously_observed_visible_target", - time.monotonic(), - ) - return - position_p95 = ( - 0.0 - if not pose_position_errors - else float(np.percentile(pose_position_errors, 95.0)) - ) - orientation_p95 = ( - 0.0 - if not pose_orientation_errors - else float(np.percentile(pose_orientation_errors, 95.0)) - ) - if ( - position_p95 > self.combination_maximum_position_p95_m - or orientation_p95 > self.combination_maximum_orientation_p95_rad - ): - retry_key = ("combination", self.combination_validation_index) - append_jsonl( - self.raw_path, - { - "kind": "combination_validation_failure", - "reason": "combination_pose_prediction_failed", - "pose_index": self.combination_validation_index, - "pose_name": item.name, - "label_zh": item.label_zh, - "capture_attempt": ( - self.validation_retry_counts.get(retry_key, 0) + 1 - ), - "requested_command_u8": list(item.command_u8), - "feedback_u8": [ - round(float(value), 6) for value in feedback - ], - "angle_branch_by_motor": list(motor_directions), - "evaluated_targets": sorted(set(evaluated_target_keys)), - "target_errors": sorted( - target_errors, key=lambda value: value["target"] - ), - "valid_frames_by_view": { - view: len(frames) - for view, frames in ( - self.combination_validation_frames_buffer.items() - ) - }, - "position_p95_m": round(position_p95, 8), - "maximum_position_p95_m": round( - self.combination_maximum_position_p95_m, 8 - ), - "orientation_p95_rad": round(orientation_p95, 8), - "orientation_p95_deg": round( - math.degrees(orientation_p95), 6 - ), - "maximum_orientation_p95_rad": round( - self.combination_maximum_orientation_p95_rad, 8 - ), - "maximum_orientation_p95_deg": round( - math.degrees( - self.combination_maximum_orientation_p95_rad - ), - 6, - ), - "passed": False, - }, - ) - self._retry_combination_validation_or_pause( - "combination_pose_prediction_failed", time.monotonic() - ) - return - self.combination_tag_mounts.update(pending_mounts) - for key in set(observed_target_keys): - self.combination_tag_observation_counts[key] = ( - self.combination_tag_observation_counts.get(key, 0) + 1 - ) - for key in set(evaluated_target_keys): - self.combination_tag_validation_counts[key] = ( - self.combination_tag_validation_counts.get(key, 0) + 1 - ) - is_final_pose = self.combination_validation_index + 1 >= len( - self.combination_validation_items - ) - if is_final_pose: - coverage = combination_target_coverage( - self.combination_tag_observation_counts, - self.combination_tag_validation_counts, - ) - if not coverage["coverage_passed"]: - append_jsonl( - self.raw_path, - { - "kind": "combination_target_coverage_failure", - "pose_name": item.name, - **coverage, - }, - ) - self._retry_combination_validation_or_pause( - "combination_target_coverage_incomplete", time.monotonic() - ) - return - self.combination_position_errors_m.extend(pose_position_errors) - self.combination_orientation_errors_rad.extend(pose_orientation_errors) - append_jsonl( - self.raw_path, - { - "kind": "combination_validation_sample", - "pose_index": self.combination_validation_index, - "pose_name": item.name, - "label_zh": item.label_zh, - "requested_command_u8": list(item.command_u8), - "feedback_u8": [round(float(value), 6) for value in feedback], - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in all_frames]) - ), - "views": observations, - "observed_targets": sorted(set(observed_target_keys)), - "evaluated_targets": sorted(set(evaluated_target_keys)), - "valid_frames_by_view": { - view: len(frames) - for view, frames in self.combination_validation_frames_buffer.items() - }, - "position_p95_m": round(position_p95, 8), - "orientation_p95_rad": round(orientation_p95, 8), - "passed": True, - }, - ) - self.combination_validation_index += 1 - self.active_combination_validation = None - if self.combination_validation_index >= len( - self.combination_validation_items - ): - self.combination_validation_completed = True - self._begin_return_baseline("finalize") - else: - self._begin_return_baseline("combination_next") - - 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) - return - key = ("combination", self.combination_validation_index) - retries = self.validation_retry_counts.get(key, 0) - if retries >= self.automatic_sweep_retry_limit: - self._pause(reason) - return - retries += 1 - self.validation_retry_counts[key] = retries - for view in self.combination_validation_frames_buffer: - runtime = self.views.get(view) - if runtime is None: - continue - self._reset_view_trackers(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - append_jsonl( - self.raw_path, - { - "kind": "automatic_combination_retry", - "pose_name": item.name, - "reason": reason, - "retry": retries, - "retry_limit": self.automatic_sweep_retry_limit, - }, - ) - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.validation_stage_started_at = now - self.motion_stage_started_at = now - self.position_hold_since = None - self.state = STATE_VALIDATION_MOVE - self.reason = f"automatic_retry_{reason}" - self._reset_motion_progress(now, self._command_vector_error_u8(item.command_u8)) - self._publish_command(list(item.command_u8)) - - def _start_next_validation(self) -> None: - if self.validation_index >= len(self.validation_items): - self.active_validation = None - self._begin_return_baseline("finalize") - return - self.active_validation = self.validation_items[self.validation_index] - validation_motor = self.active_validation.spec.motor_index - validation_target = float(self.active_validation.command_u8) - validation_current = ( - float(self.latest_state_u8[validation_motor]) - if len(self.latest_state_u8) == 20 - else validation_target - ) - self.active_validation_direction = ( - DIRECTION_INCREASING - if validation_target > validation_current + 0.5 - else DIRECTION_DECREASING - if validation_target < validation_current - 0.5 - else canonical_zero_direction( - self.profile, self.active_validation.spec.joints[0] - ) - ) - self.validation_frames_buffer.clear() - self.position_hold_since = None - self.validation_stage_started_at = time.monotonic() - self._reset_motion_progress( - self.validation_stage_started_at, - self._motion_command_error_u8( - self.active_validation.spec, - self.active_validation.command_u8, - ), - ) - self.state = STATE_VALIDATION_MOVE - self.reason = ( - f"validation_{self.active_validation.spec.view}_motor_" - f"{self.active_validation.spec.motor_index}_" - f"command_{self.active_validation.command_u8}" - ) - self._publish_speed_profile( - self._speed_profile_for_spec(self.active_validation.spec) - ) - self._publish_command( - build_calibration_motion_command( - self.active_validation.spec, - self.active_validation.command_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - - def _finish_validation_capture(self) -> None: - assert self.active_validation is not None - item = self.active_validation - command = item.command_u8 - for name in item.spec.joints: - selected = _frames_for_joint( - self.validation_frames_buffer, name - ) - if len(selected) < self.validation_frames: - raise RuntimeError( - f"random validation {name} has too few frames" - ) - fit = self.measured_fits.get(name) - if fit is None: - fit = self.validation_only_fits[name] - quaternion = robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[name] - for frame in selected - ] - )[0] - image_relative_xy_px = np.median( - np.asarray( - [ - frame.image_vectors_xy_px[name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - observed = measure_joint_curve_observation( - fit, - quaternion_xyzw=quaternion, - image_relative_xy_px=image_relative_xy_px, - ) - expected_curve = ( - fit.decreasing_rad - if self.active_validation_direction == DIRECTION_DECREASING - else fit.increasing_rad - if self.active_validation_direction == DIRECTION_INCREASING - else fit.angle_rad - ) - expected = float(expected_curve[command]) - self.validation_errors_rad.append(observed - expected) - previous_spec = item.spec - self.validation_index += 1 - self.active_validation = None - self.active_validation_direction = None - if self.validation_index >= len(self.validation_items): - self._begin_return_baseline("finalize") - elif self.validation_items[self.validation_index].spec != previous_spec: - self._begin_return_baseline("validation_next") - else: - self._start_next_validation() - - def _finalize(self) -> None: - if self.zero_result is None: - raise RuntimeError("URDF zero solution is missing") - errors = np.abs(np.asarray(self.validation_errors_rad, dtype=float)) - validation_passed = bool( - self.zero_result.passed - and ( - not self.combination_validation_enabled - or self.combination_validation_completed - ) - and ( - not self.validation_enabled - or ( - errors.size > 0 - and float(np.mean(errors)) - <= self.maximum_validation_mae_rad - and float(np.percentile(errors, 95.0)) - <= self.maximum_validation_p95_rad - and ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - or float(np.max(errors)) - <= self.maximum_validation_error_rad - ) - ) - ) - ) - passed = bool(self.fit_quality_passed and validation_passed) - if not passed: - raise RuntimeError("final_calibration_quality_failed") - endpoint_zero_offsets = ( - G20ThreeCameraCalibrationNode - ._endpoint_zero_offsets_for_publication(self) - ) - stamp = ( - self.session_dir.name - if re.fullmatch(r"\d{8}_\d{6}", self.session_dir.name) - else None - ) - # Zero calibration changes only joint-origin rotations. Measured - # motion curves belong in schema-v4 JSON and must never enlarge the - # original CAD/mechanical safety limits in the generated URDF. - # JSON schema v4 publishes eight decimal places. Generate the URDF - # from those exact same values so the pair is numerically inseparable - # instead of differing by harmless formatter quantisation. - published_zero_offsets = { - 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 = { - name: published_zero_offsets[name] - for name in self.zero_profile.direct_zero_joints - } - correction_plan = None - typed_profile = getattr(self, "calibration_profile", None) - if typed_profile is not None: - frozen_names = typed_profile.scope.frozen_joints[ - self.recalibration_scope - ] - correction_plan = build_correction_plan( - typed_profile, - source_sha256=_file_sha256(self.source_urdf_path), - scope=self.recalibration_scope, - frozen_offsets_rad={ - name: urdf_offsets[name] for name in frozen_names - }, - ) - urdf_input_path = self.final_path.with_name( - f"{self.final_path.stem}_urdf_correction_input.json" - ) - atomic_write_json( - urdf_input_path, - build_g20_urdf_input_payload( - side=self.hand_type, - layout_id=self.profile.layout_id, - serial_number=self.serial_number, - source_urdf=self.source_urdf_path, - offsets_rad=urdf_offsets, - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - ), - ) - urdf_offsets, endpoint_zero_offsets = load_g20_urdf_input( - urdf_input_path, - source_urdf=self.source_urdf_path, - side=self.hand_type, - layout_id=self.profile.layout_id, - serial_number=self.serial_number, - ) - self.corrected_urdf_path = write_zero_corrected_urdf( - source_urdf=self.source_urdf_path, - output_directory=self.corrected_urdf_output_dir, - serial_number=self.serial_number, - offsets_rad=urdf_offsets, - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - timestamp=stamp, - correction_plan=correction_plan, - ) - try: - if self.standalone_thumb_calibration: - payload = build_standalone_thumb_payload( - profile=self.profile, - serial_number=self.serial_number, - measured_fits=self.measured_fits, - thumb_offsets_rad={ - name: published_zero_offsets[name] - for name in ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - ) - }, - validation_errors_rad=self.validation_errors_rad, - baseline_command_u8=self.baseline_command, - source_urdf_sha256=_file_sha256( - self.source_urdf_path - ), - camera_extrinsics_sha256=_file_sha256( - self.camera_extrinsics_file - ), - corrected_urdf_sha256=_file_sha256( - self.corrected_urdf_path - ), - ) - else: - payload = build_compact_payload( - serial_number=self.serial_number, - measured_fits=self.measured_fits, - urdf_zero_offsets_rad=published_zero_offsets, - validation_errors_rad=self.validation_errors_rad, - passed=passed, - baseline=self.baseline_command, - side=self.hand_type, - layout_id=self.profile.layout_id, - zero_uncertainty_rad=( - self.zero_result.offset_confidence_half_width_rad - ), - zero_cycle_offsets_rad=self.zero_result.cycle_offsets_rad, - zero_observers=self.zero_profile.offset_observer_joint, - artifact_hashes=( - { - "source_urdf_sha256": _file_sha256( - self.source_urdf_path - ), - "camera_extrinsics_sha256": _file_sha256( - self.camera_extrinsics_file - ), - "corrected_urdf_sha256": _file_sha256( - self.corrected_urdf_path - ), - } - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - cross_view_roll_metrics=getattr( - self, "cross_view_roll_metrics", {} - ), - joint_dynamic_diagnostics=getattr( - self, "joint_dynamic_diagnostics", {} - ), - zero_geometry_diagnostics=( - { - "training_cycles": self.zero_result.training_cycles, - "validation_cycle": self.zero_result.validation_cycle, - "axis_line_rms_m": self.zero_result.axis_line_rms_m, - "validation_line_error_by_joint_m": ( - self.zero_result.validation_line_error_by_joint_m - ), - "observability_rank": ( - self.zero_result.observability_rank - ), - "observability_parameter_count": ( - self.zero_result.observability_parameter_count - ), - "observability_condition_number": ( - self.zero_result.observability_condition_number - ), - "offset_covariance_rad2": ( - self.zero_result.offset_covariance_rad2 - ), - "palm_orientation_sources": [ - { - "source_joint": item.source_joint, - "model_joint": item.model_joint, - "cycle": item.cycle, - "observed_arc_rad": item.observed_arc_rad, - "rotation_orthogonal_rms_rad": ( - item.rotation_orthogonal_rms_rad - ), - "axis_estimator": item.axis_estimator, - "incremental_pair_count": ( - item.incremental_pair_count - ), - } - for item in getattr( - self, - "palm_orientation_measurements", - (), - ) - ], - "palm_orientation_rejections": dict( - getattr( - self, - "palm_orientation_rejections", - {}, - ) - ), - } - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - ) - # The JSON is the commit marker for the URDF/JSON pair. It is - # atomically renamed only after the corrected URDF and all three - # artifact hashes have been produced and schema-validated. - atomic_write_json(self.final_path, payload) - except Exception: - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - rejected = self.corrected_urdf_path.with_suffix( - ".urdf.rejected" - ) - self.corrected_urdf_path.replace(rejected) - self.corrected_urdf_path = None - raise - self.completed_payload = payload - self.state = STATE_COMPLETE - self.reason = "calibration_passed" if passed else "quality_failed" - self.get_logger().info( - f"Three-camera calibration complete: passed={passed}; {self.final_path}" - ) - - def _pause(self, reason: str) -> None: - self._publish_hold_current() - self.paused_reason = str(reason) - self.state = STATE_PAUSED - 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) - return - retries += 1 - self.motion_retry_counts[reason] = retries - reset_views: list[str] = [] - task_reference_preserved = False - if ( - reason == "sweep_start_tag_timeout" - and self.state == STATE_PREPARE_SWEEP - and self.active_sweep is not None - ): - # Re-publishing the same endpoint command cannot repair a PnP - # tracker stuck on a planar mirror branch. Give every active - # camera a genuinely fresh bounded initialization window while - # retaining the task-level endpoint reference established by the - # visibility precheck/earlier cycles. - profile = _node_profile(self) - task_reference_preserved = ( - _preserve_pnp_task_reference_for_sweep( - self.active_sweep, - is_fit_retry=bool( - getattr(self, "active_sweep_is_fit_retry", False) - ), - has_precheck_anchor=( - profile.layout_id == G20_RIGHT_19_LAYOUT - ), - ) - ) - reset_trackers = getattr(self, "_reset_view_trackers", None) - reset_diagnostics = getattr( - self, "_reset_view_pnp_diagnostics", None - ) - for view in _sweep_views(profile, self.active_sweep.spec): - runtime = getattr(self, "views", {}).get(view) - if runtime is None or not callable(reset_trackers): - continue - reset_trackers( - runtime, - preserve_task_reference=task_reference_preserved, - ) - if callable(reset_diagnostics): - reset_diagnostics(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - reset_views.append(view) - getattr(self, "sweep_start_frames", []).clear() - append_jsonl( - self.raw_path, - { - "kind": "automatic_motion_retry", - "state": self.state, - "reason": reason, - "retry": retries, - "retry_limit": self.automatic_motion_retry_limit, - "pnp_trackers_reset": reset_views, - "task_reference_preserved": task_reference_preserved, - }, - ) - self.motion_stage_started_at = now - self.position_hold_since = None - if self.state == STATE_RETURN_BASELINE: - self._reset_motion_progress(now, self._baseline_error_u8()) - return_command = ( - G20ThreeCameraCalibrationNode._current_return_command(self) - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, return_command - ) - ) - self._publish_command( - list(return_command) - ) - elif self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: - preparation_command = getattr( - self, - "preparation_command_u8", - tuple( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ), - ) - self._reset_motion_progress( - now, - self._command_vector_error_u8(preparation_command), - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, preparation_command - ) - ) - self._publish_command(list(preparation_command)) - 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 - item = self.active_validation - key = (_sweep_storage_key(item.spec), item.command_u8) - retries = self.validation_retry_counts.get(key, 0) - retry_limit = self.automatic_sweep_retry_limit - if retries >= retry_limit: - self._pause(reason) - return - retries += 1 - self.validation_retry_counts[key] = retries - for view in _sweep_views(_node_profile(self), item.spec): - runtime = self.views[view] - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 - self.validation_frames_buffer.clear() - self.validation_stage_started_at = now - self._reset_motion_progress( - now, - self._motion_command_error_u8(item.spec, item.command_u8), - ) - self.position_hold_since = None - self.state = STATE_VALIDATION_MOVE - append_jsonl( - self.raw_path, - { - "kind": "automatic_validation_retry", - "view": item.spec.view, - "motor_index": item.spec.motor_index, - "requested_command_u8": item.command_u8, - "reason": reason, - "retry": retries, - "retry_limit": retry_limit, - }, - ) - self._publish_speed_profile(self._speed_profile_for_spec(item.spec)) - self._publish_command( - build_calibration_motion_command( - item.spec, - item.command_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - self.reason = f"automatic_retry_{reason}" - - def _timer_callback(self) -> None: - now = time.monotonic() - try: - self._advance(now) - except Exception as error: - self.get_logger().error( - f"Calibration paused: {error}\n{traceback.format_exc()}" - ) - self._pause(str(error)) - if now - self.last_status_publish >= 0.5: - self.last_status_publish = now - self._publish_status(now) - - def _advance(self, now: float) -> None: - if self.state == STATE_PREFLIGHT: - if not self.started: - if self._all_devices_ready(now): - self.state = STATE_WAIT_START - self.reason = "call_start_for_baseline_recovery" - elif self.startup_baseline_recovered and self._all_preflight_ready(now): - locker = getattr(self, "_lock_fixed_base_references", None) - if locker is None or locker(): - if getattr(self, "resume_checkpoint_pending", False): - self.resume_checkpoint_pending = False - self.state = STATE_IMPORTING_BASE - self.reason = "reading_base_session_records" - self.base_import_progress = { - "phase": "reading", - "records_read": 0, - "bytes_read": 0, - "total_bytes": int( - self.resume_raw_samples_path.stat().st_size - ), - "fraction": 0.0, - } - self._publish_status(time.monotonic()) - self._restore_durable_task_checkpoint() - self._start_next_sweep() - else: - self.reason = "locking_fixed_base_references" - return - if self.state == STATE_WAIT_START: - if not self._all_devices_ready(now): - self.state = STATE_PREFLIGHT - self.reason = "device_preflight_lost" - return - if self.state == STATE_RETURN_BASELINE: - baseline_reached = self._baseline_reached() - baseline_details = self._baseline_error_details() - if ( - not baseline_reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(baseline_details["error_u8"]), - context="return_baseline", - details=baseline_details, - ) - ): - return - if baseline_reached: - return_waypoints = getattr( - self, "return_waypoints", deque() - ) - if return_waypoints: - # Intermediate avoidance waypoints need confirmed feedback, - # but only the final baseline needs the dedicated 0.5 s - # backlash hold. This removes several seconds of no-op - # waiting from every multi-finger return sequence. - self.return_command_u8 = return_waypoints.popleft() - self.position_hold_since = None - self.motion_stage_started_at = now - self._reset_motion_progress( - now, self._baseline_error_u8() - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.return_command_u8 - ) - ) - self._publish_command( - list(self.return_command_u8) - ) - return - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self.baseline_hold_seconds: - after = self.baseline_after - self.motion_retry_counts.pop("return_baseline_timeout", None) - self.position_hold_since = None - if after in { - "next_sweep", - "next_cycle", - "next_task_same_finger", - "resume_sweep", - "retry_sweep", - }: - self._start_next_sweep() - elif after == "fit": - self._fit_all_curves() - elif after in {"validation_next", "resume_validation"}: - self._start_next_validation() - elif after == "combination_next": - self._start_next_combination_validation() - elif after == "finalize": - self._finalize() - elif after == "startup_tag_preflight": - # Discard observations and PnP branches from the - # interrupted pose. The normal preflight then requires - # a fresh window of all three fixed palm Tags while the - # hand is confirmed at baseline. - self._finish_startup_baseline_recovery() - elif after == "abort": - self.state = STATE_ABORTED - self.reason = ( - "safe_abort_complete:" - f"{self.abort_original_reason or 'operator_abort'}" - ) - else: - self.position_hold_since = None - return - if self.state == STATE_PREPARE_SWEEP: - assert self.active_sweep is not None - preparation_command = getattr( - self, - "preparation_command_u8", - tuple( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ), - ) - reached = self._command_vector_reached(preparation_command) - 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}" - ) - prepare_details = self._command_vector_error_details( - preparation_command, prepare_context - ) - if ( - not reached - and self._pause_if_motion_stalled( - now=now, - error_u8=self._command_vector_error_u8( - preparation_command - ), - context=prepare_context, - details=prepare_details, - ) - ): - return - preparation_waypoints = getattr( - self, "preparation_waypoints", deque() - ) - if reached and preparation_waypoints: - self.preparation_command_u8 = ( - preparation_waypoints.popleft() - ) - self.motion_stage_started_at = now - self.position_hold_since = None - self._reset_motion_progress( - now, - self._command_vector_error_u8( - self.preparation_command_u8 - ), - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.preparation_command_u8 - ) - ) - self._publish_command(list(self.preparation_command_u8)) - return - if reached: - # Pose-entry waypoints use conservative joint-class speeds. - # Switch to the accepted task scan speed only after the full - # start pose has been reached, then honour the SDK settle time. - self._publish_speed_profile( - self._speed_profile_for_spec(self.active_sweep.spec) - ) - speed_ready = bool( - now - self.speed_commanded_at - >= self.speed_setting_settle_seconds - ) - # Do not leave the start endpoint until every contributing camera - # has enough timestamp-synchronised observations. The retained - # frames become the strict 0/255 endpoint bin and, in the first - # training round, the first steady command checkpoint. - minimum_start_frames = ( - 3 - if _steady_checkpoint_commands( - _node_profile(self), self.active_sweep - ) - else 1 - ) - start_frames_ready = _frames_cover_sweep_joints( - self.sweep_start_frames, - self.active_sweep.spec, - minimum_start_frames, - ) - if reached and speed_ready and not start_frames_ready: - self.reason = "waiting_for_task_tags_at_sweep_start" - if ( - reached - and speed_ready - and start_frames_ready - ): - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self._active_endpoint_hold_seconds(): - self.motion_retry_counts.pop( - "sweep_start_position_timeout", None - ) - self.motion_retry_counts.pop( - "sweep_start_tag_timeout", None - ) - self.position_hold_since = None - self._begin_active_sweep(now) - else: - self.position_hold_since = None - return - if self.state == STATE_SWEEP: - assert self.active_sweep is not None - motor = self.active_sweep.spec.motor_index - # 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 - ) - checkpoint_active = bool( - getattr(self, "sweep_checkpoint_mode", False) - and checkpoint_target_value is not None - and not getattr(self, "sweep_baseline_pending", False) - ) - motion_target_u8 = ( - int(self.baseline_command[motor]) - if getattr(self, "sweep_baseline_pending", False) - else int( - checkpoint_target_value - if checkpoint_target_value is not None - else self.active_sweep.target_u8 - ) - ) - target_reached = ( - self._steady_checkpoint_reached( - self.active_sweep.spec, motion_target_u8 - ) - if checkpoint_active - else self._motion_command_reached( - self.active_sweep.spec, motion_target_u8 - ) - ) - sweep_context = ( - f"sweep_baseline_motor_{motor}" - if getattr(self, "sweep_baseline_pending", False) - else f"sweep_motor_{motor}" - ) - sweep_details = self._motion_command_error_details( - self.active_sweep.spec, - motion_target_u8, - sweep_context, - ) - if ( - checkpoint_active - and int(sweep_details.get("motor_index", -1)) == motor - ): - sweep_details["tolerance_u8"] = max( - float( - self.steady_checkpoint_command_feedback_tolerance_u8 - ), - float(sweep_details["tolerance_u8"]), - ) - if ( - not target_reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(sweep_details["error_u8"]), - context=sweep_context, - details=sweep_details, - ) - ): - return - if getattr(self, "sweep_baseline_pending", False): - if target_reached: - if self.sweep_baseline_hold_since is None: - # Frames acquired during deceleration into 127 are not - # static observations. Wait baseline_hold_seconds - # after feedback first reaches 127, then collect a - # fresh multi-frame baseline sample. - self.sweep_baseline_frames.clear() - self.sweep_baseline_hold_since = now - elif ( - now - self.sweep_baseline_hold_since - >= self.baseline_hold_seconds - and _frames_cover_sweep_joints( - self.sweep_baseline_frames, - self.active_sweep.spec, - self.minimum_baseline_hold_frames, - ) - ): - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since = None - self.sweep_endpoint_since = None - self.reason = ( - "collecting_timestamp_synchronised_tag_centres" - ) - if getattr(self, "sweep_checkpoint_mode", False): - self._publish_next_checkpoint(now) - else: - self._reset_motion_progress( - now, - self._motion_command_error_u8( - self.active_sweep.spec, - self.active_sweep.target_u8, - ), - ) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.target_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - else: - self.sweep_baseline_hold_since = None - self.sweep_baseline_frames.clear() - return - if getattr(self, "sweep_checkpoint_mode", False): - checkpoint_target = self.sweep_checkpoint_target_u8 - if checkpoint_target is None: - if self.sweep_checkpoint_commands: - # Recover a missing in-flight target by publishing the - # next queued checkpoint instead of aborting a healthy - # hardware session. - self._publish_next_checkpoint(now) - return - self.sweep_checkpoint_mode = False - else: - if target_reached: - if self.sweep_checkpoint_hold_since is None: - self.sweep_checkpoint_frames.clear() - self.sweep_checkpoint_hold_since = now - elif ( - now - self.sweep_checkpoint_hold_since - >= self.endpoint_hold_seconds - and _frames_cover_sweep_joints( - self.sweep_checkpoint_frames, - self.active_sweep.spec, - 3, - ) - ): - if not self._steady_checkpoint_feedback_is_stable( - self.active_sweep.spec, - self.sweep_checkpoint_frames, - ): - # The command/feedback offset is acceptable, - # but feedback is still settling inside that - # band. Restart the hold window without - # issuing another command or weakening the - # mechanical-stall gate. - self.sweep_checkpoint_hold_since = now - self.sweep_checkpoint_frames.clear() - return - self._record_command_checkpoint( - self.active_sweep, - int(checkpoint_target), - list(self.sweep_checkpoint_frames), - ) - self.sweep_checkpoint_hold_since = None - self.sweep_checkpoint_frames.clear() - if self._publish_next_checkpoint(now): - return - self.sweep_endpoint_since = now - else: - self.sweep_checkpoint_hold_since = None - self.sweep_checkpoint_frames.clear() - if self.sweep_checkpoint_target_u8 is not None: - return - if target_reached: - if self.sweep_endpoint_since is None: - self.sweep_endpoint_since = now - else: - self.sweep_endpoint_since = None - if ( - self.sweep_endpoint_since is not None - and now - self.sweep_endpoint_since - >= self._active_endpoint_hold_seconds() - ): - self._finish_active_sweep() - return - if self.state == STATE_VALIDATION_MOVE: - if self.active_combination_validation is not None: - command = self.active_combination_validation.command_u8 - reached = self._command_vector_reached(command) - details = self._command_vector_error_details( - command, - f"combination_{self.active_combination_validation.name}", - ) - if ( - not reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(details["error_u8"]), - context=str(details["stage"]), - details=details, - ) - ): - return - if reached: - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self.endpoint_hold_seconds: - self.position_hold_since = None - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.validation_stage_started_at = now - self.state = STATE_VALIDATION_CAPTURE - self.reason = ( - "combination_validation_capture_" - f"{self.active_combination_validation.name}" - ) - else: - self.position_hold_since = None - return - assert self.active_validation is not None - reached = self._motion_command_reached( - self.active_validation.spec, - self.active_validation.command_u8, - ) - validation_context = ( - "validation_motor_" - f"{self.active_validation.spec.motor_index}" - ) - validation_details = self._motion_command_error_details( - self.active_validation.spec, - self.active_validation.command_u8, - validation_context, - ) - if ( - not reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(validation_details["error_u8"]), - context=validation_context, - details=validation_details, - ) - ): - return - speed_ready = bool( - now - self.speed_commanded_at - >= self.speed_setting_settle_seconds - ) - if reached and speed_ready: - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self.endpoint_hold_seconds: - self.position_hold_since = None - self.validation_frames_buffer.clear() - self.validation_stage_started_at = now - self.state = STATE_VALIDATION_CAPTURE - self.reason = "capturing_random_validation_pose" - else: - self.position_hold_since = None - return - if self.state == STATE_VALIDATION_CAPTURE: - if self.active_combination_validation is not None: - if all( - len(frames) >= self.combination_validation_frames - for frames in self.combination_validation_frames_buffer.values() - ): - self._finish_combination_validation_capture() - elif now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_combination_validation_or_pause( - "combination_validation_capture_timeout", now - ) - return - if _frames_cover_sweep_joints( - self.validation_frames_buffer, - self.active_validation.spec, - self.validation_frames, - ): - self._finish_validation_capture() - elif now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_validation_or_pause( - "validation_capture_timeout", now - ) - - def _publish_status(self, now: float) -> None: - views: dict[str, dict[str, Any]] = {} - for name, runtime in self.views.items(): - required_roles = set(self._required_roles_for_view(name)) - live_required_roles = set( - self._live_required_roles_for_view( - name, tuple(required_roles) - ) - ) - locked_role = self._locked_base_role_for_active_capture(name) - locked_roles = set(() if locked_role is None else (locked_role,)) - views[name] = { - "ready": self._view_ready(runtime, now), - "stream_alive": bool( - runtime.last_message_at > 0.0 - and now - runtime.last_message_at <= 1.0 - ), - "camera_info_valid": runtime.camera_info_valid, - "camera_extrinsics_valid": runtime.extrinsics_valid, - "detection_hz": round(runtime.detection_hz, 2), - "valid_rate": round(runtime.valid_rate, 4), - "required_tag_ids": sorted( - runtime.view_tags[role] for role in required_roles - ), - "locked_reference_tag_ids": sorted( - runtime.view_tags[role] for role in locked_roles - ), - "locked_base_corner_drift_px": round( - runtime.latest_locked_base_corner_drift_px, 4 - ), - "locked_base_maximum_corner_drift_px": ( - self.fixed_base_maximum_corner_drift_px - ), - "locked_base_corner_drift_count": ( - runtime.locked_base_corner_drift_count - ), - "configured_tag_ids": sorted(runtime.view_tags.values()), - "visible_configured_tag_ids": sorted( - tag_id - for role, tag_id in runtime.view_tags.items() - if role in runtime.latest_tag_quality - and self._quality_valid( - runtime.latest_tag_quality[role], include_pnp=False - ) - ), - "detected_tag_ids": sorted( - tag_id - for tag_id, role in runtime.role_by_id.items() - if role in required_roles - and role in runtime.latest_tag_quality - ), - "missing_tag_ids": sorted( - tag_id - for tag_id, role in runtime.role_by_id.items() - if role in live_required_roles - and role not in runtime.latest_tag_quality - ), - "pnp_rejections": dict(runtime.latest_pnp_rejections), - "group_pnp_reason": runtime.latest_group_pnp_reason, - "group_missing_candidate_roles": list( - runtime.latest_group_missing_candidate_roles - ), - "pnp_candidate_diagnostics": { - role: { - "tag_id": runtime.view_tags[role], - **dict(diagnostics), - } - for role, diagnostics in ( - runtime.tracker.last_candidate_diagnostics_by_role.items() - ) - if role in required_roles and role not in locked_roles - }, - "pnp_pose_valid": runtime.latest_pnp_valid, - "pnp_rejection_counts": dict( - runtime.pnp_rejection_counts - ), - "group_pnp_rejection_counts": dict( - runtime.group_pnp_rejection_counts - ), - "pnp_initialization_progress": ( - None - if runtime.pnp_initialization_progress is None - else { - "accepted": runtime.pnp_initialization_progress[0], - "required": runtime.pnp_initialization_progress[1], - } - ), - "pnp_reset_count": runtime.pnp_reset_count, - "pnp_invalid_seconds": ( - 0.0 - if runtime.pnp_invalid_since is None - else round(max(0.0, now - runtime.pnp_invalid_since), 3) - ), - } - active: dict[str, Any] = {} - if self.motion_stall_details: - active = dict(self.motion_stall_details) - elif self.active_sweep is not None: - motor = self.active_sweep.spec.motor_index - start_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, - self.active_sweep.spec, - self.active_sweep.start_u8, - ) - ) - target_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, - self.active_sweep.spec, - self.active_sweep.target_u8, - ) - ) - mechanical_start_tolerance = self._endpoint_tolerance_for_spec( - self.active_sweep.spec, self.active_sweep.start_u8 - ) - mechanical_target_tolerance = self._endpoint_tolerance_for_spec( - self.active_sweep.spec, self.active_sweep.target_u8 - ) - values = [ - float(frame.state_u8[motor]) for frame in self.sweep_frames - ] - bins = sorted( - { - ( - self.active_sweep.start_u8 - if abs(value - self.active_sweep.start_u8) - <= start_tolerance - else self.active_sweep.target_u8 - if abs(value - self.active_sweep.target_u8) - <= target_tolerance - else int(np.clip(np.rint(value), 0, 255)) - ) - for value in values - } - ) - adjacent_bin_gaps = [ - (right - left, left, right) - for left, right in zip(bins, bins[1:]) - ] - maximum_bin_gap, gap_start_u8, gap_end_u8 = max( - adjacent_bin_gaps, - default=(0, None, None), - ) - missing_endpoints = [ - endpoint for endpoint in (0, 255) if endpoint not in bins - ] - actual = ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[motor]) - ) - motion_progress = None - if actual is not None: - motion_progress = float( - np.clip( - abs(actual - self.active_sweep.start_u8) / 255.0, - 0.0, - 1.0, - ) - ) - storage_key = _sweep_storage_key(self.active_sweep.spec) - frame_counts = { - name: len(_frames_for_joint(self.sweep_frames, name)) - for name in self.active_sweep.spec.joints - } - baseline_frame_counts = { - name: len( - _frames_for_joint(self.sweep_baseline_frames, name) - ) - for name in self.active_sweep.spec.joints - } - detection_rates_by_view = { - view: ( - 0.0 - if getattr( - self, "sweep_detection_total_by_view", {} - ).get(view, 0) <= 0 - else getattr( - self, "sweep_detection_valid_by_view", {} - ).get(view, 0) - / getattr( - self, "sweep_detection_total_by_view", {} - )[view] - ) - for view in _sweep_views( - self.profile, self.active_sweep.spec - ) - } - active = { - "kind": "sweep", - "view": self.active_sweep.spec.view, - "motor_index": motor, - "joints": list(self.active_sweep.spec.joints), - "cycle": self.active_sweep.cycle + 1, - "repetitions": self.repetitions, - "direction": self.active_sweep.direction, - "task_name": self.active_sweep.spec.key, - "fit_attempt": self.sweep_attempts.get( - storage_key, 1 - ), - "fit_attempt_limit": self.automatic_fit_retry_limit + 1, - "fit_retry_cycles": ( - [ - cycle + 1 - for cycle in sorted( - getattr(self, "retry_cycles", set()) - ) - ] - if getattr(self, "active_sweep_is_fit_retry", False) - else [] - ), - "automatic_retry_count": max( - self.sweep_retry_counts.get( - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, - ), - 0, - ), - self.motion_retry_counts.get( - "sweep_start_tag_timeout", 0 - ), - self.motion_retry_counts.get( - "sweep_start_position_timeout", 0 - ), - ), - "automatic_retry_limit": self.automatic_sweep_retry_limit, - "retry_speed_scale": ( - 1.0 - if self.sweep_retry_counts.get( - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, - ), - 0, - ) - == 0 - else self.retry_speed_scales[ - min( - self.sweep_retry_counts[ - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, - ) - ], - len(self.retry_speed_scales), - ) - - 1 - ] - ), - "endpoint_hold_seconds": self._active_endpoint_hold_seconds(), - "sweep_timeout_seconds": self._active_sweep_timeout_seconds(), - "motor_stall_timeout_seconds": self.motor_stall_timeout_seconds, - "motor_stall_minimum_progress_u8": ( - self.motor_stall_minimum_progress_u8 - ), - "start_u8": self.active_sweep.start_u8, - "target_u8": self.active_sweep.target_u8, - "current_motion_target_u8": ( - int( - getattr( - self, - "preparation_command_u8", - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ), - )[motor] - ) - if self.state == STATE_PREPARE_SWEEP - else int(self.baseline_command[motor]) - if getattr(self, "sweep_baseline_pending", False) - else int( - self.sweep_checkpoint_target_u8 - if getattr(self, "sweep_checkpoint_target_u8", None) - is not None - else self.active_sweep.target_u8 - ) - ), - "steady_checkpoint_mode": bool( - getattr(self, "sweep_checkpoint_mode", False) - ), - "remaining_steady_checkpoints": len( - getattr(self, "sweep_checkpoint_commands", ()) - ), - "baseline_hold_pending": bool( - getattr(self, "sweep_baseline_pending", False) - ), - "baseline_hold_valid_frames": min( - baseline_frame_counts.values(), default=0 - ), - "minimum_baseline_hold_frames": int( - getattr(self, "minimum_baseline_hold_frames", 10) - ), - "actual_u8": actual, - "motion_progress": motion_progress, - "valid_frames": min(frame_counts.values(), default=0), - "valid_frames_by_joint": frame_counts, - "detection_frames": int( - getattr(self, "sweep_detection_total_frames", 0) - ), - "detection_valid_frames": int( - getattr(self, "sweep_detection_valid_frames", 0) - ), - "detection_rate": min( - detection_rates_by_view.values(), default=0.0 - ), - "detection_rate_by_view": detection_rates_by_view, - "sample": { - "minimum_u8": None if not values else min(values), - "maximum_u8": None if not values else max(values), - "span_u8": 0.0 if not values else max(values) - min(values), - "bin_count": len(bins), - "minimum_bin_count": self.minimum_sweep_bins, - "maximum_bin_gap": int(maximum_bin_gap), - "maximum_bin_gap_start_u8": gap_start_u8, - "maximum_bin_gap_end_u8": gap_end_u8, - "allowed_maximum_bin_gap": self.maximum_bin_gap, - "missing_endpoint_u8": missing_endpoints, - "endpoint_tolerance_u8": max( - start_tolerance, target_tolerance - ), - "start_endpoint_tolerance_u8": start_tolerance, - "target_endpoint_tolerance_u8": target_tolerance, - "mechanical_endpoint_tolerance_u8": max( - mechanical_start_tolerance, - mechanical_target_tolerance, - ), - "synchronised_endpoint_tolerance_margin_u8": float( - getattr( - self, - "synchronised_endpoint_tolerance_margin_u8", - 1.0, - ) - ), - }, - "auxiliary_motors": [ - { - "motor_index": index, - "command_u8": command, - "actual_u8": ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[index]) - ), - } - for index, command in calibration_auxiliary_commands( - self.active_sweep.spec, profile=self.profile - ).items() - ], - "speed": { - "commanded_finger_speed": self._speed_profile_for_spec( - self.active_sweep.spec - ), - "reported_finger_speed": self.latest_hand_info.get("speed"), - "normal_speed": self.normal_calibration_speed, - "index_roll_speed": self.index_roll_calibration_speed, - "index_flex_speed": self.index_flex_calibration_speed, - "adaptive_formal_speed_scale": float( - self.formal_speed_scales.get( - self.active_sweep.spec.key, 1.0 - ) - ), - }, - } - elif self.active_validation is not None: - motor = self.active_validation.spec.motor_index - active = { - "kind": "validation", - "view": self.active_validation.spec.view, - "motor_index": motor, - "command_u8": self.active_validation.command_u8, - "actual_u8": ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[motor]) - ), - "valid_frames": len(self.validation_frames_buffer), - "auxiliary_motors": [ - { - "motor_index": index, - "command_u8": command, - "actual_u8": ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[index]) - ), - } - for index, command in calibration_auxiliary_commands( - self.active_validation.spec, profile=self.profile - ).items() - ], - "speed": { - "commanded_finger_speed": self._speed_profile_for_spec( - self.active_validation.spec - ), - "reported_finger_speed": self.latest_hand_info.get("speed"), - "normal_speed": self.normal_calibration_speed, - "index_roll_speed": self.index_roll_calibration_speed, - "index_flex_speed": self.index_flex_calibration_speed, - "adaptive_formal_speed_scale": float( - self.formal_speed_scales.get( - self.active_validation.spec.key, 1.0 - ) - ), - }, - } - elif self.fit_failure: - active = dict(self.fit_failure) - group_pnp_reasons = { - name: runtime.latest_group_pnp_reason - for name, runtime in self.views.items() - if runtime.latest_group_pnp_reason - } - if group_pnp_reasons: - active = dict(active) - active["group_pnp_reasons"] = group_pnp_reasons - pnp_rejection_counts = { - name: dict(runtime.pnp_rejection_counts) - for name, runtime in self.views.items() - if runtime.pnp_rejection_counts - } - if pnp_rejection_counts: - active = dict(active) - active["pnp_rejection_counts"] = pnp_rejection_counts - group_pnp_rejection_counts = { - name: dict(runtime.group_pnp_rejection_counts) - for name, runtime in self.views.items() - if runtime.group_pnp_rejection_counts - } - if group_pnp_rejection_counts: - active = dict(active) - active["group_pnp_rejection_counts"] = ( - group_pnp_rejection_counts - ) - pnp_initialization_progress = { - name: { - "accepted": runtime.pnp_initialization_progress[0], - "required": runtime.pnp_initialization_progress[1], - } - for name, runtime in self.views.items() - if runtime.pnp_initialization_progress is not None - } - if pnp_initialization_progress: - active = dict(active) - active["pnp_initialization_progress"] = ( - pnp_initialization_progress - ) - resumed_tasks = set(self.resumed_task_keys) - completed_sweep_count = sum( - 1 - for index, item in enumerate(self.sweep_items) - if index < self.sweep_index or item.spec.key in resumed_tasks - ) - scan_progress = ( - 0.0 - if not self.sweep_items - else float( - np.clip( - completed_sweep_count / len(self.sweep_items), 0.0, 1.0 - ) - ) - ) - status_validation_index = ( - self.combination_validation_index - if self.combination_validation_items - else self.validation_index - ) - status_validation_total = ( - len(self.combination_validation_items) - if self.combination_validation_items - else len(self.validation_items) - ) - payload = { - "serial_number": self.serial_number, - "session_dir": str(self.session_dir), - "hand_type": self.hand_type, - "reference_finger": self.profile.reference_finger, - "state": self.state, - "reason": self.reason, - "preflight_mode": ( - "recovering_baseline" - if self.state == STATE_RETURN_BASELINE - and self.baseline_after == "startup_tag_preflight" - else ( - "device_only_before_baseline" - if self.state in {STATE_PREFLIGHT, STATE_WAIT_START} - and not self.started - else ( - "baseline_tags_after_recovery" - if self.state == STATE_PREFLIGHT - and self.started - and self.startup_baseline_recovered - else "" - ) - ) - ), - "progress": round( - _overall_progress( - self.state, - scan_progress=scan_progress, - validation_index=status_validation_index, - validation_total=status_validation_total, - ), - 4, - ), - "scan_progress": round(scan_progress, 4), - "completed_sweeps": self.sweep_index, - "total_sweeps": len(self.sweep_items), - "baseline_command_u8": list(self.baseline_command), - "return_command_u8": list( - G20ThreeCameraCalibrationNode._current_return_command(self) - ), - "hand_state_fresh": bool( - len(self.latest_state_u8) == 20 - and now - self.last_state_at <= 1.0 - ), - "feedback_hz": round( - ( - 0.0 - if len(self.state_receive_times) < 2 - or self.state_receive_times[-1] - <= self.state_receive_times[0] - else (len(self.state_receive_times) - 1) - / ( - self.state_receive_times[-1] - - self.state_receive_times[0] - ) - ), - 2, - ), - "views": views, - "active": active, - "result_path": ( - str(self.final_path) if self.completed_payload is not None else "" - ), - "corrected_urdf_path": ( - "" - if self.corrected_urdf_path is None - else str(self.corrected_urdf_path) - ), - "camera_extrinsics_error": self.extrinsics_error, - "resume": { - "used": bool(self.resumed_task_keys), - "checkpoint_requested": bool( - self.resume_raw_samples_path is not None - ), - "checkpoint_pending": bool( - getattr(self, "resume_checkpoint_pending", False) - ), - "source_session": self.resume_source_session, - "start_position_policy": getattr( - self, "resume_position_policy", "not_requested" - ), - "start_position_changed_views": list( - getattr(self, "resume_position_changed_views", ()) - ), - "start_position_unverifiable_views": list( - getattr( - self, - "resume_position_unverifiable_views", - (), - ) - ), - "start_position_drift_by_view_px": { - view: round(float(value), 6) - for view, value in getattr( - self, "resume_position_drift_by_view_px", {} - ).items() - }, - "recalibration_scope": self.recalibration_scope, - "recalibration_task_keys": list( - self.recalibration_task_keys - ), - "completed_task_count": len(self.resumed_task_keys), - "total_task_count": len(self.profile.sweep_specs), - "completed_task_keys": list(self.resumed_task_keys), - }, - "base_import": dict( - getattr(self, "base_import_progress", {}) - ), - "quality": ( - {} if self.completed_payload is None else self.completed_payload["quality"] - ), - "thumb_yaw_cross_session": dict( - getattr(self, "thumb_yaw_cross_session_diagnostic", {}) - ), - "combination_validation": { - "enabled": bool(self.combination_validation_enabled), - "completed": bool(self.combination_validation_completed), - "completed_poses": int(self.combination_validation_index), - "total_poses": len(self.combination_validation_items), - "position_p95_m": ( - None - if not self.combination_position_errors_m - else round( - float( - np.percentile( - self.combination_position_errors_m, 95.0 - ) - ), - 8, - ) - ), - "orientation_p95_rad": ( - None - if not self.combination_orientation_errors_rad - else round( - float( - np.percentile( - self.combination_orientation_errors_rad, 95.0 - ) - ), - 8, - ) - ), - **combination_target_coverage( - self.combination_tag_observation_counts, - self.combination_tag_validation_counts, - ), - }, - } - status = String() - status.data = json.dumps(payload, ensure_ascii=False) - self.status_publisher.publish(status) - text = String() - text.data = render_three_camera_status_text_zh(payload) - self.status_text_publisher.publish(text) - - -def main(args: list[str] | None = None) -> None: - configure_fastdds_large_image_transport() - rclpy.init(args=args) - node: G20ThreeCameraCalibrationNode | None = None - try: - node = G20ThreeCameraCalibrationNode() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py deleted file mode 100644 index 58731bb..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py +++ /dev/null @@ -1,736 +0,0 @@ -"""One-command runner for a registered hand-calibration product.""" - -from __future__ import annotations - -import argparse -from datetime import datetime -import json -import os -from pathlib import Path -import signal -import subprocess -import sys -import time -import traceback -from typing import Any, Mapping - -from ament_index_python.packages import get_package_share_directory -import rclpy -from rclpy.node import Node -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from ...hikrobot_camera import configure_fastdds_large_image_transport -from ...operator_report import ( - ProgressEstimator, - build_failure_report, - render_progress_zh, -) -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 -EXIT_QUALITY = 2 -EXIT_SAFETY = 3 -STATUS_TIMEOUT_SECONDS = 90.0 -FITTING_STATUS_TIMEOUT_SECONDS = 600.0 - - -def _status_timeout_seconds(status: Mapping[str, Any]) -> float: - """Return the watchdog deadline for the node's current phase. - - Motion and acquisition are expected to publish twice a second and retain - the strict transport watchdog. The final 3-D fit is intentionally a - synchronous, CPU-bound operation, so its executor cannot service the - status timer until the fit returns. The node publishes an explicit - FITTING status immediately before entering that operation; allow that - known phase enough time without weakening motion safety checks. - """ - if str(status.get("state", "")) == "FITTING": - return FITTING_STATUS_TIMEOUT_SECONDS - return STATUS_TIMEOUT_SECONDS - - -def _calibration_node_exited_before_status(log_path: Path) -> bool: - """Detect a launch child crash while the parent launch is still alive.""" - try: - with log_path.open("rb") as stream: - stream.seek(0, os.SEEK_END) - size = stream.tell() - stream.seek(max(0, size - 65536), os.SEEK_SET) - tail = stream.read().decode("utf-8", errors="replace") - except OSError: - return False - return ( - "[three_camera_calibration_node-" in tail - and "]: process has died" in tail - ) - - -class CalibrationMonitor(Node): - def __init__(self) -> None: - super().__init__("g20_calibration_product_runner") - self.latest_status: dict[str, Any] = {} - self.last_status_at = time.monotonic() - self.start_requested = False - self.start_future: Any = None - self.abort_future: Any = None - self.create_subscription(String, "/g20_calibration/status", self._status, 10) - self.start_client = self.create_client(Trigger, "/g20_calibration/start") - self.abort_client = self.create_client(Trigger, "/g20_calibration/abort") - - def _status(self, message: String) -> None: - try: - payload = json.loads(message.data) - except (TypeError, json.JSONDecodeError): - return - if isinstance(payload, dict): - self.latest_status = payload - self.last_status_at = time.monotonic() - - def maybe_start(self) -> None: - if self.start_requested or self.latest_status.get("state") != "WAIT_START": - return - if not self.start_client.service_is_ready(): - self.start_client.wait_for_service(timeout_sec=0.05) - return - self.start_requested = True - self.start_future = self.start_client.call_async(Trigger.Request()) - - def abort(self) -> None: - if not self.abort_client.service_is_ready(): - self.abort_client.wait_for_service(timeout_sec=1.0) - if self.abort_client.service_is_ready(): - self.abort_future = self.abort_client.call_async(Trigger.Request()) - - -class ProgressConsole: - def __init__(self, serial_number: str) -> None: - self.serial_number = serial_number - self.estimator = ProgressEstimator.start() - self.last_text = "" - self.last_issue = "" - - def update(self, status: Mapping[str, Any]) -> None: - text = render_progress_zh(self.serial_number, status, self.estimator) - if text == self.last_text: - return - self.last_text = text - if sys.stdout.isatty(): - sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") - sys.stdout.flush() - else: - print(text, flush=True) - reason = str(status.get("reason", "")) - if reason.startswith("automatic_retry_") and reason != self.last_issue: - self.last_issue = reason - active = status.get("active", {}) - print( - "\n".join( - [ - f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}", - "系统处理：只按原速度重扫当前方向" - f"（第 {active.get('automatic_retry_count', 1)}/" - f"{active.get('automatic_retry_limit', 1)} 次）", - ] - ), - flush=True, - ) - - -def _default_product_config() -> Path: - try: - installed = Path( - get_package_share_directory("linkerhand_calibration") - ) / "config" / "g20_right_product.yaml" - if installed.is_file(): - return installed - except Exception: - pass - return ( - Path.cwd() - / "src/linkerhand_calibration/config/g20_right_product.yaml" - ).resolve() - - -def _launch_command( - config: ProductConfig, - session: Path, - *, - resume_from: Path | None = None, - recalibration_scope: str = "full", -) -> list[str]: - values = { - "model": config.model, - "hand_type": config.side, - "tag_layout": config.tag_layout, - "serial_number": config.serial_number, - "can_interface": config.can_interface, - "session_dir": str(session), - "output_root": str(config.output_root), - "camera_extrinsics_file": str(config.camera_extrinsics), - "source_urdf_path": str(config.source_urdf), - "source_urdf_expected_sha256": config.source_urdf_sha256, - "corrected_urdf_output_dir": str(session), - "calibration_config": str(config.calibration_config), - "tag_config": str(config.tag_config), - "commands_enabled": "true", - "start_cameras": "true", - "start_sdk": "true", - "record_bag": "false", - "validation_enabled": "false", - "recalibration_scope": recalibration_scope, - } - if resume_from is not None: - values["resume_raw_samples_path"] = str( - resume_from / "raw_samples.jsonl" - ) - for view, camera in config.cameras.items(): - values[f"{view}_camera_serial"] = camera["serial_number"] - values[f"{view}_camera_name"] = camera["camera_name"] - values[f"{view}_camera_info_url"] = camera["camera_info"] - return [ - "ros2", - "launch", - "linkerhand_calibration", - "three_camera_calibration.launch.py", - *(f"{name}:={value}" for name, value in values.items()), - ] - - -def _stop_stack(process: subprocess.Popen[Any]) -> None: - if process.poll() is not None: - return - try: - os.killpg(process.pid, signal.SIGINT) - except ProcessLookupError: - return - try: - process.wait(timeout=15.0) - except subprocess.TimeoutExpired: - try: - os.killpg(process.pid, signal.SIGTERM) - except ProcessLookupError: - return - try: - process.wait(timeout=5.0) - except subprocess.TimeoutExpired: - try: - os.killpg(process.pid, signal.SIGKILL) - except ProcessLookupError: - return - process.wait(timeout=5.0) - - -def _write_trace(log_path: Path, error: BaseException) -> None: - with log_path.open("a", encoding="utf-8") as stream: - stream.write("\n[one-command exception]\n") - traceback.print_exception(type(error), error, error.__traceback__, file=stream) - - -def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None: - monitor.abort() - deadline = time.monotonic() + float(timeout_seconds) - while time.monotonic() < deadline and rclpy.ok(): - rclpy.spin_once(monitor, timeout_sec=0.1) - if monitor.latest_status.get("state") == "ABORTED": - return - - -def _run_hardware_session( - config: ProductConfig, - session: Path, - *, - resume_from: Path | None = None, - recalibration_scope: str = "full", -) -> tuple[dict[str, Any], int]: - session.mkdir(parents=True, exist_ok=False) - (session / "raw_samples.jsonl").touch() - log_path = session / "calibration.log" - log_stream = log_path.open("a", encoding="utf-8", buffering=1) - atomic_session_pointer(config.session_root, "latest_attempt", session) - monitor = CalibrationMonitor() - console = ProgressConsole(config.serial_number) - process: subprocess.Popen[Any] | None = None - latest_status: dict[str, Any] = { - "state": "PREFLIGHT", - "reason": "starting_ros_stack", - "progress": 0.0, - "views": {}, - "feedback_hz": 0.0, - } - exit_code = EXIT_QUALITY - try: - process = subprocess.Popen( - _launch_command( - config, - session, - resume_from=resume_from, - recalibration_scope=recalibration_scope, - ), - cwd=config.workspace, - stdout=log_stream, - stderr=subprocess.STDOUT, - text=True, - start_new_session=True, - ) - launched_at = time.monotonic() - last_render = 0.0 - last_startup_log_check = 0.0 - while True: - rclpy.spin_once(monitor, timeout_sec=0.1) - if monitor.latest_status: - latest_status = monitor.latest_status - monitor.maybe_start() - now = time.monotonic() - if now - last_render >= 0.5: - console.update(latest_status) - last_render = now - if monitor.start_future is not None and monitor.start_future.done(): - response = monitor.start_future.result() - if response is None or not response.success: - message = "start service failed" if response is None else response.message - raise RuntimeError(f"CFG-START-008:{message}") - monitor.start_future = None - state = str(latest_status.get("state", "")) - if state == "COMPLETE": - exit_code = EXIT_PASS - break - if state in {"PAUSED", "ABORTED"}: - reason = str(latest_status.get("reason", "calibration_paused")) - exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY - if state == "PAUSED" and "stall" not in reason: - # Ordinary quality failures return to the reviewed baseline - # before the process tree is stopped. Mechanical stalls - # deliberately skip this path and keep the current pose. - failure_status = dict(latest_status) - _request_safe_abort(monitor) - latest_status = failure_status - break - if process.poll() is not None: - raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}") - if ( - not monitor.latest_status - and now - last_startup_log_check >= 0.5 - ): - last_startup_log_check = now - log_stream.flush() - if _calibration_node_exited_before_status(log_path): - raise RuntimeError( - "CAM-STATUS-202:calibration node exited before status" - ) - if ( - not monitor.latest_status - and now - launched_at > STATUS_TIMEOUT_SECONDS - ): - raise RuntimeError("CAM-STATUS-202:no calibration status received") - if ( - monitor.latest_status - and now - monitor.last_status_at - > _status_timeout_seconds(monitor.latest_status) - ): - raise RuntimeError("MOTION-COMM-303:calibration status stopped") - except KeyboardInterrupt as error: - latest_status["state"] = "ABORTED" - latest_status["reason"] = "operator_abort" - _request_safe_abort(monitor) - _write_trace(log_path, error) - exit_code = EXIT_SAFETY - except BaseException as error: - latest_status["state"] = "PAUSED" - latest_status["reason"] = str(error) - _write_trace(log_path, error) - exit_code = EXIT_QUALITY - finally: - if process is not None: - _stop_stack(process) - monitor.destroy_node() - log_stream.flush() - os.fsync(log_stream.fileno()) - log_stream.close() - - if exit_code != EXIT_PASS: - _, block = build_failure_report( - config, - session, - latest_status, - reason=str(latest_status.get("reason", "unknown_failure")), - ) - print(block, flush=True) - return latest_status, exit_code - - -def _startup_failure_block(path: Path, error: BaseException) -> str: - return "\n".join( - [ - "========== 请复制以下内容给开发者 ==========", - "结果：FAIL", - "错误代码：CFG-PRODUCT-001", - "失败阶段：启动静态预检", - f"问题：{error}", - f"产品配置：{path}", - "自动处理：未启动相机、SDK或机械手运动", - "建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。", - "========== 复制结束 ==========", - ] - ) - - -def _automatic_resume_candidate(config: ProductConfig) -> Path | None: - """Return the newest compatible failed attempt, never a passed session. - - Do not trust only ``latest_attempt``. A process interrupted during the - device-only startup gate may have already moved that pointer while still - containing no ``session_start`` checkpoint. In that case walk backwards - to the preceding usable failed session instead of throwing away hours of - completed tasks. - """ - root = config.session_root - try: - resolved_root = root.resolve(strict=True) - except OSError: - return None - - candidates: list[Path] = [] - pointer = config.session_root / "latest_attempt" - if pointer.exists(): - try: - candidates.append(pointer.resolve(strict=True)) - except OSError: - pass - try: - candidates.extend( - sorted( - ( - path - for path in root.iterdir() - if path.is_dir() and not path.name.startswith("latest_") - ), - key=lambda path: path.name, - reverse=True, - ) - ) - except OSError: - return None - - passed_pointer = config.session_root / "latest_passed" - passed: Path | None = None - if passed_pointer.exists(): - try: - passed = passed_pointer.resolve(strict=True) - except OSError: - pass - - seen: set[Path] = set() - for unresolved in candidates: - try: - candidate = unresolved.resolve(strict=True) - except OSError: - continue - if candidate in seen: - continue - seen.add(candidate) - if candidate.parent != resolved_root or not candidate.is_dir(): - continue - # A failed attempt older than the current formal release is stale and - # must not seed a new independent calibration. - if passed is not None and candidate.name <= passed.name: - continue - raw_path = candidate / "raw_samples.jsonl" - if not raw_path.is_file(): - continue - summary_path = candidate / "calibration_summary_zh.json" - summary: dict[str, Any] | None = None - if summary_path.is_file(): - try: - loaded = json.loads(summary_path.read_text(encoding="utf-8")) - except (OSError, json.JSONDecodeError): - continue - if not isinstance(loaded, dict) or loaded.get("result") != "FAIL": - continue - summary = loaded - hashes = summary.get("hashes", {}) - if not isinstance(hashes, Mapping): - continue - if ( - str(hashes.get("source_urdf_sha256", "")) - != config.source_urdf_sha256 - or str(hashes.get("camera_extrinsics_sha256", "")) - != config.camera_extrinsics_sha256 - ): - continue - start: dict[str, Any] | None = None - try: - with raw_path.open("r", encoding="utf-8") as stream: - for line in stream: - if not line.strip(): - continue - value = json.loads(line) - if ( - isinstance(value, dict) - and value.get("kind") == "session_start" - ): - start = value - break - except (OSError, json.JSONDecodeError): - 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") - != config.source_urdf_sha256 - ): - continue - if summary is None: - # Ctrl+C can terminate the ROS launch tree before the wrapper gets - # a chance to create calibration_summary_zh.json. The immutable - # checkpoint itself is enough to resume only after independently - # proving that its external geometry still matches the product. - try: - checkpoint_extrinsics = Path( - str(start["camera_extrinsics_file"]) - ).expanduser().resolve(strict=True) - if sha256_file(checkpoint_extrinsics) != ( - config.camera_extrinsics_sha256 - ): - continue - except (KeyError, OSError, ValueError): - continue - return candidate - return None - - -def _resolve_partial_base_session( - config: ProductConfig, base_session: str | Path | None -) -> Path: - """Validate the complete passed session that donates non-target tasks.""" - if base_session is None or not str(base_session).strip(): - raise ValueError( - "partial scope requires --base-session pointing to a passed " - "complete G20 right session" - ) - candidate = Path(base_session).expanduser().resolve(strict=True) - root = config.session_root.resolve() - if candidate.parent != root or not candidate.is_dir(): - raise ValueError( - "base session must resolve to a direct session directory under " - f"{root}" - ) - raw_path = candidate / "raw_samples.jsonl" - summary_path = candidate / "calibration_summary_zh.json" - payload_path = ( - candidate - / f"g20_right_{config.serial_number}_calibration.json" - ) - for required in (raw_path, summary_path, payload_path): - if not required.is_file(): - raise ValueError(f"base session is missing required artifact: {required}") - try: - summary = json.loads(summary_path.read_text(encoding="utf-8")) - except json.JSONDecodeError as error: - raise ValueError("base session summary is invalid JSON") from error - if ( - not isinstance(summary, dict) - or summary.get("result") != "PASS" - or not bool(summary.get("quality", {}).get("passed")) - ): - raise ValueError("base session is not a formally passed session") - hashes = summary.get("hashes", {}) - expected_hashes = { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - } - if not isinstance(hashes, Mapping) or any( - str(hashes.get(name, "")) != expected - for name, expected in expected_hashes.items() - ): - raise ValueError( - "base session source URDF, camera extrinsics or calibration " - "configuration differs from the current product" - ) - return candidate - - -def run( - config_path: str | Path, - *, - workspace: str | Path | None = None, - preflight_only: bool = False, - allow_resume: bool = True, - scope: str = "full", - base_session: str | Path | None = None, -) -> int: - path = Path(config_path).expanduser().resolve() - try: - # Resolve every file and camera identity before allowing a hardware - # process to start. A second load enables the real CAN existence gate. - config = load_product_config(path, workspace=workspace, check_can=False) - load_product_config(path, workspace=workspace, check_can=True) - selected_scope = str(scope).strip().lower() - if selected_scope not in {"full", "thumb", "fingers"}: - raise ValueError("scope must be one of: full, thumb, fingers") - if selected_scope == "full" and base_session is not None: - raise ValueError( - "--base-session is valid only with --scope thumb/fingers" - ) - partial_base = None - if selected_scope == "fingers" or base_session is not None: - partial_base = _resolve_partial_base_session(config, base_session) - except BaseException as error: - print(_startup_failure_block(path, error), flush=True) - return EXIT_QUALITY - if preflight_only: - print("PASS：产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。") - return EXIT_PASS - - config.session_root.mkdir(parents=True, exist_ok=True) - resume_candidate = ( - partial_base - if selected_scope != "full" - else (_automatic_resume_candidate(config) if allow_resume else None) - ) - if resume_candidate is not None: - if selected_scope == "thumb": - print( - "拇指专项标定：四指任务继承自已通过会话 " - f"{resume_candidate.name}；4项拇指任务将全部重新采集，" - "四指零位保持不变。", - flush=True, - ) - elif selected_scope == "fingers": - print( - "四指专项标定：拇指任务和4个拇指零位继承自已通过会话 " - f"{resume_candidate.name}；12项四指任务将全部重新采集。", - flush=True, - ) - else: - print( - "检测到兼容的失败会话，将恢复已完整通过的关节任务：" - f"{resume_candidate.name}。失败中的当前任务会从头重做。", - flush=True, - ) - elif selected_scope == "thumb": - print( - "独立拇指标定：不导入四指会话；仅采集4项拇指任务，" - "四指URDF零位保持原始CAD值。", - flush=True, - ) - maximum_sessions = config.required_independent_passes - for pass_index in range(maximum_sessions): - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session = config.session_root / stamp - while session.exists(): - time.sleep(1.0) - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session = config.session_root / stamp - if maximum_sessions > 1: - print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True) - status, code = _run_hardware_session( - config, - session, - resume_from=( - resume_candidate - if selected_scope != "full" or pass_index == 0 - else None - ), - recalibration_scope=selected_scope, - ) - if code != EXIT_PASS: - return code - # Keep the exact node-side completion contract durable before the - # independent publication layer starts. If publication itself fails, - # developers can re-run artifact checks without repeating motion or - # inventing lost combination-validation metrics. - atomic_write_json(session / "node_status.json", status) - try: - summary, release_ready = finalize_session_artifacts( - config, session, node_status=status - ) - except BaseException as error: - _write_trace(session / "calibration.log", error) - status = dict(status) - status["state"] = "PAUSED" - status["reason"] = f"PUB-ARTIFACT-601:{error}" - _, block = build_failure_report(config, session, status, reason=status["reason"]) - print(block, flush=True) - return EXIT_QUALITY - if release_ready: - result_pointer = ( - config.session_root / "latest_thumb_passed" - if selected_scope == "thumb" and partial_base is None - else config.session_root / "latest_passed" - ) - print( - "\n".join( - [ - f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。", - f"正式结果：{result_pointer}", - f"JSON：{session / summary['artifacts']['json']}", - f"URDF：{summary['artifacts']['urdf']}", - ] - ), - flush=True, - ) - return EXIT_PASS - print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True) - return EXIT_QUALITY - - -def main(args: list[str] | None = None) -> None: - parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定") - parser.add_argument("--config", default=str(_default_product_config())) - parser.add_argument("--workspace", default=None) - parser.add_argument("--preflight-only", action="store_true") - parser.add_argument( - "--scope", - choices=("full", "thumb", "fingers"), - default="full", - help=( - "full重新标定全手；thumb仅重采4项拇指任务；" - "fingers复用已认证拇指并仅重采12项四指任务" - ), - ) - parser.add_argument( - "--base-session", - default=None, - help=( - "可选：thumb模式将结果合并到该完整会话；" - "fingers模式必须提供该基础会话" - ), - ) - parser.add_argument( - "--no-resume", - action="store_true", - help="忽略失败会话，从第一个关节开始全新采集", - ) - arguments = parser.parse_args(args) - configure_fastdds_large_image_transport() - ros_log_dir = Path( - os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs") - ) - ros_log_dir.mkdir(parents=True, exist_ok=True) - rclpy.init() - try: - code = run( - arguments.config, - workspace=arguments.workspace, - preflight_only=arguments.preflight_only, - allow_resume=not arguments.no_resume, - scope=arguments.scope, - base_session=arguments.base_session, - ) - finally: - if rclpy.ok(): - rclpy.shutdown() - raise SystemExit(code) - - -if __name__ == "__main__": - main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py deleted file mode 100644 index 34d081d..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py +++ /dev/null @@ -1,4606 +0,0 @@ -"""Three-dimensional joint-axis fitting and URDF zero correction.""" - -from __future__ import annotations - -from dataclasses import dataclass, replace -from datetime import datetime -import math -from pathlib import Path -import re -from typing import Any, Mapping, Sequence -import xml.etree.ElementTree as ET - -import numpy as np -from scipy.optimize import least_squares -from scipy.spatial.transform import Rotation -from scipy.stats import t as student_t - -from ...core import fit_rotation_axis, robust_rotation_summary -from ...core.urdf import ( - UrdfCorrectionPlan, - UrdfJointPatch, - UrdfPatchSet, - materialize_relative_mesh_assets as _materialize_relative_mesh_assets, - write_urdf_patches, -) -from .profile import ( - G20_RIGHT_19_LAYOUT, - IMAGE_TRAJECTORY_JOINTS, - LEFT_HAND_PROFILE, - RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, - RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, - HandCalibrationProfile, - JointCurveFit, - get_hand_calibration_profile, -) -from ...sample_schema import explicit_domain_value -from ...trajectory import ( - _fit_circle_with_axis, - _fit_joint_curve, - _fit_plane_axis, - _plane_basis, -) - - -@dataclass(frozen=True) -class ZeroCalibrationProfile: - hand: HandCalibrationProfile - direct_zero_joints: tuple[str, ...] - axis_joints: tuple[str, ...] - # Dynamic command-angle curves may be shared by unobserved fingers. This - # mapping is intentionally *not* an authorization to copy an absolute - # encoder/URDF zero between independent motors. - inherited_zero_joints: Mapping[str, str] - # Absolute static-zero inheritance requires an independent mechanical - # guarantee. With the current 11-Tag layout only the reference finger is - # observed, so the conservative mapping is empty and unobserved active - # finger origins retain their source-CAD zero. - inherited_static_zero_joints: Mapping[str, str] - constrained_circle_joints: frozenset[str] - root_anchor_joints: frozenset[str] - axis_parent_joint: Mapping[str, str] - phase_parent_joint: Mapping[str, str] - offset_observer_joint: Mapping[str, str] - # Some serial offsets can be observed from the angle between two axes - # captured by the same camera. The mapping is geometric topology only; - # it never contains a model- or serial-specific zero value. - same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]] - fixed_direct_zero_offsets_rad: Mapping[str, float] - static_output_zero_offsets_rad: Mapping[str, float] - # Product full-hand fitting uses the five root-axis line pattern. The - # thumb kernel instead uses only the serial thumb chain, so its nuisance - # palm pose cannot be influenced by finger observations. - base_pose_strategy: str = "full_hand" - # A serial-chain model may use a separate palm-root joint axis to fix the - # otherwise free rotation about its primary root axis. G20 retains its - # historical defaults; other model profiles can name the physical anchor - # explicitly without introducing model-specific branches in the solver. - orientation_anchor_joint: str | None = None - # A model whose feedback direction is mechanically reviewed may use the - # 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: - return self.hand.reference_finger - - -def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: - if hand.layout_id == G20_RIGHT_19_LAYOUT: - return _build_right_19_zero_profile(hand) - reference = hand.reference_finger - reference_roll = f"{reference}_mcp_roll" - reference_pitch = f"{reference}_mcp_pitch" - reference_pip = f"{reference}_pip" - reference_dip = f"{reference}_dip" - direct = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - reference_roll, - reference_pitch, - reference_pip, - ) - inherited = { - f"{finger}_{suffix}": f"{reference}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - if finger != reference - for suffix in ("mcp_roll", "mcp_pitch", "pip") - } - axis_parent = { - "thumb_cmc_yaw": "thumb_cmc_roll", - "thumb_cmc_pitch": "thumb_cmc_yaw", - reference_pitch: reference_roll, - } - phase_parent = { - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - reference_pip: reference_pitch, - reference_dip: reference_pip, - } - observer = { - "thumb_cmc_roll": "thumb_cmc_yaw", - "thumb_cmc_yaw": "thumb_cmc_pitch", - "thumb_cmc_pitch": "thumb_mcp", - "thumb_mcp": "thumb_ip", - reference_roll: reference_pitch, - reference_pitch: reference_pip, - reference_pip: reference_dip, - } - fixed_direct_zero_offsets = { - "thumb_mcp": 0.0, - reference_roll: 0.0, - reference_pitch: 0.0, - reference_pip: 0.0, - } - return ZeroCalibrationProfile( - hand=hand, - direct_zero_joints=direct, - axis_joints=( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "thumb_ip", - reference_roll, - reference_pitch, - reference_pip, - reference_dip, - ), - inherited_zero_joints=inherited, - inherited_static_zero_joints={}, - constrained_circle_joints=( - hand.image_trajectory_joints | {reference_dip, "thumb_cmc_yaw"} - ), - root_anchor_joints=frozenset({"thumb_cmc_roll", reference_roll}), - axis_parent_joint=axis_parent, - phase_parent_joint=phase_parent, - offset_observer_joint=observer, - same_view_axis_pair_by_offset={}, - fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets, - static_output_zero_offsets_rad={}, - ) - - -def _build_right_19_zero_profile( - hand: HandCalibrationProfile, -) -> ZeroCalibrationProfile: - """Return the 16-active-zero graph for the 19-Tag layout. - - Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but - repeated motion with one unchanged camera/Tag installation cannot by - itself distinguish a fixed parallel-axis phase bias from an encoder zero. - The three coupled thumb-CMC offsets must therefore remain a visual - multi-axis solve; their electrical endpoints are not assumed to coincide - with source-CAD limits. Independently repeatable contact endpoints still - anchor thumb MCP, finger MCP-pitch and PIP from each session. DIP stays - passive and has no independently identifiable static zero. - """ - fingers = ("index", "middle", "ring", "pinky") - direct = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in fingers - for suffix in ("mcp_roll", "mcp_pitch", "pip") - ), - ) - axis_joints = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "thumb_ip", - *( - f"{finger}_{suffix}" - for finger in fingers - for suffix in ("mcp_roll", "mcp_pitch", "pip", "dip") - ), - ) - axis_parent = { - "thumb_cmc_yaw": "thumb_cmc_roll", - "thumb_cmc_pitch": "thumb_cmc_yaw", - **{ - f"{finger}_mcp_pitch": f"{finger}_mcp_roll" - for finger in fingers - }, - } - phase_parent = { - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - **{ - f"{finger}_{child}": f"{finger}_{parent}" - for finger in fingers - for child, parent in ( - ("pip", "mcp_pitch"), - ("dip", "pip"), - ) - }, - } - observer = { - "thumb_cmc_roll": "thumb_cmc_yaw", - "thumb_cmc_yaw": "thumb_cmc_pitch", - "thumb_cmc_pitch": "thumb_mcp", - "thumb_mcp": "thumb_ip", - **{ - f"{finger}_{target}": f"{finger}_{observed}" - for finger in fingers - for target, observed in ( - ("mcp_roll", "mcp_pitch"), - ("mcp_pitch", "pip"), - ("pip", "dip"), - ) - }, - } - root_anchors = frozenset( - {"thumb_cmc_roll", *(f"{finger}_mcp_roll" for finger in fingers)} - ) - constrained = frozenset( - set(hand.image_trajectory_joints) - | {"thumb_cmc_yaw"} - # The side camera observes all four flexion axes nearly end-on. A - # 16 mm monocular planar Tag gives a very repeatable orientation axis, - # while its command-correlated depth bias can tilt the otherwise clean - # centre-trajectory plane by several degrees. Use the orientation - # axis to constrain that circle; PIP retains its independently fitted - # axis line and all final SE(3)/holdout quality gates. - | set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS) - # _fit_axis_measurement_raw constrains every passive DIP direction to - # its source-URDF-parallel upstream PIP. Declare the same fact here - # so validation does not compare that trusted direction with the - # depth-biased free plane of a distal Tag trajectory. - | set(RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS) - ) - return ZeroCalibrationProfile( - hand=hand, - direct_zero_joints=tuple(direct), - axis_joints=tuple(axis_joints), - inherited_zero_joints={}, - inherited_static_zero_joints={}, - constrained_circle_joints=constrained, - root_anchor_joints=root_anchors, - axis_parent_joint=axis_parent, - phase_parent_joint=phase_parent, - offset_observer_joint=observer, - same_view_axis_pair_by_offset={ - "thumb_cmc_yaw": ("thumb_cmc_roll", "thumb_cmc_pitch") - }, - # Endpoint-observable offsets are supplied by the caller. CMC remains - # visually solved and no serial-specific zero is hidden in the shared - # profile. - fixed_direct_zero_offsets_rad={}, - static_output_zero_offsets_rad={}, - ) - - -def get_right_19_thumb_zero_profile() -> ZeroCalibrationProfile: - """Return the independent four-zero thumb observer graph. - - Roll and MCP are supplied as mechanical endpoint datums by the caller; - yaw is observed by the same-view roll/pitch axis pair and pitch by the - pitch/MCP serial phase. No finger axis is part of this profile. - """ - full = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) - thumb_direct = tuple( - name for name in full.direct_zero_joints if name.startswith("thumb_") - ) - thumb_axes = tuple( - name for name in full.axis_joints if name.startswith("thumb_") - ) - return replace( - full, - direct_zero_joints=thumb_direct, - axis_joints=thumb_axes, - constrained_circle_joints=frozenset( - name - for name in full.constrained_circle_joints - if name.startswith("thumb_") - ), - root_anchor_joints=frozenset({"thumb_cmc_roll"}), - axis_parent_joint={ - child: parent - for child, parent in full.axis_parent_joint.items() - if child.startswith("thumb_") and parent.startswith("thumb_") - }, - phase_parent_joint={ - child: parent - for child, parent in full.phase_parent_joint.items() - if child.startswith("thumb_") and parent.startswith("thumb_") - }, - offset_observer_joint={ - target: observer - for target, observer in full.offset_observer_joint.items() - if target.startswith("thumb_") and observer.startswith("thumb_") - }, - same_view_axis_pair_by_offset={ - target: pair - for target, pair in full.same_view_axis_pair_by_offset.items() - if target.startswith("thumb_") - }, - fixed_direct_zero_offsets_rad={}, - static_output_zero_offsets_rad={}, - base_pose_strategy="thumb_serial", - ) - - -RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset( - { - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") - ), - } -) - -# Thumb CMC roll has a directly observed actuator-to-actuator travel, but its -# electrical stop is not used as a runtime URDF limit. Keep it out of -# RIGHT_19_MECHANICAL_ENDPOINT_JOINTS (whose offsets also move upper limits), -# and use its endpoint estimate only as the final static roll origin. This -# makes roll independent of the fitted palm-frame phase and of all yaw logic. -RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS = frozenset({"thumb_cmc_roll"}) -RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS = ( - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - | RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS -) -RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS = RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS - - -def anchor_right_19_mechanical_endpoint_curves( - curves: Mapping[str, JointCurveFit], - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - *, - maximum_direction_difference_rad: float = math.radians(1.0), - maximum_curve_correction_rad: float = math.radians(3.0), - feedback_endpoint_joints: frozenset[str] = frozenset(), - endpoint_joints: frozenset[str] | None = None, -) -> dict[str, JointCurveFit]: - """Set mechanical-endpoint curve scale from direct SO(3) travel. - - A fitted-axis projection is useful for a dense, signed command curve, but - a small view-dependent orthogonal PnP component can change that - projection's full-scale value. The rotation magnitude between the two - settled endpoint poses is the revolute-joint travel itself: conjugating - both poses by a rigid hand/camera transform or either fixed Tag mounting - rotation cannot change it. - - Use every supplied cycle and both sweep directions as independent endpoint - measurements, reject disagreement, and apply their robust median as one - scale correction to the already validated curve. Dense CMC-roll records - are selected in feedback coordinates; settled mechanical-contact records - remain selected in requested-command coordinates. - """ - if ( - not math.isfinite(maximum_direction_difference_rad) - or maximum_direction_difference_rad <= 0.0 - ): - raise ValueError("endpoint direction difference limit must be positive") - if ( - not math.isfinite(maximum_curve_correction_rad) - or maximum_curve_correction_rad <= 0.0 - ): - raise ValueError("endpoint curve correction limit must be positive") - - result = dict(curves) - selected_endpoint_joints = ( - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - if endpoint_joints is None - else frozenset(str(name) for name in endpoint_joints) - ) - unknown_endpoint_joints = sorted( - set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) - ) - if unknown_endpoint_joints: - raise ValueError( - "endpoint joints are not endpoint measurements: " - + ",".join(unknown_endpoint_joints) - ) - unknown_feedback_joints = sorted( - set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) - ) - if unknown_feedback_joints: - raise ValueError( - "feedback endpoint joints are not endpoint measurements: " - + ",".join(unknown_feedback_joints) - ) - - for name in sorted(selected_endpoint_joints): - fit = curves.get(name) - records = list(records_by_joint.get(name, ())) - if fit is None: - raise ValueError(f"missing measured endpoint curve for {name}") - - direction_travel: dict[str, float] = {} - all_travel: list[float] = [] - for direction in ("decreasing", "increasing"): - cycles = sorted( - { - int(record.get("cycle", 0)) - for record in records - if str(record.get("direction")) == direction - } - ) - branch_travel: list[float] = [] - for cycle in cycles: - endpoint_rotations: dict[int, Rotation] = {} - for command in (0, 255): - quaternions = [] - for record in records: - if ( - str(record.get("direction")) != direction - or int(record.get("cycle", 0)) != cycle - ): - continue - endpoint_value = ( - explicit_domain_value(record, "feedback") - if name in feedback_endpoint_joints - else record.get( - "requested_command_u8", - record.get("command_u8", -1), - ) - ) - if endpoint_value is None: - continue - if int(round(float(endpoint_value))) == command: - quaternions.append(_relative_rotation(record)) - if not quaternions: - raise ValueError( - f"{name} {direction} cycle {cycle} is missing " - f"settled endpoint {command}" - ) - endpoint_rotations[command] = Rotation.from_quat( - robust_rotation_summary(quaternions)[0] - ) - branch_travel.append( - float( - ( - endpoint_rotations[255].inv() - * endpoint_rotations[0] - ).magnitude() - ) - ) - if not branch_travel: - raise ValueError(f"{name} is missing {direction} endpoint travel") - direction_travel[direction] = float(np.median(branch_travel)) - all_travel.extend(branch_travel) - - travel_values = np.asarray(list(direction_travel.values()), dtype=float) - direction_difference = float(np.ptp(travel_values)) - if direction_difference > maximum_direction_difference_rad: - raise ValueError( - f"{name} settled endpoint directions disagree: " - f"{math.degrees(direction_difference):.3f}deg" - ) - cycle_range = float(np.ptp(np.asarray(all_travel, dtype=float))) - if cycle_range > maximum_direction_difference_rad: - raise ValueError( - f"{name} settled endpoint cycles disagree: " - f"{math.degrees(cycle_range):.3f}deg" - ) - direct_travel = float(np.median(np.asarray(all_travel, dtype=float))) - curve = np.asarray(fit.angle_rad, dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"{name} endpoint curve must contain 256 finite bins") - projected_travel = float(curve[0] - curve[255]) - if direct_travel <= 0.0 or projected_travel <= 0.0: - raise ValueError(f"{name} endpoint travel must be positive") - correction = abs(projected_travel - direct_travel) - if correction > maximum_curve_correction_rad: - raise ValueError( - f"{name} fitted/direct endpoint travel differs by " - f"{math.degrees(correction):.3f}deg" - ) - scale = direct_travel / projected_travel - - def scaled(values: Sequence[float]) -> tuple[float, ...]: - return tuple(float(value) * scale for value in values) - - circle = dict(fit.circle) - circle.update( - { - "mechanical_endpoint_direct_travel_rad": direct_travel, - "mechanical_endpoint_direction_difference_rad": ( - direction_difference - ), - "mechanical_endpoint_cycle_range_rad": cycle_range, - "mechanical_endpoint_sample_count": len(all_travel), - "mechanical_endpoint_raw_curve_travel_rad": projected_travel, - "mechanical_endpoint_curve_scale": scale, - } - ) - quality = dict(fit.quality) - if "arc_rad" in quality: - quality["arc_rad"] = float(quality["arc_rad"]) * scale - result[name] = replace( - fit, - angle_rad=scaled(fit.angle_rad), - decreasing_rad=scaled(fit.decreasing_rad), - increasing_rad=scaled(fit.increasing_rad), - circle=circle, - maximum_monotonic_correction_rad=( - float(fit.maximum_monotonic_correction_rad) * scale - ), - maximum_hysteresis_rad=float(fit.maximum_hysteresis_rad) * scale, - quality=quality, - zero_offset_rad=float(fit.zero_offset_rad) * scale, - ) - return result - - -def derive_right_19_mechanical_endpoint_offsets( - source_urdf: str | Path, - curves: Mapping[str, JointCurveFit], - *, - maximum_offset_rad: float = math.radians(5.0), - endpoint_joints: frozenset[str] | None = None, -) -> dict[str, float]: - """Estimate encoder origins from repeatable measured mechanical endpoints. - - For the four fingers feedback 0 is the independently verified palm-contact - endpoint; thumb MCP uses its independently verified actuator endpoint. - The source URDF upper limit describes those same physical endpoints. - Therefore ``origin_offset + measured_travel == CAD_upper``. - - Thumb CMC pitch/yaw remain excluded because their endpoint-to-CAD contract - is not independently established. CMC roll is included as a post-solve - origin only: its measured travel determines roll without allowing the - palm common-direction fit to write that joint. - - The returned corrections are measured, generally non-zero encoder zeros; - they do not retain the CAD origin. Using full relative rotation travel - makes them invariant to rigid hand movement and arbitrary fixed Tag - mounting rotation, unlike a cross-camera absolute planar-PnP phase. - """ - if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0: - raise ValueError("mechanical endpoint maximum offset must be positive") - root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() - joints = {str(node.get("name")): node for node in root.findall("joint")} - result: dict[str, float] = {} - selected_endpoint_joints = ( - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - if endpoint_joints is None - else frozenset(str(name) for name in endpoint_joints) - ) - unknown_endpoint_joints = sorted( - set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) - ) - if unknown_endpoint_joints: - raise ValueError( - "endpoint joints are not endpoint measurements: " - + ",".join(unknown_endpoint_joints) - ) - for name in sorted(selected_endpoint_joints): - fit = curves.get(name) - joint = joints.get(name) - limit = None if joint is None else joint.find("limit") - if fit is None: - raise ValueError(f"missing measured endpoint curve for {name}") - if limit is None or limit.get("upper") is None: - raise ValueError(f"source URDF joint {name} has no upper limit") - upper = float(limit.get("upper")) - curve = np.asarray(fit.angle_rad, dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"{name} endpoint curve must contain 256 finite bins") - measured_endpoint = float(curve[0]) - if measured_endpoint <= 0.0 or abs(float(curve[255])) > math.radians(0.05): - raise ValueError( - f"{name} endpoint curve does not use command 255 as its zero reference" - ) - if abs(measured_endpoint - float(np.max(curve))) > math.radians(0.05): - raise ValueError( - f"{name} command 0 is not the measured upper mechanical endpoint" - ) - offset = upper - measured_endpoint - if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: - raise ValueError( - f"{name} endpoint-derived zero offset is outside the safe range: " - f"{math.degrees(offset):.3f}deg" - ) - result[name] = offset - return result - - -def get_zero_calibration_profile( - side: str, layout_id: str = "legacy_11" -) -> ZeroCalibrationProfile: - return _build_zero_profile(get_hand_calibration_profile(side, layout_id)) - - -LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) - -# Backwards-compatible left-hand aliases. -CONSTRAINED_CIRCLE_JOINTS = LEFT_ZERO_PROFILE.constrained_circle_joints -DIRECT_ZERO_JOINTS = LEFT_ZERO_PROFILE.direct_zero_joints -FIXED_DIRECT_ZERO_OFFSETS_RAD = dict( - LEFT_ZERO_PROFILE.fixed_direct_zero_offsets_rad -) -OPTIMIZED_ZERO_JOINTS = tuple( - name for name in DIRECT_ZERO_JOINTS - if name not in FIXED_DIRECT_ZERO_OFFSETS_RAD -) -AXIS_JOINTS = LEFT_ZERO_PROFILE.axis_joints -INHERITED_ZERO_JOINTS = dict(LEFT_ZERO_PROFILE.inherited_zero_joints) - - -def circle_direction_is_constrained( - joint_name: str, constrained_circle_joints: frozenset[str] -) -> bool: - """Match the constraint used by both canonical and side aliases.""" - name = str(joint_name) - return name in constrained_circle_joints or name.endswith("_side") - - -def select_cross_view_roll_direction_source( - primary_cone_residuals_rad: Sequence[float], - secondary_cone_residuals_rad: Sequence[float], - maximum_cone_mismatch_rad: float, -) -> str: - """Choose one roll-axis view for the complete repeated-sweep group. - - The choice must not be made independently for every cycle. A residual - sitting just either side of the cone gate can otherwise alternate the - selected camera and turn a fixed cross-view bias into a false cycle-axis - spread. The secondary view wins only with a three-quarter consensus and - a median residual inside the unchanged geometry gate; ambiguous groups - retain the primary view. - """ - primary = np.asarray(primary_cone_residuals_rad, dtype=float) - secondary = np.asarray(secondary_cone_residuals_rad, dtype=float) - if ( - primary.ndim != 1 - or secondary.ndim != 1 - or len(primary) != len(secondary) - or len(primary) < 3 - or not np.all(np.isfinite(primary)) - or not np.all(np.isfinite(secondary)) - or float(maximum_cone_mismatch_rad) <= 0.0 - ): - return "primary" - limit = float(maximum_cone_mismatch_rad) - required_consensus = int(math.ceil(0.75 * len(primary))) - primary_passes = int(np.count_nonzero(primary <= limit)) - secondary_passes = int(np.count_nonzero(secondary <= limit)) - if ( - secondary_passes >= required_consensus - and primary_passes < required_consensus - and float(np.median(secondary)) <= limit - and float(np.median(primary)) > limit - ): - return "secondary" - return "primary" - - -ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 - -# Fit an axis-line point from the complete relative SE(3) trajectory instead -# of using only the moving Tag centre's free 3-D circle. For an approximately -# end-on view, planar-PnP optical depth is deliberately projected out: image -# x/y still observes the radial motion that determines an axis line, while -# the independent Tag depths are the dominant source of false zero phase. -AXIS_POINT_MINIMUM_ROTATION_RAD = math.radians(3.0) -AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD = math.radians(45.0) -AXIS_POINT_RESIDUAL_SCALE_M = 0.001 - -# A joint's own screw axis is invariant to its own encoder-zero offset. Only -# the downstream axis selected by the kinematic chain can observe that zero. -# Feeding every axis-direction and axis-line residual into the optimizer lets -# fixed CAD/PnP geometry residuals push unrelated zero offsets to their bounds. -# Keep the two root lines as the palm-pose anchor, then use only the component -# that actually observes the preceding zero in each serial chain. -ZERO_AXIS_PARENT_JOINT = dict(LEFT_ZERO_PROFILE.axis_parent_joint) -ZERO_AXIS_OBSERVATION_JOINTS = frozenset( - set(LEFT_ZERO_PROFILE.root_anchor_joints) | set(ZERO_AXIS_PARENT_JOINT) -) -ZERO_LINE_OBSERVATION_JOINTS = frozenset( - set(LEFT_ZERO_PROFILE.root_anchor_joints) - | set(LEFT_ZERO_PROFILE.phase_parent_joint) -) -ZERO_ROOT_ANCHOR_JOINTS = LEFT_ZERO_PROFILE.root_anchor_joints -ZERO_MINIMUM_AXIS_CONE_RAD = math.radians(15.0) - - -def _zero_sensitive_axis_error_rad( - predicted_axis: Sequence[float], - observed_axis: Sequence[float], - parent_axis: Sequence[float], -) -> float: - """Return only the axis error that a parent-joint zero can change. - - Rotating a downstream axis about its parent preserves their mutual cone - angle. The component normal to their plane is therefore the observable - encoder-zero error; cone-angle mismatch belongs to fixed geometry/PnP and - must not push a zero offset or fail its holdout validation. - """ - predicted = _vector(predicted_axis, 3, name="predicted axis") - predicted /= np.linalg.norm(predicted) - observed = _vector(observed_axis, 3, name="observed axis") - observed /= np.linalg.norm(observed) - parent = _vector(parent_axis, 3, name="parent axis") - parent /= np.linalg.norm(parent) - if float(predicted @ observed) < 0.0: - observed = -observed - predicted_projected = predicted - parent * float(predicted @ parent) - observed_projected = observed - parent * float(observed @ parent) - predicted_norm = float(np.linalg.norm(predicted_projected)) - observed_norm = float(np.linalg.norm(observed_projected)) - minimum_projection = math.sin(ZERO_MINIMUM_AXIS_CONE_RAD) - if min(predicted_norm, observed_norm) < minimum_projection: - raise ValueError( - "parent and downstream axes have an unobservable cone angle" - ) - predicted_projected /= predicted_norm - observed_projected /= observed_norm - return math.atan2( - float(parent @ np.cross(predicted_projected, observed_projected)), - float( - np.clip(predicted_projected @ observed_projected, -1.0, 1.0) - ), - ) - - -def _axis_cone_mismatch_rad( - predicted_axis: Sequence[float], - observed_axis: Sequence[float], - parent_axis: Sequence[float], -) -> float: - """Return zero-invariant parent/downstream cone-angle disagreement.""" - predicted = _vector(predicted_axis, 3, name="predicted axis") - predicted /= np.linalg.norm(predicted) - observed = _vector(observed_axis, 3, name="observed axis") - observed /= np.linalg.norm(observed) - parent = _vector(parent_axis, 3, name="parent axis") - parent /= np.linalg.norm(parent) - predicted_cone = math.acos( - abs(float(np.clip(parent @ predicted, -1.0, 1.0))) - ) - observed_cone = math.acos( - abs(float(np.clip(parent @ observed, -1.0, 1.0))) - ) - return abs(predicted_cone - observed_cone) - - -def _vector(value: Sequence[float], size: int, *, name: str) -> np.ndarray: - result = np.asarray(value, dtype=float) - if result.shape != (size,) or not np.all(np.isfinite(result)): - raise ValueError(f"{name} must contain {size} finite values") - return result - - -def _pose_matrix(payload: Mapping[str, Any]) -> np.ndarray: - translation = _vector(payload["translation_xyz_m"], 3, name="translation") - quaternion = _vector(payload["quaternion_xyzw"], 4, name="quaternion") - quaternion /= np.linalg.norm(quaternion) - result = np.eye(4) - result[:3, :3] = Rotation.from_quat(quaternion).as_matrix() - result[:3, 3] = translation - return result - - -def _relative_rotation(record: Mapping[str, Any]) -> np.ndarray: - quaternion = _vector( - record["relative_quaternion_xyzw"], 4, name="relative quaternion" - ) - return quaternion / np.linalg.norm(quaternion) - - -def _reference_group_key(record: Mapping[str, Any]) -> tuple[Any, Any]: - return record.get("cycle"), record.get("direction") - - -def _canonical_reference_records( - records: Sequence[Mapping[str, Any]], - canonical_zero_direction: str | None, -) -> list[Mapping[str, Any]]: - """Select the sole physical-zero branch when one is configured.""" - if canonical_zero_direction is None: - return list(records) - if canonical_zero_direction not in {"decreasing", "increasing"}: - raise ValueError("canonical zero direction is invalid") - selected = [ - record - for record in records - if str(record.get("direction")) == canonical_zero_direction - ] - if not selected: - raise ValueError( - f"canonical {canonical_zero_direction} zero branch has no samples" - ) - return selected - - -def _interpolate_reference_rotation( - 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: - command = int(record["command_u8"]) - by_command.setdefault(command, []).append(_relative_rotation(record)) - if not by_command: - return None - - rotations = { - command: Rotation.from_quat(robust_rotation_summary(values)[0]) - for command, values in by_command.items() - } - zero = int(zero_command_u8) - if zero in rotations: - return rotations[zero] - - lower = [command for command in rotations if command < zero] - upper = [command for command in rotations if command > zero] - lower_command = max(lower) if lower else None - upper_command = min(upper) if upper else None - if lower_command is not None and upper_command is not None: - lower_distance = zero - lower_command - upper_distance = upper_command - zero - 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) - delta = (lower_rotation.inv() * upper_rotation).as_rotvec() - return lower_rotation * Rotation.from_rotvec(delta * fraction) - - nearest_command = min(rotations, key=lambda command: abs(command - zero)) - if maximum_distance_u8 is None or abs( - nearest_command - zero - ) <= int(maximum_distance_u8): - return rotations[nearest_command] - return None - - -def _near_zero_records( - records: Sequence[Mapping[str, Any]], zero_command_u8: int -) -> list[Mapping[str, Any]]: - groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} - for record in records: - groups.setdefault(_reference_group_key(record), []).append(record) - selected: list[Mapping[str, Any]] = [] - zero = int(zero_command_u8) - for group in groups.values(): - distance = min(abs(int(record["command_u8"]) - zero) for record in group) - if distance > ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: - continue - selected.extend( - record - for record in group - if abs(int(record["command_u8"]) - zero) == distance - ) - return selected - - -def _baseline_reference( - 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: - groups.setdefault(_reference_group_key(record), []).append(record) - values = [ - rotation.as_quat() - for group in groups.values() - if ( - rotation := _interpolate_reference_rotation( - group, - zero_command_u8, - maximum_distance_u8, - ) - ) - is not None - ] - if not values: - 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] - - -def baseline_hysteresis_by_cycle_rad( - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - axis_xyz: Sequence[float] | None = None, -) -> tuple[float, ...]: - """Return decreasing/increasing joint-angle disagreement per cycle. - - Full SO(3) disagreement includes planar-PnP tilt noise that is orthogonal - to the fitted revolute axis. When an axis is supplied, report only the - physically meaningful component about that axis; the orthogonal component - remains covered by the independent rotation-model residual gate. - """ - samples = [dict(record) for record in records] - axis: np.ndarray | None = None - if axis_xyz is not None: - axis = _vector(axis_xyz, 3, name="baseline hysteresis axis") - norm = float(np.linalg.norm(axis)) - if norm <= 0.0: - raise ValueError("baseline hysteresis axis must be non-zero") - axis /= norm - result: list[float] = [] - for cycle in sorted({int(record["cycle"]) for record in samples}): - rotations: dict[str, Rotation] = {} - for direction in ("decreasing", "increasing"): - selected = [ - record - for record in samples - if int(record["cycle"]) == cycle - and str(record["direction"]) == direction - ] - reference = _interpolate_reference_rotation( - selected, zero_command_u8 - ) - if reference is None: - raise ValueError( - f"cycle {cycle} {direction} is missing baseline samples" - ) - rotations[direction] = reference - delta = ( - rotations["decreasing"].inv() - * rotations["increasing"] - ) - result.append( - float( - delta.magnitude() - if axis is None - else abs(float(delta.as_rotvec() @ axis)) - ) - ) - if not result: - raise ValueError("baseline hysteresis requires at least one cycle") - return tuple(result) - - -def fit_rotation_joint_curve( - records: Sequence[Mapping[str, Any]], - *, - 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: - raise ValueError("rotation trajectory requires at least 12 samples") - vectors: list[np.ndarray] = [] - commands: list[int] = [] - values_by_record: list[float] = [] - references: dict[int, Rotation] = {} - for cycle in sorted({int(record["cycle"]) for record in samples}): - cycle_records = [ - record for record in samples if int(record["cycle"]) == cycle - ] - references[cycle] = Rotation.from_quat( - _baseline_reference( - _canonical_reference_records( - cycle_records, canonical_zero_direction - ), - zero_command_u8, - zero_reference_maximum_distance_u8, - ) - ) - for record in samples: - observed = Rotation.from_quat(_relative_rotation(record)) - vector = ( - references[int(record["cycle"])].inv() * observed - ).as_rotvec() - vectors.append(vector) - commands.append(int(record["command_u8"])) - axis = fit_rotation_axis(vectors, commands) - values_by_record = [float(vector @ axis) for vector in vectors] - curves, correction, hysteresis = _fit_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"): - values = np.asarray(curves[key], dtype=float) - values -= float(values[int(zero_command_u8)]) - curves[key] = [round(float(value), 8) for value in values] - else: - canonical_key = f"{canonical_zero_direction}_rad" - shared_zero = float(curves[canonical_key][int(zero_command_u8)]) - for key in ("decreasing_rad", "increasing_rad"): - values = np.asarray(curves[key], dtype=float) - shared_zero - curves[key] = [round(float(value), 8) for value in values] - # Before the bridge has observed motion direction it must use the - # same branch that defines the URDF physical zero, never an average - # pose that the mechanism may not be able to occupy. - curves["angle_rad"] = list(curves[canonical_key]) - orthogonal = [ - float(np.linalg.norm(vector - float(vector @ axis) * axis)) - for vector in vectors - ] - return JointCurveFit( - angle_rad=tuple(float(value) for value in curves["angle_rad"]), - decreasing_rad=tuple(float(value) for value in curves["decreasing_rad"]), - increasing_rad=tuple(float(value) for value in curves["increasing_rad"]), - circle={ - "space": "relative_rotation_3d", - "axis_xyz": [float(value) for value in axis], - "zero_command_u8": int(zero_command_u8), - "reference_quaternion_xyzw": [ - float(value) - for value in _baseline_reference( - _canonical_reference_records( - samples, canonical_zero_direction - ), - zero_command_u8, - zero_reference_maximum_distance_u8, - ) - ], - "canonical_zero_direction": canonical_zero_direction, - }, - maximum_monotonic_correction_rad=float(correction), - maximum_hysteresis_rad=float(hysteresis), - quality={ - "rotation_orthogonal_rms_rad": float( - np.sqrt(np.mean(np.square(orthogonal))) - ), - "arc_rad": float( - max(curves["angle_rad"]) - min(curves["angle_rad"]) - ), - }, - ) - - -def measure_rotation_joint_observation( - fit: JointCurveFit, quaternion_xyzw: Sequence[float] -) -> float: - """Measure one parent-to-child orientation with a fitted 3-D curve.""" - if fit.circle.get("space") != "relative_rotation_3d": - raise ValueError("joint fit is not a relative-rotation curve") - reference = Rotation.from_quat( - _vector( - fit.circle["reference_quaternion_xyzw"], - 4, - name="reference quaternion", - ) - ) - observed = Rotation.from_quat( - _vector(quaternion_xyzw, 4, name="observed quaternion") - ) - axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") - axis /= np.linalg.norm(axis) - return float((reference.inv() * observed).as_rotvec() @ axis) - - -def measure_joint_curve_observation( - fit: JointCurveFit, - *, - quaternion_xyzw: Sequence[float] | None = None, - image_relative_xy_px: Sequence[float] | None = None, -) -> float: - """Measure one observation in the same space as its fitted curve.""" - if fit.circle.get("space") == "relative_rotation_3d": - if quaternion_xyzw is None: - raise ValueError("rotation observation quaternion is missing") - return measure_rotation_joint_observation(fit, quaternion_xyzw) - if fit.circle.get("space") != "image_2d": - raise ValueError("unsupported joint curve observation representation") - if image_relative_xy_px is None: - raise ValueError("image curve observation point is missing") - point = np.asarray(image_relative_xy_px, dtype=float) - centre = np.asarray(fit.circle["center_xy_px"], dtype=float) - reference = np.asarray(fit.circle["reference_xy_px"], dtype=float) - vector = point - centre - if ( - point.shape != (2,) - or not np.all(np.isfinite(point)) - or float(np.linalg.norm(vector)) < 1.0e-9 - ): - raise ValueError("image curve observation point is invalid") - return float(fit.circle["orientation_sign"]) * math.atan2( - float(reference[0] * vector[1] - reference[1] * vector[0]), - float(reference @ vector), - ) - - -def rotation_curve_holdout_errors( - fit: JointCurveFit, - 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] - if not samples: - raise ValueError("holdout records are empty") - canonical_zero_direction = fit.circle.get("canonical_zero_direction") - reference = Rotation.from_quat( - _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") - axis /= np.linalg.norm(axis) - errors: list[float] = [] - for record in samples: - observed = Rotation.from_quat(_relative_rotation(record)) - angle = float((reference.inv() * observed).as_rotvec() @ axis) - command = int(record["command_u8"]) - direction = str(record["direction"]) - expected_curve = ( - fit.decreasing_rad - if direction == "decreasing" - else fit.increasing_rad - ) - errors.append(angle - float(expected_curve[command])) - return tuple(errors) - - -def joint_curve_holdout_errors( - fit: JointCurveFit, - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, -) -> tuple[float, ...]: - """Validate either a rotation or fixed-parent image-circle curve.""" - if fit.circle.get("space") == "relative_rotation_3d": - return rotation_curve_holdout_errors( - fit, records, zero_command_u8=zero_command_u8 - ) - if fit.circle.get("space") != "image_2d": - raise ValueError("unsupported joint curve holdout representation") - if not records: - raise ValueError("holdout records are empty") - errors: list[float] = [] - for record in records: - observed = measure_joint_curve_observation( - fit, - image_relative_xy_px=record["image_relative_xy_px"], - ) - command = int(record["command_u8"]) - direction = str(record["direction"]) - expected_curve = ( - fit.decreasing_rad - if direction == "decreasing" - else fit.increasing_rad - ) - errors.append(observed - float(expected_curve[command])) - return tuple(errors) - - -@dataclass(frozen=True) -class JointAxisMeasurement: - joint: str - cycle: int - axis_common_xyz: tuple[float, float, float] - point_common_xyz_m: tuple[float, float, float] - condition_state_u8: tuple[float, ...] - plane_rms_m: float - radial_rms_m: float - rotation_circle_axis_difference_rad: float - # Optical-axis direction expressed in the shared calibration frame. It - # lets the phase solver discard the least reliable monocular-PnP depth - # component when the joint axis is viewed approximately end-on. Older - # recordings and synthetic callers may omit it and retain the 3-D path. - view_normal_common_xyz: tuple[float, float, float] | None = None - # Logical commands that produced the condition. Firmware feedback may - # saturate a few u8 short of an endpoint (for example right motor 10 reads - # 250 for command 255); kinematics must use the command-indexed curve while - # retaining condition_state_u8 for diagnostics and safety. - condition_command_u8: tuple[float, ...] | None = None - axis_direction_source: str = "unspecified" - 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 - # clear the actual quality source instead of blindly rescanning ``joint``. - pose_axis_line_source_joints: tuple[str, ...] = () - # Camera centre that observed ``point_common_xyz_m``. When populated, - # the zero solver can use only the ray from this centre to the fitted axis - # point. That ray is the depth-free interpretation-plane observation of - # the physical axis; translating a monocular planar-PnP solution along - # its optical ray therefore cannot rotate the recovered palm frame. - axis_point_camera_center_common_xyz_m: ( - tuple[float, float, float] | None - ) = None - # Normal of the source camera's interpretation plane for this axis line. - # Unlike a 3-D PnP line point, the plane is unchanged by optical-depth - # error. Several named parallel root lines jointly recover their common - # physical direction as the null direction of these plane normals. - axis_point_interpretation_plane_normal_common_xyz: ( - tuple[float, float, float] | None - ) = None - - -@dataclass(frozen=True) -class PalmOrientationMeasurement: - """Direction-only joint observation from a partially visible sweep. - - The moving Tag may disappear before the motor reaches its far endpoint. - Only the relative SO(3) trajectory is retained, so fixed Tag translation - and mounting rotation cannot define the palm phase. - """ - - source_joint: str - model_joint: str - cycle: int - axis_common_xyz: tuple[float, float, float] - condition_state_u8: tuple[float, ...] - observed_arc_rad: float - rotation_orthogonal_rms_rad: float - axis_estimator: str = "baseline_relative_so3" - incremental_pair_count: int = 0 - - -PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 = 24 -PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD = math.radians(0.3) -PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT = 24 -PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE = 75.0 - - -def _incremental_common_rotation_axis( - samples_by_bin: Mapping[ - tuple[str, int], Sequence[Mapping[str, Any]] - ], - *, - command_separation_u8: int = ( - PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 - ), - minimum_rotation_rad: float = ( - PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD - ), - minimum_pair_count: int = PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT, - consensus_percentile: float = ( - PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE - ), -) -> tuple[np.ndarray, int]: - """Fit one physical axis from robust local Tag rotations. - - A single baseline-to-endpoint logarithm is sensitive to smooth planar-PnP - curvature: the fitted axis then changes when the same Tag trajectory is - translated to another part of the image. Local finite rotations are - expressed directly in the common camera frame, and their undirected - weighted consensus estimates the physical revolute axis. The worst - quartile is discarded once more after the initial consensus, which is - enough to reject endpoint branch curvature without learning a - serial-specific yaw value or an image-position correction table. - """ - separation = int(command_separation_u8) - if separation < 4 or separation > 64: - raise ValueError("palm axis command separation must be in [4, 64]") - if not 0.0 < float(minimum_rotation_rad) < math.pi: - raise ValueError("palm axis minimum increment must be in (0, pi)") - if int(minimum_pair_count) < 6: - raise ValueError("palm axis minimum pair count must be at least six") - percentile = float(consensus_percentile) - if not 50.0 <= percentile <= 90.0: - raise ValueError("palm axis consensus percentile must be in [50, 90]") - - binned_common_rotations: dict[tuple[str, int], Rotation] = {} - for key, group in samples_by_bin.items(): - child_quaternions = [] - for record in group: - pose = record.get("child_pose_common") - if not isinstance(pose, Mapping): - continue - quaternion = pose.get("quaternion_xyzw") - if quaternion is not None: - child_quaternions.append(quaternion) - if not child_quaternions: - continue - binned_common_rotations[(str(key[0]), int(key[1]))] = ( - Rotation.from_quat( - robust_rotation_summary(child_quaternions)[0] - ) - ) - - directions: list[np.ndarray] = [] - weights: list[float] = [] - for direction in ("decreasing", "increasing"): - commands = sorted( - command - for candidate_direction, command in binned_common_rotations - if candidate_direction == direction - ) - for start_command in commands: - candidates = [ - command - for command in commands - if separation <= command - start_command <= separation + 2 - ] - if not candidates: - continue - end_command = candidates[0] - start = binned_common_rotations[(direction, start_command)] - end = binned_common_rotations[(direction, end_command)] - local_rotvec = (start.inv() * end).as_rotvec() - common_rotvec = start.apply(local_rotvec) - increment = float(np.linalg.norm(common_rotvec)) - if increment < float(minimum_rotation_rad): - continue - directions.append(common_rotvec / increment) - weights.append(increment) - - if len(directions) < int(minimum_pair_count): - raise ValueError( - "palm axis has too few full-stroke local rotation pairs: " - f"{len(directions)}/{int(minimum_pair_count)}" - ) - vectors = np.asarray(directions, dtype=float) - increments = np.asarray(weights, dtype=float) - - def consensus(selected: np.ndarray) -> np.ndarray: - scatter = np.einsum( - "n,ni,nj->ij", - increments[selected], - vectors[selected], - vectors[selected], - ) - eigenvalues, eigenvectors = np.linalg.eigh(scatter) - if not np.all(np.isfinite(eigenvalues)): - raise ValueError("palm axis local-rotation consensus is invalid") - axis = eigenvectors[:, -1] - return axis / np.linalg.norm(axis) - - selected = np.ones(len(vectors), dtype=bool) - axis = consensus(selected) - # Two deterministic refinement rounds prevent the initial scatter from - # being pulled toward a dense endpoint-bias cluster. - for _ in range(2): - deviations = np.arccos( - np.clip(np.abs(vectors @ axis), -1.0, 1.0) - ) - limit = float(np.percentile(deviations, percentile)) - selected = deviations <= limit - if int(np.count_nonzero(selected)) < int(minimum_pair_count): - raise ValueError("palm axis robust consensus retained too few pairs") - axis = consensus(selected) - return axis, int(np.count_nonzero(selected)) - - -def fit_partial_palm_orientation_measurement( - source_joint: str, - model_joint: str, - records: Sequence[Mapping[str, Any]], - *, - cycle: int, - zero_command_u8: int, - minimum_arc_rad: float = math.radians(15.0), - maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), - maximum_command_distance_u8: int = 255, -) -> PalmOrientationMeasurement: - """Fit a physical axis within a configured window around encoder zero.""" - cycle_samples = [ - dict(record) - for record in records - if int(record.get("cycle", -1)) == int(cycle) - ] - if len(cycle_samples) < 12: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has too few visible samples" - ) - if not 0.0 < float(minimum_arc_rad) < math.pi: - raise ValueError("palm orientation minimum arc must be in (0, pi)") - if float(maximum_rotation_orthogonal_rms_rad) <= 0.0: - raise ValueError( - "palm orientation rotation residual limit must be positive" - ) - command_distance = int(maximum_command_distance_u8) - if command_distance < 1 or command_distance > 255: - raise ValueError( - "palm orientation command distance must be in [1, 255]" - ) - zero = int(zero_command_u8) - samples = [ - record - for record in cycle_samples - if abs(int(record["command_u8"]) - zero) <= command_distance - ] - if len(samples) < 12: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has too few zero-adjacent " - "visible samples" - ) - reference = Rotation.from_quat( - _baseline_reference(samples, int(zero_command_u8)) - ) - samples_by_bin: dict[tuple[str, int], list[Mapping[str, Any]]] = {} - for record in samples: - key = ( - str(record.get("direction", "")), - int(record["command_u8"]), - ) - samples_by_bin.setdefault(key, []).append(record) - if len(samples_by_bin) < 6: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has too few visible command bins" - ) - binned_rotations = [ - Rotation.from_quat( - robust_rotation_summary( - [_relative_rotation(record) for record in group] - )[0] - ) - for group in samples_by_bin.values() - ] - vectors = [ - (reference.inv() * rotation).as_rotvec() - for rotation in binned_rotations - ] - axis_child = fit_rotation_axis( - vectors, [command for _direction, command in samples_by_bin] - ) - angles = np.asarray( - [float(vector @ axis_child) for vector in vectors], dtype=float - ) - observed_arc = float(np.ptp(angles)) - orthogonal_rms = float( - np.sqrt( - np.mean( - [ - np.linalg.norm( - vector - float(vector @ axis_child) * axis_child - ) - ** 2 - for vector in vectors - ] - ) - ) - ) - if observed_arc < float(minimum_arc_rad): - raise ValueError( - f"{source_joint} cycle {cycle + 1} visible rotation arc " - f"{math.degrees(observed_arc):.3f}deg is below " - f"{math.degrees(minimum_arc_rad):.3f}deg" - ) - if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad): - raise ValueError( - f"{source_joint} cycle {cycle + 1} zero-adjacent rotation " - f"residual {math.degrees(orthogonal_rms):.3f}deg exceeds " - f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg" - ) - zero_records = _near_zero_records(samples, int(zero_command_u8)) - if not zero_records: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has no visible zero pose" - ) - parent_poses = np.asarray( - [_pose_matrix(record["parent_pose_common"]) for record in zero_records] - ) - parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix(matrix[:3, :3]).as_quat() - for matrix in parent_poses - ] - )[0] - axis_parent = reference.apply(axis_child) - axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent) - axis_common /= np.linalg.norm(axis_common) - axis_estimator = "baseline_relative_so3" - incremental_pair_count = 0 - # Product recordings persist the selected moving-Tag pose in the common - # camera frame. Use the complete visible stroke only for a local-motion - # axis consensus; retain the zero-adjacent fit above as the unchanged arc - # and residual quality gate. Legacy/synthetic records without the common - # child pose keep their previous estimator exactly. - if any( - isinstance(record.get("child_pose_common"), Mapping) - for record in cycle_samples - ): - full_stroke_bins: dict[ - tuple[str, int], list[Mapping[str, Any]] - ] = {} - for record in cycle_samples: - key = ( - str(record.get("direction", "")), - int(record["command_u8"]), - ) - full_stroke_bins.setdefault(key, []).append(record) - incremental_axis, incremental_pair_count = ( - _incremental_common_rotation_axis(full_stroke_bins) - ) - if float(incremental_axis @ axis_common) < 0.0: - incremental_axis = -incremental_axis - axis_common = incremental_axis - axis_estimator = "robust_full_stroke_local_so3_v1" - state = np.median( - np.asarray( - [record["state_u8"] for record in zero_records], dtype=float - ), - axis=0, - ) - return PalmOrientationMeasurement( - source_joint=str(source_joint), - model_joint=str(model_joint), - cycle=int(cycle), - axis_common_xyz=tuple(float(value) for value in axis_common), - condition_state_u8=tuple(float(value) for value in state), - observed_arc_rad=observed_arc, - rotation_orthogonal_rms_rad=orthogonal_rms, - axis_estimator=axis_estimator, - incremental_pair_count=incremental_pair_count, - ) - - -def fit_partial_palm_orientation_measurements( - *, - sources: Mapping[str, str], - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - motor_by_source: Mapping[str, int], - baseline_command_u8: Sequence[int], - cycles: Sequence[int], - minimum_sources: int, - minimum_arc_rad: float = math.radians(15.0), - maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), - maximum_command_distance_u8: int = 255, -) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]: - """Fit every usable optional source and require configured coverage.""" - source_map = {str(name): str(model) for name, model in sources.items()} - minimum = int(minimum_sources) - if not source_map: - if minimum != 0: - raise ValueError( - "palm orientation minimum is non-zero without sources" - ) - return (), {} - if minimum < 2 or minimum > len(source_map): - raise ValueError( - "palm orientation minimum source count must be between two " - "and the configured source count" - ) - missing_motors = sorted(set(source_map) - set(motor_by_source)) - if missing_motors: - raise ValueError( - "palm orientation sources are missing motor mappings: " - + ",".join(missing_motors) - ) - fitted: list[PalmOrientationMeasurement] = [] - rejected: dict[str, str] = {} - for cycle in (int(value) for value in cycles): - cycle_fitted: list[PalmOrientationMeasurement] = [] - for source_joint, model_joint in source_map.items(): - motor = int(motor_by_source[source_joint]) - try: - measurement = fit_partial_palm_orientation_measurement( - source_joint, - model_joint, - records_by_joint.get(source_joint, ()), - cycle=cycle, - zero_command_u8=int(baseline_command_u8[motor]), - minimum_arc_rad=minimum_arc_rad, - maximum_rotation_orthogonal_rms_rad=( - maximum_rotation_orthogonal_rms_rad - ), - maximum_command_distance_u8=( - maximum_command_distance_u8 - ), - ) - except Exception as error: - rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error) - continue - cycle_fitted.append(measurement) - if len(cycle_fitted) < minimum: - cycle_reasons = { - key: value - for key, value in rejected.items() - if key.endswith(f":cycle{cycle + 1}") - } - raise ValueError( - f"palm orientation cycle {cycle + 1} has " - f"{len(cycle_fitted)}/{minimum} usable sources: " - + "; ".join( - f"{key}={value}" for key, value in cycle_reasons.items() - ) - ) - fitted.extend(cycle_fitted) - return tuple(fitted), rejected - - -def with_depth_free_axis_projection( - measurement: JointAxisMeasurement, - camera_center_common_xyz_m: Sequence[float], -) -> JointAxisMeasurement: - """Attach the source-camera interpretation plane of an axis line. - - The plane is defined by the camera centre and the fitted 3-D line, but is - only a projective observation: moving either fitted line point along its - optical ray leaves the plane unchanged. A fixed Tag mount changes the - moving point trajectory, not its recovered physical screw axis. - """ - camera_center = _vector( - camera_center_common_xyz_m, 3, name="axis camera centre" - ) - point = np.asarray(measurement.point_common_xyz_m, dtype=float) - axis = np.asarray(measurement.axis_common_xyz, dtype=float) - axis /= np.linalg.norm(axis) - ray = point - camera_center - ray_norm = float(np.linalg.norm(ray)) - if ray_norm <= 1.0e-6: - raise ValueError("axis point coincides with its source camera") - plane_normal = np.cross(ray / ray_norm, axis) - plane_norm = float(np.linalg.norm(plane_normal)) - if plane_norm <= 1.0e-6: - raise ValueError("axis projection is degenerate in its source camera") - plane_normal /= plane_norm - return replace( - measurement, - axis_point_camera_center_common_xyz_m=tuple( - float(value) for value in camera_center - ), - axis_point_interpretation_plane_normal_common_xyz=tuple( - float(value) for value in plane_normal - ), - ) - - -def cross_view_side_line_source( - measurement: JointAxisMeasurement, -) -> str | None: - """Return the side alias that supplied a fallback axis-line point.""" - sources = tuple( - str(source) - for source in getattr( - measurement, "pose_axis_line_source_joints", () - ) - ) - if ( - getattr(measurement, "axis_point_source", "") - in { - "side_circle_cross_view", - "side_circle_shared_radius_cross_view", - "side_interpretation_plane_cross_view", - } - and len(sources) == 1 - and sources[0].endswith("_side") - ): - return sources[0] - return None - - -def axis_line_uses_depth_free_interpretation_plane( - measurement: JointAxisMeasurement, -) -> bool: - """Return whether only the source-camera bearing is geometrically used.""" - return bool( - measurement.axis_point_source - in { - "side_interpretation_plane_cross_view", - "front_interpretation_plane_cross_view_validated", - } - and measurement.axis_point_camera_center_common_xyz_m is not None - ) - - -def refit_axis_line_group_with_shared_radius( - measurements: Sequence[JointAxisMeasurement], - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - canonical_zero_direction: str | None, -) -> tuple[JointAxisMeasurement, ...]: - """Refit repeated cross-view axis lines with one physical radius. - - A side-view roll alias observes the same child Tag and the same physical - lever arm in every cycle. Fitting an independent radius to each short, - near-edge-on arc leaves radius and circle centre strongly correlated; - sub-pixel PnP noise can then move the reported axis line by several - millimetres even though every trajectory has a low radial residual. - - Keep a separate centre (and therefore an independent line-repeatability - check) for every cycle, but solve one shared radius from all cycles. This - is a physical constraint rather than a relaxed quality gate: the returned - lines are still checked against the unchanged cycle RMS limit, and each - cycle's radial residual remains an independent hard check. - """ - group = tuple(measurements) - if len(group) < 2: - return group - cycles = [int(measurement.cycle) for measurement in group] - if len(set(cycles)) != len(cycles): - raise ValueError("shared-radius axis group contains duplicate cycles") - - entries: list[dict[str, Any]] = [] - initial_parameters: list[float] = [] - initial_radii: list[float] = [] - for measurement in group: - cycle_records = [ - dict(record) - for record in records - if int(record.get("cycle", -1)) == int(measurement.cycle) - ] - motion_records = [ - record - for record in cycle_records - if str(record.get("kind", "sample")) != "baseline_hold_sample" - ] - if len(motion_records) < 6: - motion_records = cycle_records - if len(motion_records) < 6: - raise ValueError( - f"cycle {measurement.cycle + 1} has too few shared-radius samples" - ) - - reference_records = _canonical_reference_records( - cycle_records, canonical_zero_direction - ) - zero_records = _near_zero_records( - reference_records, int(zero_command_u8) - ) - if not zero_records: - raise ValueError( - f"cycle {measurement.cycle + 1} has no shared-radius zero pose" - ) - parent_poses = np.asarray( - [_pose_matrix(record["parent_pose_common"]) for record in zero_records] - ) - parent_translation = np.median(parent_poses[:, :3, 3], axis=0) - parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix(matrix[:3, :3]).as_quat() - for matrix in parent_poses - ] - )[0] - parent_rotation = Rotation.from_quat(parent_quaternion) - common_axis = _vector( - measurement.axis_common_xyz, 3, name="shared-radius common axis" - ) - common_axis /= np.linalg.norm(common_axis) - parent_axis = parent_rotation.inv().apply(common_axis) - parent_axis /= np.linalg.norm(parent_axis) - - points = np.asarray( - [record["relative_translation_xyz_m"] for record in motion_records], - dtype=float, - ) - basis_x, basis_y = _plane_basis(parent_axis) - origin = np.mean(points, axis=0) - local = points - origin - points_xy = np.column_stack((local @ basis_x, local @ basis_y)) - initial = _fit_circle_with_axis(points, parent_axis) - initial_center = _vector( - initial["center_xyz_m"], 3, name="initial shared-radius centre" - ) - initial_parameters.extend( - [ - float((initial_center - origin) @ basis_x), - float((initial_center - origin) @ basis_y), - ] - ) - initial_radii.append(float(initial["radius_m"])) - entries.append( - { - "measurement": measurement, - "points": points, - "points_xy": points_xy, - "origin": origin, - "basis_x": basis_x, - "basis_y": basis_y, - "parent_axis": parent_axis, - "parent_rotation": parent_rotation, - "parent_translation": parent_translation, - } - ) - - median_radius = float(np.median(initial_radii)) - if median_radius <= 1.0e-6 or not math.isfinite(median_radius): - raise ValueError("shared-radius axis group has an invalid initial radius") - initial_value = np.asarray( - [*initial_parameters, median_radius], dtype=float - ) - - def residual(parameters: np.ndarray) -> np.ndarray: - radius = float(parameters[-1]) - return np.concatenate( - [ - np.linalg.norm( - entry["points_xy"] - - parameters[2 * index : 2 * index + 2], - axis=1, - ) - - radius - for index, entry in enumerate(entries) - ] - ) / 0.0005 - - lower = np.full(initial_value.shape, -np.inf, dtype=float) - upper = np.full(initial_value.shape, np.inf, dtype=float) - lower[-1] = max(1.0e-6, 0.5 * median_radius) - upper[-1] = 2.0 * median_radius - solution = least_squares( - residual, - initial_value, - bounds=(lower, upper), - loss="soft_l1", - f_scale=1.0, - max_nfev=3000, - ) - if not solution.success: - raise ValueError( - "shared-radius axis optimization failed: " + solution.message - ) - - shared_radius = float(solution.x[-1]) - if not math.isfinite(shared_radius): - raise ValueError("shared-radius axis optimization is non-finite") - result: list[JointAxisMeasurement] = [] - for index, entry in enumerate(entries): - center_xy = solution.x[2 * index : 2 * index + 2] - center_parent = ( - entry["origin"] - + float(center_xy[0]) * entry["basis_x"] - + float(center_xy[1]) * entry["basis_y"] - ) - # The coordinate along an infinite axis is a gauge. Retain the - # robust centre of this cycle's observed axial coordinates. - axial_offsets = ( - entry["points"] - center_parent - ) @ entry["parent_axis"] - center_parent += float(np.median(axial_offsets)) * entry["parent_axis"] - center_common = ( - entry["parent_rotation"].apply(center_parent) - + entry["parent_translation"] - ) - radial_residual = ( - np.linalg.norm( - entry["points_xy"] - center_xy, - axis=1, - ) - - shared_radius - ) - radial_rms = float(np.sqrt(np.mean(np.square(radial_residual)))) - measurement = entry["measurement"] - result.append( - replace( - measurement, - point_common_xyz_m=tuple( - float(value) for value in center_common - ), - radial_rms_m=max(float(measurement.radial_rms_m), radial_rms), - axis_point_source="side_circle_shared_radius_cross_view", - ) - ) - return tuple(result) - - -def maximum_axis_line_cycle_spread_m( - measurements: Sequence[JointAxisMeasurement], -) -> float: - """Measure repeatability of independently fitted near-parallel lines. - - Axis-line points have an arbitrary coordinate along their own direction. - Compare only the perpendicular displacement, symmetrically against both - fitted directions, so that the result remains meaningful with the small - allowed cycle-to-cycle direction variation. - """ - maximum = 0.0 - for left_index, left in enumerate(measurements): - left_axis = np.asarray(left.axis_common_xyz, dtype=float) - left_axis /= np.linalg.norm(left_axis) - left_point = np.asarray(left.point_common_xyz_m, dtype=float) - for right in measurements[left_index + 1 :]: - right_axis = np.asarray(right.axis_common_xyz, dtype=float) - right_axis /= np.linalg.norm(right_axis) - delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point - maximum = max( - maximum, - float(np.linalg.norm(np.cross(delta, left_axis))), - float(np.linalg.norm(np.cross(delta, right_axis))), - ) - return maximum - - -def axis_line_cycle_rms_m( - measurements: Sequence[JointAxisMeasurement], -) -> float: - """Return RMS line-position scatter about the four-cycle consensus. - - For parallel lines, the sum of squared pairwise distances divided by - ``n**2`` equals the mean squared distance from their centroid. Averaging - each pair's distance against both near-parallel directions preserves that - identity while avoiding an arbitrary choice of one cycle's direction. - """ - count = len(measurements) - if count < 2: - return 0.0 - squared_pairwise_sum = 0.0 - for left_index, left in enumerate(measurements): - left_axis = np.asarray(left.axis_common_xyz, dtype=float) - left_axis /= np.linalg.norm(left_axis) - left_point = np.asarray(left.point_common_xyz_m, dtype=float) - for right in measurements[left_index + 1 :]: - right_axis = np.asarray(right.axis_common_xyz, dtype=float) - right_axis /= np.linalg.norm(right_axis) - delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point - left_distance = float(np.linalg.norm(np.cross(delta, left_axis))) - right_distance = float(np.linalg.norm(np.cross(delta, right_axis))) - squared_pairwise_sum += 0.5 * ( - left_distance**2 + right_distance**2 - ) - return float(math.sqrt(squared_pairwise_sum / (count**2))) - - -def _fit_axis_point_from_pose_trajectory( - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - axis_parent_xyz: Sequence[float], - angle_axis_parent_xyz: Sequence[float], - phase_reference_point_parent_xyz: Sequence[float], - 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. - - If ``T(q)`` maps the moving Tag into its parent Tag frame, then - ``T(q) @ inv(T(0))`` is a rotation about the physical joint axis and its - translation obeys ``(I - R(q)) p = t(q)``. Solving this equation over the - complete trajectory uses both pose orientation and translation and avoids - treating a monocular Tag-centre depth arc as ground-truth geometry. - """ - samples = [dict(record) for record in records] - reference_samples = _canonical_reference_records( - samples, canonical_zero_direction - ) - zero_records = _near_zero_records(reference_samples, zero_command_u8) - if not zero_records: - raise ValueError( - f"axis-point fit has no record near baseline {zero_command_u8}" - ) - - reference_rotation = Rotation.from_quat( - _baseline_reference(reference_samples, zero_command_u8) - ).as_matrix() - reference_translation = np.median( - np.asarray( - [record["relative_translation_xyz_m"] for record in zero_records], - dtype=float, - ), - axis=0, - ) - axis = _vector(axis_parent_xyz, 3, name="axis-point direction") - axis /= np.linalg.norm(axis) - angle_axis = _vector( - angle_axis_parent_xyz, 3, name="axis-point angle direction" - ) - angle_axis /= np.linalg.norm(angle_axis) - phase_reference_point = _vector( - phase_reference_point_parent_xyz, - 3, - name="axis-point phase reference", - ) - helper = ( - np.asarray([1.0, 0.0, 0.0]) - if abs(float(axis[0])) < 0.8 - else np.asarray([0.0, 1.0, 0.0]) - ) - basis_first = np.cross(axis, helper) - basis_first /= np.linalg.norm(basis_first) - basis = np.column_stack( - (basis_first, np.cross(axis, basis_first)) - ) - - view_normal_common = None - use_image_plane_projection = False - if view_normal_common_xyz is not None: - view_normal_common = _vector( - view_normal_common_xyz, 3, name="view normal" - ) - view_normal_common /= np.linalg.norm(view_normal_common) - reference_parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix( - _pose_matrix(record["parent_pose_common"])[:3, :3] - ).as_quat() - for record in zero_records - ] - )[0] - reference_parent_rotation = Rotation.from_quat( - reference_parent_quaternion - ) - reference_view_normal_parent = reference_parent_rotation.inv().apply( - view_normal_common - ) - reference_view_normal_parent /= np.linalg.norm( - reference_view_normal_parent - ) - use_image_plane_projection = abs( - float(axis @ reference_view_normal_parent) - ) >= math.cos(AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD) - - 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] = [] - reference_radial = reference_translation - phase_reference_point - reference_radial -= axis * float(reference_radial @ axis) - reference_radial_norm = float(np.linalg.norm(reference_radial)) - if reference_radial_norm < 1.0e-6: - raise ValueError("axis-point phase reference radius is degenerate") - reference_radial /= reference_radial_norm - for record in samples: - observed_rotation = Rotation.from_quat( - _relative_rotation(record) - ).as_matrix() - observed_delta_rotation = observed_rotation @ reference_rotation.T - raw_angle = float( - Rotation.from_matrix(observed_delta_rotation).as_rotvec() - @ angle_axis - ) - if abs(raw_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: - continue - observed_translation = _vector( - record["relative_translation_xyz_m"], - 3, - name="relative translation", - ) - observed_radial = observed_translation - phase_reference_point - observed_radial -= axis * float(observed_radial @ axis) - observed_radial_norm = float(np.linalg.norm(observed_radial)) - if observed_radial_norm <= 1.0e-6: - continue - observed_radial /= observed_radial_norm - raw_angles.append(raw_angle) - translation_angles.append( - math.atan2( - float(axis @ np.cross(reference_radial, observed_radial)), - float( - np.clip(reference_radial @ observed_radial, -1.0, 1.0) - ), - ) - ) - orientation_sign = 1.0 - if raw_angles and float( - np.sum(np.asarray(raw_angles) * np.asarray(translation_angles)) - ) < 0.0: - orientation_sign = -1.0 - - for record in samples: - observed_rotation = Rotation.from_quat( - _relative_rotation(record) - ).as_matrix() - observed_translation = _vector( - record["relative_translation_xyz_m"], - 3, - name="relative translation", - ) - observed_delta_rotation = observed_rotation @ reference_rotation.T - signed_angle = float( - Rotation.from_matrix(observed_delta_rotation).as_rotvec() - @ angle_axis - ) * orientation_sign - if abs(signed_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: - continue - # Direction and angle have deliberately separate sources. A trusted - # upstream/circle direction may replace a biased planar-PnP axis, while - # the relative-orientation trajectory still provides a repeatable - # scalar travel. Reconstruct the admissible revolute motion before - # solving its axis line instead of feeding a contradictory 3-D - # rotation into the translation equations. - # The rotation-trajectory axis is undirected. Align its scalar sign - # with the selected physical axis using the Tag-centre trajectory; - # this remains robust even when a trusted upstream direction replaces - # a badly biased planar-PnP orientation axis. - delta_rotation = Rotation.from_rotvec( - axis * signed_angle - ).as_matrix() - delta_translation = ( - observed_translation - - delta_rotation @ reference_translation - ) - # A screw-driven revolute joint may carry real translation along its - # axis. That component is in the null space of (I - R), contains no - # information about the perpendicular axis-line point, and must not - # inflate or bias the line fit as if the mechanism were pure rotary. - axis_projection = ( - np.eye(3) - np.outer(axis, axis) - if allow_axial_translation - else np.eye(3) - ) - 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( - _pose_matrix(record["parent_pose_common"])[:3, :3] - ) - view_normal_parent = parent_rotation.inv().apply( - view_normal_common - ) - view_normal_parent /= np.linalg.norm(view_normal_parent) - image_projection = np.eye(3) - np.outer( - view_normal_parent, view_normal_parent - ) - projection = image_projection @ axis_projection - # A single end-on camera cannot distinguish absolute depth from a - # shift along the revolute axis. Keep the observable image-plane - # equations only; adding a free baseline-depth variable makes the - # axis point rank-deficient and pretends that this gauge is - # observable. Parallel phase chains are later compared in the - # same view, where their common depth gauge cancels. - used_image_plane_projection = True - matrices.append(projection @ axis_point_matrix) - translations.append(projection @ delta_translation) - - if len(matrices) < 6: - raise ValueError("axis-point fit has too few observable pose samples") - matrix = np.vstack(matrices) - translation = np.concatenate(translations) - singular_values = np.linalg.svd(matrix, compute_uv=False) - if ( - singular_values.size < 2 - or singular_values[-1] <= 1.0e-9 - or singular_values[-1] / singular_values[0] < 1.0e-3 - ): - raise ValueError("axis-point pose trajectory is ill-conditioned") - - unknown_count = matrix.shape[1] - solution = least_squares( - lambda value: ( - matrix @ value - translation - ) / AXIS_POINT_RESIDUAL_SCALE_M, - np.zeros(unknown_count, dtype=float), - loss="soft_l1", - f_scale=1.0, - max_nfev=1000, - ) - if not solution.success: - raise ValueError( - "axis-point pose optimization failed: " + solution.message - ) - 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 - else "pose_trajectory_3d" - ) - return basis @ solution.x[:2], rms, source - - -def fit_joint_axis_measurement( - joint: str, - records: Sequence[Mapping[str, Any]], - *, - cycle: int, - zero_command_u8: int, - axis_common_constraint: Sequence[float] | None = None, - 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 = [ - dict(record) for record in records if int(record["cycle"]) == int(cycle) - ] - if len(samples) < 12: - raise ValueError(f"{joint} cycle {cycle + 1} has too few samples") - points = np.asarray( - [record["relative_translation_xyz_m"] for record in samples], dtype=float - ) - free_circle_axis, free_plane_rms = _fit_plane_axis([points]) - singular_values = np.linalg.svd( - points - np.mean(points, axis=0), compute_uv=False - ) - circle_axis_observability = ( - 0.0 - if singular_values.size < 2 or singular_values[0] <= 1.0e-12 - else float(singular_values[1] / singular_values[0]) - ) - - reference_samples = _canonical_reference_records( - samples, canonical_zero_direction - ) - reference = Rotation.from_quat( - _baseline_reference(reference_samples, zero_command_u8) - ) - rotation_vectors = [ - (reference.inv() * Rotation.from_quat(_relative_rotation(record))).as_rotvec() - for record in samples - ] - commands = [int(record["command_u8"]) for record in samples] - rotation_axis_child = fit_rotation_axis(rotation_vectors, commands) - # reference maps the child Tag frame at baseline into the parent Tag - # frame. The quaternion delta axis is expressed in that child frame, - # while the fitted centre circle is expressed in the parent frame. This - # conversion is what makes arbitrary Tag mounting rotations harmless. - rotation_axis = reference.apply(rotation_axis_child) - if float(rotation_axis @ free_circle_axis) < 0.0: - free_circle_axis = -free_circle_axis - disagreement = math.acos( - float(np.clip(rotation_axis @ free_circle_axis, -1.0, 1.0)) - ) - - zero_records = _near_zero_records(reference_samples, zero_command_u8) - if not zero_records: - raise ValueError( - f"{joint} cycle has no record near baseline {zero_command_u8}" - ) - parent_poses = np.asarray( - [_pose_matrix(record["parent_pose_common"]) for record in zero_records] - ) - parent_translation = np.median(parent_poses[:, :3, 3], axis=0) - parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix(matrix[:3, :3]).as_quat() - for matrix in parent_poses - ] - )[0] - parent_rotation = Rotation.from_quat(parent_quaternion) - axis_direction_source = "rotation" - - if axis_common_constraint is not None: - common_axis = _vector( - axis_common_constraint, 3, name="common axis constraint" - ) - common_axis /= np.linalg.norm(common_axis) - fitted_axis = parent_rotation.inv().apply(common_axis) - if float(fitted_axis @ rotation_axis) < 0.0: - fitted_axis = -fitted_axis - circle = _fit_circle_with_axis(points, fitted_axis) - plane_rms = float(circle["plane_rms_m"]) - axis_direction_source = "upstream_constraint" - elif str(joint) in constrained_circle_joints: - # The side/front views are close to end-on for these axes. Monocular - # planar-PnP depth bias can therefore tilt even an apparently smooth, - # low-residual Tag-centre circle. The relative orientation trajectory - # observes the screw-axis direction directly and is invariant to an - # arbitrary fixed Tag mounting rotation; use it to constrain the 3-D - # circle and estimate only the axis line. Never switch the direction - # source based on a rotation-vs-circle threshold: that discontinuity - # makes otherwise identical repeat cycles choose different models. - fitted_axis = rotation_axis - circle = _fit_circle_with_axis(points, fitted_axis) - plane_rms = float(circle["plane_rms_m"]) - else: - # Oblique trajectories with an observable 3-D motion plane retain the - # independent rotation/centre cross-check and fuse both estimates. - free_circle = _fit_circle_with_axis(points, free_circle_axis) - fitted_axis = rotation_axis + free_circle_axis - if float(np.linalg.norm(fitted_axis)) < 1.0e-9: - raise ValueError(f"{joint} rotation and centre axes are opposed") - fitted_axis /= np.linalg.norm(fitted_axis) - circle = free_circle - plane_rms = max( - float(free_plane_rms), float(circle["plane_rms_m"]) - ) - 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, - zero_command_u8=zero_command_u8, - axis_parent_xyz=fitted_axis, - angle_axis_parent_xyz=rotation_axis, - 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=separate_axial, - residual_diagnostics=residual_diagnostics, - ) - ) - point_common = parent_rotation.apply(point_parent) + parent_translation - state = np.median( - np.asarray([record["state_u8"] for record in zero_records], dtype=float), - axis=0, - ) - return JointAxisMeasurement( - joint=str(joint), - cycle=int(cycle), - axis_common_xyz=tuple(float(value) for value in axis_common), - point_common_xyz_m=tuple(float(value) for value in point_common), - condition_state_u8=tuple(float(value) for value in state), - plane_rms_m=float(plane_rms), - radial_rms_m=float(circle["radial_rms_m"]), - rotation_circle_axis_difference_rad=float(disagreement), - view_normal_common_xyz=( - None - if view_normal_common_xyz is None - else tuple( - float(value) - for value in _vector( - view_normal_common_xyz, 3, name="view normal" - ) - ) - ), - axis_direction_source=axis_direction_source, - 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),), - ) - - -@dataclass(frozen=True) -class _UrdfJoint: - name: str - parent: str - child: str - origin: np.ndarray - axis: np.ndarray - mimic_joint: str | None - mimic_multiplier: float - mimic_offset: float - - -class UrdfKinematicModel: - def __init__(self, source: str | Path) -> None: - self.source = Path(source).expanduser().resolve() - if not self.source.is_file(): - raise ValueError(f"source URDF does not exist: {self.source}") - root = ET.parse(self.source).getroot() - self.joints: dict[str, _UrdfJoint] = {} - self.parent_joint_by_child: dict[str, str] = {} - for element in root.findall("joint"): - if element.get("type") == "fixed": - pass - name = str(element.get("name")) - parent = element.find("parent") - child = element.find("child") - if parent is None or child is None: - continue - origin_node = element.find("origin") - xyz = _parse_triplet( - "0 0 0" if origin_node is None else origin_node.get("xyz", "0 0 0") - ) - rpy = _parse_triplet( - "0 0 0" if origin_node is None else origin_node.get("rpy", "0 0 0") - ) - origin = np.eye(4) - origin[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() - origin[:3, 3] = xyz - axis_node = element.find("axis") - axis = _parse_triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - axis /= np.linalg.norm(axis) - mimic = element.find("mimic") - model = _UrdfJoint( - name=name, - parent=str(parent.get("link")), - child=str(child.get("link")), - origin=origin, - axis=axis, - mimic_joint=None if mimic is None else str(mimic.get("joint")), - mimic_multiplier=( - 1.0 if mimic is None else float(mimic.get("multiplier", "1")) - ), - mimic_offset=( - 0.0 if mimic is None else float(mimic.get("offset", "0")) - ), - ) - self.joints[name] = model - self.parent_joint_by_child[model.child] = name - - def _chain(self, target_joint: str) -> list[_UrdfJoint]: - if target_joint not in self.joints: - raise ValueError(f"URDF is missing joint {target_joint}") - result: list[_UrdfJoint] = [] - current = self.joints[target_joint] - while True: - result.append(current) - parent_joint = self.parent_joint_by_child.get(current.parent) - if parent_joint is None: - break - current = self.joints[parent_joint] - result.reverse() - return result - - def link_transform( - self, - target_joint: str, - *, - zero_offsets: Mapping[str, float], - joint_angles: Mapping[str, float], - independent_mimic_angles: bool = False, - ) -> np.ndarray: - """Return base-to-child-link pose for calibration validation. - - ``independent_mimic_angles`` lets the product validator use the five - independently measured passive JSON curves while the emitted URDF's - original mimic XML remains untouched. - """ - transform = np.eye(4) - resolved = {str(name): float(value) for name, value in joint_angles.items()} - for joint in self._chain(target_joint): - transform = transform @ joint.origin @ _axis_rotation( - joint.axis, float(zero_offsets.get(joint.name, 0.0)) - ) - if independent_mimic_angles and joint.name in resolved: - angle = resolved[joint.name] - elif joint.mimic_joint is None: - angle = float(resolved.get(joint.name, 0.0)) - else: - angle = ( - joint.mimic_multiplier - * float(resolved.get(joint.mimic_joint, 0.0)) - + joint.mimic_offset - ) - resolved[joint.name] = angle - transform = transform @ _axis_rotation(joint.axis, angle) - return transform - - def axis_line( - self, - target_joint: str, - *, - zero_offsets: Mapping[str, float], - joint_angles: Mapping[str, float], - ) -> tuple[np.ndarray, np.ndarray]: - transform = np.eye(4) - resolved_angles = dict(joint_angles) - for joint in self._chain(target_joint): - corrected_origin = joint.origin @ _axis_rotation( - joint.axis, float(zero_offsets.get(joint.name, 0.0)) - ) - joint_frame = transform @ corrected_origin - if joint.name == target_joint: - return ( - joint_frame[:3, :3] @ joint.axis, - joint_frame[:3, 3].copy(), - ) - if joint.mimic_joint is None: - angle = float(resolved_angles.get(joint.name, 0.0)) - else: - angle = ( - joint.mimic_multiplier - * float(resolved_angles.get(joint.mimic_joint, 0.0)) - + joint.mimic_offset - ) - resolved_angles[joint.name] = angle - transform = joint_frame @ _axis_rotation(joint.axis, angle) - raise RuntimeError(f"could not resolve axis line for {target_joint}") - - -def _parse_triplet(value: str) -> np.ndarray: - result = np.asarray([float(item) for item in str(value).split()], dtype=float) - if result.shape != (3,) or not np.all(np.isfinite(result)): - raise ValueError(f"invalid URDF vector: {value}") - return result - - -def _axis_rotation(axis: Sequence[float], angle: float) -> np.ndarray: - result = np.eye(4) - result[:3, :3] = Rotation.from_rotvec( - _vector(axis, 3, name="joint axis") * float(angle) - ).as_matrix() - return result - - -@dataclass(frozen=True) -class ZeroSolveResult: - direct_offsets_rad: Mapping[str, float] - all_active_offsets_rad: Mapping[str, float] - base_translation_xyz_m: tuple[float, float, float] - base_quaternion_xyzw: tuple[float, float, float, float] - validation_errors_rad: tuple[float, ...] - validation_error_by_joint_rad: Mapping[str, float] - validation_line_error_by_joint_m: Mapping[str, float] - axis_line_rms_m: float - passed: bool - cycle_offsets_rad: Mapping[str, tuple[float, ...]] - offset_uncertainty_rad: Mapping[str, float] - offset_confidence_half_width_rad: Mapping[str, float] - training_cycles: tuple[int, ...] - validation_cycle: int - validation_original_error_by_joint_rad: Mapping[str, float] - validation_improvement_by_joint_rad: Mapping[str, float] - validation_improvement_confidence_lower_rad: Mapping[str, float] - observability_rank: int - observability_parameter_count: int - observability_condition_number: float - offset_covariance_rad2: Mapping[str, float] - 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( - *, - thumb_result: ZeroSolveResult, - preserved_offsets_rad: Mapping[str, float] | None = None, - companion_result: ZeroSolveResult | None = None, -) -> ZeroSolveResult: - """Merge an independent thumb solve without allowing reverse coupling. - - ``companion_result`` is the finger/full solver result whose thumb values - were fixed to ``thumb_result``. For a thumb-only run, the 12 certified - finger values are supplied through ``preserved_offsets_rad`` instead. - """ - thumb_names = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - if set(thumb_result.direct_offsets_rad) != thumb_names: - raise ValueError("independent thumb result must contain exactly four zeros") - full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) - direct_names = set(full_profile.direct_zero_joints) - preserved = { - str(name): float(value) - for name, value in (preserved_offsets_rad or {}).items() - } - if set(preserved) - (direct_names - thumb_names): - raise ValueError("preserved offsets contain a thumb or unknown joint") - if any(not math.isfinite(value) for value in preserved.values()): - raise ValueError("preserved offsets must be finite") - - if companion_result is None: - if set(preserved) != direct_names - thumb_names: - raise ValueError("thumb-only merge requires all 12 certified finger zeros") - direct = dict(preserved) - all_active = {name: 0.0 for name in full_profile.hand.active_joints} - all_active.update(preserved) - base_result = thumb_result - else: - if set(companion_result.direct_offsets_rad) != direct_names: - raise ValueError("full companion result must contain all 16 zeros") - direct = dict(companion_result.direct_offsets_rad) - all_active = dict(companion_result.all_active_offsets_rad) - base_result = companion_result - direct.update(thumb_result.direct_offsets_rad) - all_active.update(thumb_result.direct_offsets_rad) - - def merged_map(name: str) -> dict[str, Any]: - values = ( - {} - if companion_result is None - else dict(getattr(companion_result, name)) - ) - values.update(dict(getattr(thumb_result, name))) - return values - - failure_reasons = merged_map("failure_reasons") - passed = bool( - thumb_result.passed - and (companion_result is None or companion_result.passed) - ) - return replace( - base_result, - direct_offsets_rad=direct, - all_active_offsets_rad=all_active, - validation_errors_rad=( - tuple(thumb_result.validation_errors_rad) - if companion_result is None - else ( - *companion_result.validation_errors_rad, - *thumb_result.validation_errors_rad, - ) - ), - validation_error_by_joint_rad=merged_map( - "validation_error_by_joint_rad" - ), - validation_line_error_by_joint_m=merged_map( - "validation_line_error_by_joint_m" - ), - axis_line_rms_m=( - thumb_result.axis_line_rms_m - if companion_result is None - else max( - companion_result.axis_line_rms_m, - thumb_result.axis_line_rms_m, - ) - ), - passed=passed, - cycle_offsets_rad=merged_map("cycle_offsets_rad"), - offset_uncertainty_rad=merged_map("offset_uncertainty_rad"), - offset_confidence_half_width_rad=merged_map( - "offset_confidence_half_width_rad" - ), - validation_original_error_by_joint_rad=merged_map( - "validation_original_error_by_joint_rad" - ), - validation_improvement_by_joint_rad=merged_map( - "validation_improvement_by_joint_rad" - ), - validation_improvement_confidence_lower_rad=merged_map( - "validation_improvement_confidence_lower_rad" - ), - offset_covariance_rad2=merged_map("offset_covariance_rad2"), - axis_cone_mismatch_by_joint_rad=merged_map( - "axis_cone_mismatch_by_joint_rad" - ), - axis_cone_bias_classification_by_joint=merged_map( - "axis_cone_bias_classification_by_joint" - ), - failure_reasons=failure_reasons, - ) - - -def expand_right_19_thumb_zero_result_with_cad_fingers( - thumb_result: ZeroSolveResult, -) -> ZeroSolveResult: - """Build a standalone thumb result with every finger left at source CAD. - - This is intentionally different from inheriting a prior finger - calibration: the twelve non-thumb values are explicit zeros and no finger - observation or artifact is required. - """ - full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) - thumb_names = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - if set(thumb_result.direct_offsets_rad) != thumb_names: - raise ValueError("independent thumb result must contain exactly four zeros") - direct = {name: 0.0 for name in full_profile.direct_zero_joints} - direct.update(thumb_result.direct_offsets_rad) - all_active = {name: 0.0 for name in full_profile.hand.active_joints} - all_active.update(thumb_result.direct_offsets_rad) - return replace( - thumb_result, - direct_offsets_rad=direct, - all_active_offsets_rad=all_active, - ) - - -def _angles_from_state( - state_u8: Sequence[float], - *, - curves: Mapping[str, JointCurveFit], - motor_by_joint: Mapping[str, int], - inherited_zero_joints: Mapping[str, str] = INHERITED_ZERO_JOINTS, -) -> dict[str, float]: - state = np.asarray(state_u8, dtype=float) - result: dict[str, float] = {} - for joint, motor in motor_by_joint.items(): - source = inherited_zero_joints.get(joint, joint) - if source not in curves: - continue - command = int(np.clip(np.rint(state[int(motor)]), 0, 255)) - result[joint] = float(curves[source].angle_rad[command]) - return result - - -def solve_urdf_zero_offsets( - *, - source_urdf: str | Path, - measurements: Sequence[JointAxisMeasurement], - palm_orientation_measurements: Sequence[ - PalmOrientationMeasurement - ] = (), - curves: Mapping[str, JointCurveFit], - motor_by_joint: Mapping[str, int], - training_cycles: Sequence[int] = (0, 1), - validation_cycle: int = 2, - maximum_offset_rad: float = math.radians(20.0), - finger_maximum_offset_rad: float = math.radians(3.0), - joint_maximum_offset_rad: Mapping[str, float] | None = None, - maximum_cycle_difference_rad: float = math.radians(0.75), - minimum_applied_offset_rad: float = math.radians(0.3), - significance_sigma: float = 3.0, - maximum_validation_mae_rad: float = math.radians(1.0), - maximum_validation_p95_rad: float = math.radians(2.0), - maximum_validation_error_rad: float | None = None, - maximum_confidence_half_width_rad: float | None = None, - maximum_axis_cone_mismatch_rad: float = math.radians(5.0), - maximum_systematic_axis_cone_bias_rad: float | None = None, - maximum_pose_axis_line_rms_m: float = 0.001, - maximum_observability_condition_number: float = 1.0e10, - hand_type: str = "left", - tag_layout: str = "legacy_11", - fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, - static_output_zero_offsets_rad: Mapping[str, float] | None = None, - zero_profile: ZeroCalibrationProfile | None = None, -) -> ZeroSolveResult: - """Solve all directly observable zero offsets without fitting CAD errors. - - The product palm pose uses the robust common root-axis direction together - with independent partial-sweep MCP-pitch directions; root-line points - determine translation only. Legacy layouts retain their single observed - reference chain. Encoder zeros are then fitted from one angular residual - per joint. This prevents link length, along-axis Tag placement and - monocular depth bias from becoming encoder-zero corrections. - """ - profile = ( - get_zero_calibration_profile(hand_type, tag_layout) - if zero_profile is None - else zero_profile - ) - if profile.hand.side != str(hand_type).lower(): - raise ValueError("zero profile hand side does not match hand_type") - if profile.hand.layout_id != str(tag_layout): - raise ValueError("zero profile layout does not match tag_layout") - model = UrdfKinematicModel(source_urdf) - training = [m for m in measurements if m.cycle in set(training_cycles)] - validation = [m for m in measurements if m.cycle == int(validation_cycle)] - expected = set(profile.axis_joints) - if ( - {m.joint for m in training} != expected - or {m.joint for m in validation} != expected - ): - raise ValueError("axis measurements do not contain all required joints/cycles") - configured_palm_sources = { - str(name): str(model_joint) - for name, model_joint in ( - profile.hand.palm_orientation_sources or {} - ).items() - } - orientation_measurements = tuple(palm_orientation_measurements) - if configured_palm_sources: - minimum_orientation_sources = int( - profile.hand.minimum_palm_orientation_sources - ) - unknown_orientation_sources = sorted( - { - item.source_joint - for item in orientation_measurements - if item.source_joint not in configured_palm_sources - or configured_palm_sources[item.source_joint] - != item.model_joint - } - ) - if unknown_orientation_sources: - raise ValueError( - "palm orientation measurements do not match the product " - "profile: " - + ",".join(unknown_orientation_sources) - ) - required_orientation_cycles = { - *(int(value) for value in training_cycles), - int(validation_cycle), - } - for cycle in required_orientation_cycles: - cycle_sources = { - item.source_joint - for item in orientation_measurements - if int(item.cycle) == cycle - } - if len(cycle_sources) < minimum_orientation_sources: - raise ValueError( - f"palm orientation cycle {cycle + 1} contains " - f"{len(cycle_sources)}/{minimum_orientation_sources} " - "qualified sources" - ) - elif orientation_measurements: - raise ValueError( - "palm orientation measurements were supplied without a profile" - ) - orientation_training = tuple( - item - for item in orientation_measurements - if int(item.cycle) in set(int(value) for value in training_cycles) - ) - orientation_validation = tuple( - item - for item in orientation_measurements - if int(item.cycle) == int(validation_cycle) - ) - orientation_by_model_cycle: dict[ - tuple[str, int], PalmOrientationMeasurement - ] = {} - for item in orientation_measurements: - key = (str(item.model_joint), int(item.cycle)) - if key in orientation_by_model_cycle: - raise ValueError( - "palm orientation has multiple sources for " - f"{key[0]} cycle {key[1] + 1}" - ) - orientation_by_model_cycle[key] = item - if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad: - raise ValueError("finger maximum offset must be positive and no larger than thumb") - joint_limits = { - str(name): float(value) - for name, value in dict(joint_maximum_offset_rad or {}).items() - } - unknown_joint_limits = sorted(set(joint_limits) - set(profile.direct_zero_joints)) - if unknown_joint_limits: - raise ValueError( - "joint maximum offsets contain unknown direct joints: " - + ",".join(unknown_joint_limits) - ) - if any( - not math.isfinite(value) or not 0.0 < value <= math.radians(90.0) - for value in joint_limits.values() - ): - raise ValueError("joint maximum offsets must be finite and in (0, 90deg]") - if maximum_cycle_difference_rad <= 0.0: - raise ValueError("maximum cycle difference must be positive") - if minimum_applied_offset_rad < 0.0 or significance_sigma < 0.0: - raise ValueError("zero significance thresholds must be non-negative") - if maximum_axis_cone_mismatch_rad <= 0.0: - raise ValueError("maximum axis cone mismatch must be positive") - if ( - maximum_systematic_axis_cone_bias_rad is not None - and ( - maximum_systematic_axis_cone_bias_rad - <= maximum_axis_cone_mismatch_rad - or maximum_systematic_axis_cone_bias_rad > math.radians(90.0) - ) - ): - raise ValueError( - "maximum systematic axis cone bias must exceed the precision " - "limit and be at most 90deg" - ) - if maximum_pose_axis_line_rms_m <= 0.0: - raise ValueError("maximum pose axis-line RMS must be positive") - if maximum_observability_condition_number <= 1.0: - raise ValueError( - "maximum observability condition number must be greater than one" - ) - if ( - maximum_validation_error_rad is not None - and maximum_validation_error_rad <= 0.0 - ): - raise ValueError("maximum validation error must be positive") - if ( - maximum_confidence_half_width_rad is not None - and maximum_confidence_half_width_rad <= 0.0 - ): - raise ValueError("maximum confidence half width must be positive") - - def predicted_local( - measurement: JointAxisMeasurement, - offsets: Mapping[str, float], - ) -> tuple[np.ndarray, np.ndarray]: - angles = _angles_from_state( - ( - measurement.condition_state_u8 - if measurement.condition_command_u8 is None - else measurement.condition_command_u8 - ), - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - return model.axis_line( - measurement.joint, - zero_offsets=offsets, - joint_angles=angles, - ) - - def predicted_palm_orientation_local( - measurement: PalmOrientationMeasurement, - offsets: Mapping[str, float], - ) -> np.ndarray: - angles = _angles_from_state( - measurement.condition_state_u8, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - axis, _ = model.axis_line( - measurement.model_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - return axis / np.linalg.norm(axis) - - fixed_offsets = { - str(name): float(value) - for name, value in ( - profile.fixed_direct_zero_offsets_rad - if fixed_direct_zero_offsets_rad is None - else fixed_direct_zero_offsets_rad - ).items() - } - unknown_fixed_offsets = sorted( - set(fixed_offsets) - set(profile.direct_zero_joints) - ) - if unknown_fixed_offsets: - raise ValueError( - "fixed direct zero offsets contain unknown joints: " - + ",".join(unknown_fixed_offsets) - ) - if any( - not math.isfinite(value) or abs(value) > math.radians(90.0) - for value in fixed_offsets.values() - ): - raise ValueError( - "fixed direct zero offsets must be finite and within +/-90deg" - ) - output_offsets = { - str(name): float(value) - for name, value in ( - profile.static_output_zero_offsets_rad - if static_output_zero_offsets_rad is None - else static_output_zero_offsets_rad - ).items() - } - unknown_output_offsets = sorted( - set(output_offsets) - set(profile.direct_zero_joints) - ) - if unknown_output_offsets: - raise ValueError( - "static output zero offsets contain unknown joints: " - + ",".join(unknown_output_offsets) - ) - if any( - not math.isfinite(value) or abs(value) > math.radians(90.0) - for value in output_offsets.values() - ): - raise ValueError( - "static output zero offsets must be finite and within +/-90deg" - ) - zero_offsets = {name: 0.0 for name in profile.direct_zero_joints} - zero_offsets.update(fixed_offsets) - def fit_base_pose( - selected: Sequence[JointAxisMeasurement], - pose_offsets: Mapping[str, float] | None = None, - ) -> tuple[Rotation, np.ndarray]: - pose_zero_offsets = zero_offsets if pose_offsets is None else pose_offsets - anchors_by_joint = { - name: [item for item in selected if item.joint == name] - for name in profile.root_anchor_joints - } - if any(not items for items in anchors_by_joint.values()): - raise ValueError("zero solve is missing root axis anchors") - - def undirected_axis_average( - values: Sequence[Sequence[float]], - ) -> np.ndarray: - vectors = np.asarray(values, dtype=float) - reference = vectors[0] / np.linalg.norm(vectors[0]) - signs = np.where(vectors @ reference < 0.0, -1.0, 1.0) - result = np.sum(vectors * signs[:, None], axis=0) - norm = float(np.linalg.norm(result)) - if norm < 1.0e-9: - raise ValueError("root axis average is degenerate") - return result / norm - - root_names = tuple(sorted(profile.root_anchor_joints)) - predicted_axes: dict[str, np.ndarray] = {} - predicted_points: dict[str, np.ndarray] = {} - observed_axes: dict[str, np.ndarray] = {} - observed_points: dict[str, np.ndarray] = {} - for name in root_names: - items = anchors_by_joint[name] - predicted = [predicted_local(item, pose_zero_offsets) for item in items] - predicted_axes[name] = undirected_axis_average( - [axis for axis, _ in predicted] - ) - predicted_points[name] = np.median( - np.asarray([point for _, point in predicted]), axis=0 - ) - observed_axes[name] = undirected_axis_average( - [item.axis_common_xyz for item in items] - ) - observed_points[name] = np.median( - np.asarray( - [item.point_common_xyz_m for item in items], dtype=float - ), - axis=0, - ) - - if ( - profile.hand.layout_id == G20_RIGHT_19_LAYOUT - and profile.base_pose_strategy == "full_hand" - ): - # In the product layout none of the four finger-roll zeros is a - # mechanical prior. Using one finger's pitch axis to orient the - # palm would therefore absorb that finger's roll zero into the - # palm pose. The named positions of five parallel root lines do - # observe rotation about their common direction, independent of - # all five root-joint zeros. Fit that transverse line pattern and - # leave the unobservable translation along the common direction - # at zero; all zero-sensitive phase residuals use line-to-line - # differences and are invariant to that gauge. - predicted_common = undirected_axis_average( - [predicted_axes[name] for name in root_names] - ) - observed_common = undirected_axis_average( - [observed_axes[name] for name in root_names] - ) - predicted_center = np.mean( - np.asarray([predicted_points[name] for name in root_names]), - axis=0, - ) - observed_center = np.mean( - np.asarray([observed_points[name] for name in root_names]), - axis=0, - ) - predicted_pattern = { - name: ( - (delta := predicted_points[name] - predicted_center) - - predicted_common * float(delta @ predicted_common) - ) - for name in root_names - } - observed_pattern = { - name: ( - (delta := observed_points[name] - observed_center) - - observed_common * float(delta @ observed_common) - ) - for name in root_names - } - if max( - np.linalg.norm(value) for value in predicted_pattern.values() - ) < 0.003: - raise ValueError("root axis-line pattern is degenerate") - if max( - np.linalg.norm(value) for value in observed_pattern.values() - ) < 0.003: - raise ValueError("observed root axis-line pattern is degenerate") - - def align_axis(source: np.ndarray, target: np.ndarray) -> Rotation: - source = source / np.linalg.norm(source) - target = target / np.linalg.norm(target) - cross = np.cross(source, target) - cross_norm = float(np.linalg.norm(cross)) - dot = float(np.clip(source @ target, -1.0, 1.0)) - if cross_norm > 1.0e-10: - return Rotation.from_rotvec( - cross / cross_norm * math.atan2(cross_norm, dot) - ) - if dot > 0.0: - return Rotation.identity() - basis = np.eye(3)[int(np.argmin(np.abs(source)))] - axis = np.cross(source, basis) - axis /= np.linalg.norm(axis) - return Rotation.from_rotvec(axis * math.pi) - - candidates: list[tuple[float, Rotation, np.ndarray]] = [] - for sign in (1.0, -1.0): - signed_observed_axis = sign * observed_common - axis_rotation = align_axis( - predicted_common, signed_observed_axis - ) - mapped_pattern = { - name: axis_rotation.apply(predicted_pattern[name]) - for name in root_names - } - cosine = sum( - float(mapped_pattern[name] @ observed_pattern[name]) - for name in root_names - ) - sine = sum( - float( - signed_observed_axis - @ np.cross( - mapped_pattern[name], observed_pattern[name] - ) - ) - for name in root_names - ) - phase_rotation = Rotation.from_rotvec( - signed_observed_axis * math.atan2(sine, cosine) - ) - rotation = phase_rotation * axis_rotation - translation_samples = [] - for name in root_names: - delta = observed_points[name] - rotation.apply( - predicted_points[name] - ) - translation_samples.append( - delta - - signed_observed_axis - * float(delta @ signed_observed_axis) - ) - translation = np.median( - np.asarray(translation_samples), axis=0 - ) - - # Root-axis points recovered from planar Tags occasionally - # contain a large but repeatable depth/line-position outlier. - # A plain Procrustes sum lets one such point rotate the palm - # frame and then makes the same bias look like a common zero - # offset on all four finger-roll joints. Refine only the - # rigid palm pose with a millimetre-scale robust loss. At - # least three mutually consistent named root lines therefore - # determine the transverse pattern while an outlier remains - # visible in the line diagnostics below. - transverse_helper = np.eye(3)[ - int(np.argmin(np.abs(signed_observed_axis))) - ] - transverse_first = np.cross( - signed_observed_axis, transverse_helper - ) - transverse_first /= np.linalg.norm(transverse_first) - transverse_second = np.cross( - signed_observed_axis, transverse_first - ) - - def robust_root_pattern_residual( - value: np.ndarray, - ) -> np.ndarray: - candidate_rotation = ( - Rotation.from_rotvec( - signed_observed_axis * float(value[0]) - ) - * rotation - ) - candidate_translation = ( - translation - + transverse_first * float(value[1]) - + transverse_second * float(value[2]) - ) - residuals: list[float] = [] - for name in root_names: - delta = observed_points[name] - ( - candidate_rotation.apply(predicted_points[name]) - + candidate_translation - ) - residuals.extend( - ( - float(delta @ transverse_first) / 0.001, - float(delta @ transverse_second) / 0.001, - ) - ) - return np.asarray(residuals, dtype=float) - - root_pattern_solution = least_squares( - robust_root_pattern_residual, - np.zeros(3, dtype=float), - bounds=( - np.asarray([-math.pi, -0.25, -0.25]), - np.asarray([math.pi, 0.25, 0.25]), - ), - loss="soft_l1", - f_scale=1.0, - max_nfev=1000, - ) - if not root_pattern_solution.success: - raise ValueError("robust root axis-line pattern fit failed") - rotation = ( - Rotation.from_rotvec( - signed_observed_axis - * float(root_pattern_solution.x[0]) - ) - * rotation - ) - translation = ( - translation - + transverse_first * float(root_pattern_solution.x[1]) - + transverse_second * float(root_pattern_solution.x[2]) - ) - axis_errors: list[float] = [] - for item in selected: - predicted_axis, _ = predicted_local(item, pose_zero_offsets) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - axis_errors.append( - math.acos( - abs( - float( - np.clip( - rotation.apply(predicted_axis) - @ observed_axis, - -1.0, - 1.0, - ) - ) - ) - ) - ) - line_errors = [] - for name in root_names: - predicted_point = ( - rotation.apply(predicted_points[name]) + translation - ) - delta = observed_points[name] - predicted_point - line_errors.append( - float( - np.linalg.norm( - delta - - signed_observed_axis - * float(delta @ signed_observed_axis) - ) - ) - ) - score = float(np.mean(np.square(axis_errors))) + float( - np.mean(np.square(np.asarray(line_errors) / 0.01)) - ) - candidates.append((score, rotation, translation)) - _, rotation, translation = min( - candidates, key=lambda item: item[0] - ) - non_root_items = [ - item - for item in selected - if item.joint not in profile.root_anchor_joints - ] - if not non_root_items: - raise ValueError( - "non-parallel axes are required for palm axial translation" - ) - - def axial_translation_residual(value: np.ndarray) -> np.ndarray: - candidate_translation = ( - translation + observed_common * float(value[0]) - ) - residuals: list[float] = [] - for item in non_root_items: - predicted_axis, predicted_point = predicted_local( - item, pose_zero_offsets - ) - predicted_axis = rotation.apply(predicted_axis) - predicted_point = ( - rotation.apply(predicted_point) - + candidate_translation - ) - observed_axis = np.asarray( - item.axis_common_xyz, dtype=float - ) - if float(predicted_axis @ observed_axis) < 0.0: - observed_axis = -observed_axis - delta = np.asarray( - item.point_common_xyz_m, dtype=float - ) - predicted_point - perpendicular = delta - observed_axis * float( - delta @ observed_axis - ) - residuals.extend( - float(component) / 0.001 - for component in perpendicular - ) - return np.asarray(residuals, dtype=float) - - axial_solution = least_squares( - axial_translation_residual, - np.asarray([0.0]), - bounds=(np.asarray([-1.0]), np.asarray([1.0])), - loss="soft_l1", - f_scale=1.0, - ) - if not axial_solution.success: - raise ValueError("palm axial translation fit failed") - translation = ( - translation - + observed_common * float(axial_solution.x[0]) - ) - return rotation, translation - - # Legacy layouts have only one observed finger chain. Their - # non-parallel reference-finger pitch axis remains the orientation - # anchor; the product profile above deliberately does not use it. - primary = max( - root_names, - key=lambda name: float( - np.ptp(np.asarray(curves[name].angle_rad)) - ), - ) - orientation_anchor = profile.orientation_anchor_joint or ( - "thumb_cmc_pitch" - if profile.base_pose_strategy == "thumb_serial" - else f"{profile.reference_finger}_mcp_pitch" - ) - orientation_items = [ - item for item in selected if item.joint == orientation_anchor - ] - if not orientation_items: - raise ValueError( - f"zero solve is missing palm orientation anchor " - f"{orientation_anchor}" - ) - predicted_orientation_axis = undirected_axis_average( - [ - predicted_local(item, pose_zero_offsets)[0] - for item in orientation_items - ] - ) - observed_orientation_axis = undirected_axis_average( - [item.axis_common_xyz for item in orientation_items] - ) - - def frame_from_two_axes( - primary_axis: np.ndarray, orientation_axis: np.ndarray - ) -> np.ndarray: - first = primary_axis / np.linalg.norm(primary_axis) - second = orientation_axis - first * float(orientation_axis @ first) - second_norm = float(np.linalg.norm(second)) - if second_norm < math.sin(ZERO_MINIMUM_AXIS_CONE_RAD): - raise ValueError("palm orientation axes are nearly parallel") - second /= second_norm - return np.column_stack((first, second, np.cross(first, second))) - - predicted_frame = frame_from_two_axes( - predicted_axes[primary], - predicted_orientation_axis, - ) - candidates: list[tuple[float, Rotation, np.ndarray]] = [] - for primary_sign in (1.0, -1.0): - for orientation_sign in (1.0, -1.0): - observed_frame = frame_from_two_axes( - primary_sign * observed_axes[primary], - orientation_sign * observed_orientation_axis, - ) - rotation = Rotation.from_matrix( - observed_frame @ predicted_frame.T - ) - translation = np.median( - np.asarray( - [ - observed_points[name] - - rotation.apply(predicted_points[name]) - for name in root_names - ] - ), - axis=0, - ) - - # Resolve both undirected-axis sign branches using every - # distinct measured joint. This selects a palm-frame branch; - # it does not fit link geometry or encoder offsets. - errors_by_joint: dict[str, list[float]] = {} - for item in selected: - predicted_axis, _ = predicted_local(item, pose_zero_offsets) - predicted_axis = rotation.apply(predicted_axis) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - alignment = float( - np.clip(predicted_axis @ observed_axis, -1.0, 1.0) - ) - error = math.acos( - alignment - if item.joint in profile.directed_base_axis_joints - else abs(alignment) - ) - errors_by_joint.setdefault(item.joint, []).append(error) - branch_score = sum( - min(float(np.median(values)), math.radians(30.0)) ** 2 - for values in errors_by_joint.values() - ) - candidates.append((branch_score, rotation, translation)) - _, rotation, translation = min(candidates, key=lambda item: item[0]) - return rotation, translation - - def phase_error( - predicted_parent_axis: np.ndarray, - predicted_parent_point: np.ndarray, - predicted_child_point: np.ndarray, - observed_parent_axis: np.ndarray, - observed_parent_point: np.ndarray, - observed_child_point: np.ndarray, - view_normal_common: Sequence[float] | None = None, - ) -> float: - parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis) - observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis) - if float(parent_axis @ observed_axis) < 0.0: - observed_axis = -observed_axis - predicted_delta = predicted_child_point - predicted_parent_point - observed_delta = observed_child_point - observed_parent_point - predicted_radial = predicted_delta - parent_axis * float( - predicted_delta @ parent_axis - ) - observed_radial = observed_delta - observed_axis * float( - observed_delta @ observed_axis - ) - angle_axis = parent_axis - if view_normal_common is not None: - view_normal = np.asarray(view_normal_common, dtype=float) - normal_norm = float(np.linalg.norm(view_normal)) - if normal_norm > 1.0e-9: - view_normal /= normal_norm - # When looking approximately along the rotation axis, image - # x/y contains the complete radial phase while optical depth - # is both unnecessary and much noisier for a 16 mm planar - # Tag. For an edge-on axis the depth component is genuinely - # needed for observability, so retain the full 3-D residual. - if abs(float(parent_axis @ view_normal)) >= math.cos( - math.radians(45.0) - ): - # 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_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: - angle_axis = -angle_axis - predicted_radius = float(np.linalg.norm(predicted_radial)) - observed_radius = float(np.linalg.norm(observed_radial)) - if min(predicted_radius, observed_radius) < 0.003: - raise ValueError("parallel-axis radial phase is not observable") - predicted_radial /= predicted_radius - observed_radial /= observed_radius - return math.atan2( - float(angle_axis @ np.cross(predicted_radial, observed_radial)), - float(np.clip(predicted_radial @ observed_radial, -1.0, 1.0)), - ) - - def robust_circular_location(values: Sequence[float]) -> float: - array = np.asarray(values, dtype=float) - if array.size == 0: - raise ValueError("circular residual set is empty") - centre = math.atan2( - float(np.mean(np.sin(array))), - float(np.mean(np.cos(array))), - ) - centred = np.arctan2(np.sin(array - centre), np.cos(array - centre)) - return float( - math.atan2( - math.sin(centre + float(np.median(centred))), - math.cos(centre + float(np.median(centred))), - ) - ) - - def same_view_axis_pair_errors( - offset_joint: str, - selected: Sequence[JointAxisMeasurement], - offsets: Mapping[str, float], - ) -> tuple[float, ...]: - """Return the camera-frame-invariant residual for a serial-axis pair. - - This observation intentionally has two mirror roots. The optimiser - disambiguates them with the original cross-view estimate, but the - pair residual alone determines the final numerical zero. - """ - pair = profile.same_view_axis_pair_by_offset.get(offset_joint) - if pair is None: - return () - anchor_joint, observer_joint = pair - - def angle(left: np.ndarray, right: np.ndarray) -> float: - cosine = float(left @ right) / float( - np.linalg.norm(left) * np.linalg.norm(right) - ) - return math.acos(abs(float(np.clip(cosine, -1.0, 1.0)))) - - errors: list[float] = [] - for cycle in sorted({int(item.cycle) for item in selected}): - anchor = orientation_by_model_cycle.get((anchor_joint, cycle)) - observer = orientation_by_model_cycle.get( - (observer_joint, cycle) - ) - if anchor is None or observer is None: - return () - angles = _angles_from_state( - observer.condition_state_u8, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - predicted_anchor, _ = model.axis_line( - anchor_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - predicted_observer, _ = model.axis_line( - observer_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - observed_anchor = np.asarray( - anchor.axis_common_xyz, dtype=float - ) - observed_observer = np.asarray( - observer.axis_common_xyz, dtype=float - ) - errors.append( - angle(predicted_anchor, predicted_observer) - - angle(observed_anchor, observed_observer) - ) - return tuple(errors) - - def angular_error_samples( - offsets: Mapping[str, float], - selected: Sequence[JointAxisMeasurement], - base_rotation: Rotation, - base_translation: np.ndarray, - ) -> dict[str, tuple[float, ...]]: - by_key = {(item.joint, item.cycle): item for item in selected} - errors: dict[str, list[float]] = { - name: [] for name in profile.direct_zero_joints - } - for offset_joint, observer_joint in profile.offset_observer_joint.items(): - pair_errors = same_view_axis_pair_errors( - offset_joint, selected, offsets - ) - if pair_errors: - errors[offset_joint].extend(pair_errors) - continue - observer_items = [ - item for item in selected if item.joint == observer_joint - ] - if not observer_items: - raise ValueError(f"zero observer is missing: {observer_joint}") - for item in observer_items: - observer_axis, observer_point = predicted_local(item, offsets) - observer_axis = base_rotation.apply(observer_axis) - observer_point = ( - base_rotation.apply(observer_point) + base_translation - ) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - if observer_joint in profile.axis_parent_joint: - parent_joint = profile.axis_parent_joint[observer_joint] - angles = _angles_from_state( - ( - item.condition_state_u8 - if item.condition_command_u8 is None - else item.condition_command_u8 - ), - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - parent_axis, _ = model.axis_line( - parent_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - parent_axis = base_rotation.apply(parent_axis) - error = _zero_sensitive_axis_error_rad( - observer_axis, observed_axis, parent_axis - ) - else: - parent_joint = profile.phase_parent_joint[observer_joint] - observed_parent = by_key.get((parent_joint, item.cycle)) - if observed_parent is None: - raise ValueError( - f"phase parent is missing: {parent_joint} cycle {item.cycle}" - ) - angles = _angles_from_state( - ( - item.condition_state_u8 - if item.condition_command_u8 is None - else item.condition_command_u8 - ), - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - parent_axis, parent_point = model.axis_line( - parent_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - parent_axis = base_rotation.apply(parent_axis) - parent_point = ( - base_rotation.apply(parent_point) + base_translation - ) - error = phase_error( - parent_axis, - parent_point, - observer_point, - np.asarray(observed_parent.axis_common_xyz, dtype=float), - np.asarray(observed_parent.point_common_xyz_m, dtype=float), - np.asarray(item.point_common_xyz_m, dtype=float), - item.view_normal_common_xyz, - ) - errors[offset_joint].append(float(error)) - return { - name: tuple(values) - for name, values in errors.items() - if values - } - - def angular_errors( - offsets: Mapping[str, float], - selected: Sequence[JointAxisMeasurement], - base_rotation: Rotation, - base_translation: np.ndarray, - ) -> dict[str, float]: - return { - name: robust_circular_location(values) - for name, values in angular_error_samples( - offsets, selected, base_rotation, base_translation - ).items() - } - - product_finger_rolls = tuple( - name - for name in profile.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) - offset_limits = np.asarray( - [ - joint_limits.get( - name, - ( - finger_maximum_offset_rad - if name.startswith(("index_", "middle_", "ring_", "pinky_")) - else maximum_offset_rad - ), - ) - for name in profile.direct_zero_joints - ], - dtype=float, - ) - # Do not make a configured safety limit the numerical optimizer's bound. - # Otherwise a genuine out-of-range estimate and a modelling failure both - # collapse to exactly +/-20 or +/-3 degrees, which hides the magnitude and - # encourages pointless rescans. Search farther for diagnostics, then keep - # the original configured limits as unchanged pass/fail gates below. - diagnostic_offset_limits = np.minimum( - math.radians(90.0), - np.maximum(3.0 * offset_limits, offset_limits + math.radians(5.0)), - ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: - # The absolute rotation of the fitted palm frame about the four - # parallel roll axes is a shared gauge. Each physical finger zero is - # only its deviation from the four-finger median, but the raw scalar - # solves include that common mode. Give those four diagnostic solves - # enough range for the unchanged global common-mode limit plus the - # unchanged per-finger deviation limit; applying the per-finger bound - # before removing the gauge clips every solve to the same value and - # destroys the observable deviations. - for index, name in enumerate(profile.direct_zero_joints): - if name in product_finger_rolls: - diagnostic_offset_limits[index] = min( - math.radians(90.0), - maximum_offset_rad + offset_limits[index], - ) - - def optimise_offsets( - selected: Sequence[JointAxisMeasurement], - base_rotation: Rotation, - base_translation: np.ndarray, - initial: Mapping[str, float] | None = None, - ) -> dict[str, float]: - # The observer graph is a pair of serial chains. Solve it in that - # dependency order so a bad distal phase cannot pull an already - # observable proximal zero toward a bound. Each scalar residual keeps - # the individual cycle values; robust loss then downweights one noisy - # cycle instead of hiding it in a seven-variable compromise. - result = {name: 0.0 for name in profile.direct_zero_joints} - if initial is not None: - result.update( - { - name: float(initial[name]) - for name in profile.direct_zero_joints - } - ) - result.update(fixed_offsets) - for name, diagnostic_limit, configured_limit in zip( - profile.direct_zero_joints, - diagnostic_offset_limits, - offset_limits, - ): - if name in fixed_offsets: - continue - has_axis_pair = name in profile.same_view_axis_pair_by_offset - limit = float( - configured_limit if has_axis_pair else diagnostic_limit - ) - - def residual(value: np.ndarray) -> np.ndarray: - candidate = dict(result) - candidate[name] = float(value[0]) - samples = angular_error_samples( - candidate, selected, base_rotation, base_translation - )[name] - return np.asarray( - [ - math.atan2(math.sin(item), math.cos(item)) - / math.radians(1.0) - for item in samples - ], - dtype=float, - ) - - starts = [0.0, -0.5 * limit, 0.5 * limit] - if initial is not None: - starts.append(float(initial[name])) - solutions = [ - least_squares( - residual, - np.asarray( - [np.clip(start, -limit + 1.0e-9, limit - 1.0e-9)] - ), - bounds=(np.asarray([-limit]), np.asarray([limit])), - loss="soft_l1", - f_scale=1.0, - max_nfev=1000, - ) - for start in starts - ] - solution = min( - solutions, key=lambda item: float(np.sum(np.square(item.fun))) - ) - if not solution.success: - raise ValueError( - f"URDF zero optimization failed for {name}: " - f"{solution.message}" - ) - result[name] = float(solution.x[0]) - return result - - def solve_selected( - selected: Sequence[JointAxisMeasurement], - initial: Mapping[str, float] | None = None, - ) -> tuple[Rotation, np.ndarray, dict[str, float]]: - rotation, translation = fit_base_pose(selected) - offsets = optimise_offsets( - selected, rotation, translation, initial=initial - ) - if profile.base_pose_strategy == "thumb_serial": - # The first pose estimate only supplies a branch for the invariant - # same-view yaw solve. Refit after yaw is known so that the - # downstream pitch phase is evaluated in the corrected serial - # thumb frame, then freeze that pose for holdout validation. - rotation, translation = fit_base_pose(selected, offsets) - offsets = optimise_offsets( - selected, rotation, translation, initial=offsets - ) - return rotation, translation, offsets - - base_rotation, base_translation, training_offsets = solve_selected(training) - - def observability_residual(parameters: np.ndarray) -> np.ndarray: - rotation = Rotation.from_rotvec(parameters[:3]) - translation = parameters[3:6] * 0.05 - offsets = { - name: float(value) - for name, value in zip( - profile.direct_zero_joints, parameters[6:] - ) - } - residuals: list[float] = [] - for item in training: - predicted_axis, predicted_point = predicted_local(item, offsets) - predicted_axis = rotation.apply(predicted_axis) - predicted_axis /= np.linalg.norm(predicted_axis) - predicted_point = rotation.apply(predicted_point) + translation - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - observed_axis /= np.linalg.norm(observed_axis) - if float(predicted_axis @ observed_axis) < 0.0: - observed_axis = -observed_axis - observed_point = np.asarray(item.point_common_xyz_m, dtype=float) - predicted_moment = np.cross(predicted_point, predicted_axis) - observed_moment = np.cross(observed_point, observed_axis) - residuals.extend(float(value) for value in predicted_axis - observed_axis) - residuals.extend( - float(value) / 0.05 - for value in predicted_moment - observed_moment - ) - return np.asarray(residuals, dtype=float) - - observability_parameters = np.asarray( - [ - *base_rotation.as_rotvec(), - *(base_translation / 0.05), - *( - training_offsets[name] - for name in profile.direct_zero_joints - ), - ], - dtype=float, - ) - observability_base_residual = observability_residual( - observability_parameters - ) - observability_jacobian = np.empty( - ( - observability_base_residual.size, - observability_parameters.size, - ), - dtype=float, - ) - finite_difference_step = 1.0e-6 - for column in range(observability_parameters.size): - positive = observability_parameters.copy() - negative = observability_parameters.copy() - positive[column] += finite_difference_step - negative[column] -= finite_difference_step - observability_jacobian[:, column] = ( - observability_residual(positive) - - observability_residual(negative) - ) / (2.0 * finite_difference_step) - singular_values = np.linalg.svd( - observability_jacobian, compute_uv=False - ) - singular_threshold = ( - 0.0 - if singular_values.size == 0 - else float(singular_values[0]) * 1.0e-7 - ) - observability_rank = int( - np.count_nonzero(singular_values > singular_threshold) - ) - observability_parameter_count = int(observability_parameters.size) - observability_condition_number = ( - float("inf") - if observability_rank < observability_parameter_count - else float(singular_values[0] / singular_values[-1]) - ) - residual_dof = max( - 1, - observability_base_residual.size - observability_parameter_count, - ) - residual_variance = float( - observability_base_residual @ observability_base_residual - ) / residual_dof - covariance = residual_variance * np.linalg.pinv( - observability_jacobian.T @ observability_jacobian, - rcond=1.0e-12, - ) - offset_covariance = { - name: max(0.0, float(covariance[6 + index, 6 + index])) - for index, name in enumerate(profile.direct_zero_joints) - } - - axis_cone_mismatch_by_joint: dict[str, float] = {} - axis_cone_bias_classification_by_joint: dict[str, str] = {} - - def zero_observation_failure_reasons( - selected: Sequence[JointAxisMeasurement], - offsets: Mapping[str, float], - rotation: Rotation, - ) -> dict[str, str]: - """Reject repeatable but geometrically inadmissible zero observers.""" - by_key = {(item.joint, item.cycle): item for item in selected} - failures: dict[str, str] = {} - for offset_joint, observer_joint in profile.offset_observer_joint.items(): - if offset_joint in fixed_offsets: - continue - observer_items = [ - item for item in selected if item.joint == observer_joint - ] - if observer_joint in profile.axis_parent_joint: - parent_joint = profile.axis_parent_joint[observer_joint] - cone_mismatches: list[float] = [] - for item in observer_items: - state = ( - item.condition_state_u8 - if item.condition_command_u8 is None - else item.condition_command_u8 - ) - angles = _angles_from_state( - state, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=( - profile.inherited_zero_joints - ), - ) - predicted_axis, _ = model.axis_line( - observer_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - parent_axis, _ = model.axis_line( - parent_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - cone_mismatches.append( - _axis_cone_mismatch_rad( - rotation.apply(predicted_axis), - item.axis_common_xyz, - rotation.apply(parent_axis), - ) - ) - maximum_cone_mismatch = max(cone_mismatches) - axis_cone_mismatch_by_joint[offset_joint] = ( - maximum_cone_mismatch - ) - if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: - cone_range = float(np.ptp(cone_mismatches)) - stable_product_bias = bool( - profile.hand.stable_cross_view_cone_bias - and maximum_systematic_axis_cone_bias_rad is not None - and len(cone_mismatches) >= 4 - and maximum_cone_mismatch - <= maximum_systematic_axis_cone_bias_rad - and cone_range <= maximum_cycle_difference_rad - ) - if stable_product_bias: - # A parent zero rotates the downstream direction - # around the parent axis and cannot change their cone - # angle. The zero-sensitive residual above already - # projects both directions onto the parent-normal - # plane, so a repeatable cross-camera/planar-PnP cone - # bias cannot corrupt the written encoder zero. Keep - # it visible in the result while retaining the gross - # gate for a wrong axis, loose Tag, or moved camera. - axis_cone_bias_classification_by_joint[offset_joint] = ( - "stable_cross_view_or_planar_pnp_bias" - ) - else: - failures[offset_joint] = ( - "zero_axis_cone_mismatch_too_large" - ) - else: - parent_joint = profile.phase_parent_joint[observer_joint] - phase_items: list[JointAxisMeasurement] = [] - propagated_angle_uncertainties: list[float] = [] - for item in observer_items: - phase_items.append(item) - parent_item = by_key.get((parent_joint, item.cycle)) - if parent_item is not None: - phase_items.append(parent_item) - parent_axis = np.asarray( - parent_item.axis_common_xyz, dtype=float - ) - parent_axis /= np.linalg.norm(parent_axis) - separation = ( - np.asarray(item.point_common_xyz_m, dtype=float) - - np.asarray( - parent_item.point_common_xyz_m, dtype=float - ) - ) - radial = separation - parent_axis * float( - separation @ parent_axis - ) - effective_distance = float(np.linalg.norm(radial)) - if effective_distance <= 1.0e-6: - propagated_angle_uncertainties.append(float("inf")) - else: - line_uncertainty = math.hypot( - item.pose_axis_line_rms_m, - parent_item.pose_axis_line_rms_m, - ) - propagated_angle_uncertainties.append( - math.atan2( - line_uncertainty, effective_distance - ) - ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: - angle_limit = ( - maximum_confidence_half_width_rad - if maximum_confidence_half_width_rad is not None - else math.radians(0.5) - ) - if ( - not propagated_angle_uncertainties - or max(propagated_angle_uncertainties) > angle_limit - ): - failures[offset_joint] = ( - "zero_phase_axis_line_angle_uncertainty_too_large" - ) - elif ( - profile.hand.layout_id != G20_RIGHT_19_LAYOUT - and any( - item.pose_axis_line_rms_m - > maximum_pose_axis_line_rms_m - for item in phase_items - ) - ): - failures[offset_joint] = ( - "zero_phase_axis_line_residual_too_large" - ) - return failures - - observation_failures = zero_observation_failure_reasons( - list(measurements), training_offsets, base_rotation - ) - cycle_values: dict[str, list[float]] = { - name: [] for name in profile.direct_zero_joints - } - cycle_models: dict[int, tuple[Rotation, np.ndarray]] = {} - training_cycle_ids = tuple(sorted({int(value) for value in training_cycles})) - for cycle in training_cycle_ids: - selected = [item for item in measurements if item.cycle == cycle] - # Keep one palm pose while comparing cycles. Refitting a base pose from - # only two nearly parallel root axes per cycle makes harmless root-line - # noise appear as a large encoder-zero change. - cycle_rotation, cycle_translation = base_rotation, base_translation - cycle_offsets = optimise_offsets( - selected, - cycle_rotation, - cycle_translation, - initial=training_offsets, - ) - cycle_models[cycle] = (cycle_rotation, cycle_translation) - for name, value in cycle_offsets.items(): - cycle_values[name].append(value) - uncertainties: dict[str, float] = {} - confidence_half_widths: dict[str, float] = {} - cycle_consistent = True - inconsistent_cycles: list[str] = [] - for name, values in cycle_values.items(): - array = np.asarray(values, dtype=float) - spread = float(np.max(array) - np.min(array)) - if spread > maximum_cycle_difference_rad: - cycle_consistent = False - inconsistent_cycles.append(name) - uncertainties[name] = ( - 0.0 - if array.size < 2 - else float(np.std(array, ddof=1) / math.sqrt(array.size)) - ) - confidence_half_widths[name] = ( - float("inf") - if array.size < 2 - else float( - student_t.ppf(0.975, df=array.size - 1) - * uncertainties[name] - ) - ) - - insignificant_large: list[str] = [] - applied_training: dict[str, float] = {} - for name, value in training_offsets.items(): - if name in fixed_offsets: - applied_training[name] = fixed_offsets[name] - continue - uncertainty = uncertainties[name] - confidence_half_width = confidence_half_widths[name] - if ( - abs(value) < minimum_applied_offset_rad - or abs(value) - <= max(significance_sigma * uncertainty, confidence_half_width) - ): - applied_training[name] = 0.0 - 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 - - candidate_errors = angular_errors( - applied_training, validation, base_rotation, base_translation - ) - original_errors = angular_errors( - zero_offsets, validation, base_rotation, base_translation - ) - validation_error_by_joint: dict[str, float] = {} - original_error_by_joint: dict[str, float] = {} - improvement_by_joint: dict[str, float] = {} - improvement_confidence_lower: dict[str, float] = {} - improvement_passed = True - if profile.same_view_axis_pair_by_offset: - # Validate the same camera/Tag-mount-invariant axis-pair angle used by - # the configured offset. This keeps the side channel out of every - # other joint, including the already stable thumb CMC roll solve. - palm_orientation_validation_errors = tuple( - abs(float(error)) - for offset_joint in profile.same_view_axis_pair_by_offset - for error in same_view_axis_pair_errors( - offset_joint, validation, applied_training - ) - ) - else: - palm_orientation_validation_errors = tuple( - math.acos( - abs( - float( - np.clip( - base_rotation.apply( - predicted_palm_orientation_local( - item, zero_offsets - ) - ) - @ np.asarray(item.axis_common_xyz, dtype=float), - -1.0, - 1.0, - ) - ) - ) - ) - for item in orientation_validation - ) - palm_orientation_validation_limit = ( - maximum_validation_error_rad - if maximum_validation_error_rad is not None - else maximum_axis_cone_mismatch_rad - ) - palm_orientation_validation_passed = bool( - not configured_palm_sources - or ( - palm_orientation_validation_errors - and float(np.median(palm_orientation_validation_errors)) - <= palm_orientation_validation_limit - ) - ) - for offset_joint, observer_joint in profile.offset_observer_joint.items(): - if offset_joint in fixed_offsets: - improvement_by_joint[offset_joint] = 0.0 - improvement_confidence_lower[offset_joint] = 0.0 - continue - candidate = abs(candidate_errors[offset_joint]) - original = abs(original_errors[offset_joint]) - validation_error_by_joint[observer_joint] = candidate - original_error_by_joint[observer_joint] = original - improvement_by_joint[offset_joint] = original - candidate - if applied_training[offset_joint] != 0.0 and not candidate < original: - improvement_passed = False - for offset_joint in profile.direct_zero_joints: - if offset_joint in fixed_offsets: - improvement_confidence_lower[offset_joint] = 0.0 - continue - if applied_training[offset_joint] == 0.0: - improvement_confidence_lower[offset_joint] = 0.0 - continue - cycle_improvements: list[float] = [] - for cycle in training_cycle_ids: - selected = [item for item in measurements if item.cycle == cycle] - cycle_rotation, cycle_translation = cycle_models[cycle] - candidate = abs( - angular_errors( - applied_training, - selected, - cycle_rotation, - cycle_translation, - )[offset_joint] - ) - original = abs( - angular_errors( - zero_offsets, - selected, - cycle_rotation, - cycle_translation, - )[offset_joint] - ) - cycle_improvements.append(original - candidate) - improvement_array = np.asarray(cycle_improvements, dtype=float) - improvement_se = ( - float("inf") - if improvement_array.size < 2 - else float( - np.std(improvement_array, ddof=1) - / math.sqrt(improvement_array.size) - ) - ) - lower = ( - float("-inf") - if not math.isfinite(improvement_se) - else float( - np.mean(improvement_array) - - student_t.ppf( - 0.975, df=improvement_array.size - 1 - ) - * improvement_se - ) - ) - improvement_confidence_lower[offset_joint] = lower - if lower <= 0.0: - improvement_passed = False - - validation_errors = np.asarray( - list(validation_error_by_joint.values()), dtype=float - ) - validation_line_samples: dict[str, list[float]] = {} - for item in validation: - predicted_axis, predicted_point = predicted_local( - item, applied_training - ) - predicted_axis = base_rotation.apply(predicted_axis) - predicted_axis /= np.linalg.norm(predicted_axis) - predicted_point = ( - base_rotation.apply(predicted_point) + base_translation - ) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - observed_axis /= np.linalg.norm(observed_axis) - observed_point = np.asarray(item.point_common_xyz_m, dtype=float) - cross = np.cross(predicted_axis, observed_axis) - cross_norm = float(np.linalg.norm(cross)) - separation = observed_point - predicted_point - line_error = ( - abs(float(separation @ cross)) / cross_norm - if cross_norm > 1.0e-6 - else float( - np.linalg.norm( - separation - - predicted_axis * float(separation @ predicted_axis) - ) - ) - ) - validation_line_samples.setdefault(item.joint, []).append(line_error) - validation_line_error_by_joint = { - name: float(np.sqrt(np.mean(np.square(values)))) - for name, values in validation_line_samples.items() - } - all_validation_line_errors = np.asarray( - [ - value - for values in validation_line_samples.values() - for value in values - ], - dtype=float, - ) - axis_line_rms = ( - float("inf") - if all_validation_line_errors.size == 0 - else float( - np.sqrt(np.mean(np.square(all_validation_line_errors))) - ) - ) - finger_roll_common_mode = ( - float( - np.median( - [training_offsets[name] for name in product_finger_rolls] - ) - ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT - and product_finger_rolls - else 0.0 - ) - configured_limit_exceeded: list[str] = [] - for name, limit in zip(profile.direct_zero_joints, offset_limits): - if name in fixed_offsets: - continue - # A post-solve mechanical endpoint datum is the value that will be - # published for this joint. The visual root-axis scalar remains a - # nuisance gauge used for holdout geometry and must not be compared - # with the safety bound of a different, endpoint-anchored output. - checked_offset = output_offsets.get(name, training_offsets[name]) - if name in product_finger_rolls: - # The four roll motors share the same electrical centre and the - # absolute palm axial datum is recovered from the root-line - # pattern. Protect the independently assembled finger-to-finger - # deviations with the strict finger bound; protect their shared - # common mode with the unchanged global zero bound. Treating the - # same common datum as four independent failures is both - # over-counting and sensitive to the palm-frame gauge. - checked_offset -= finger_roll_common_mode - if abs(checked_offset) > limit + math.radians(0.01): - configured_limit_exceeded.append(name) - if ( - product_finger_rolls - and abs(finger_roll_common_mode) - > maximum_offset_rad + math.radians(0.01) - ): - configured_limit_exceeded.append("finger_mcp_roll_common_mode") - diagnostic_bound_hits: list[str] = [] - for name, limit in zip( - profile.direct_zero_joints, diagnostic_offset_limits - ): - if name in fixed_offsets: - continue - checked_offset = output_offsets.get(name, training_offsets[name]) - if name in product_finger_rolls: - # Match the configured-limit and publication convention above. - # The raw common roll is a fitted-palm-frame gauge; only the - # finger-to-finger deviation is a physical zero correction. - checked_offset -= finger_roll_common_mode - if abs(checked_offset) >= limit - math.radians(0.01): - diagnostic_bound_hits.append(name) - failure_reasons: dict[str, str] = {} - if not palm_orientation_validation_passed: - failure_reasons["palm_orientation"] = ( - "palm_orientation_holdout_too_large" - ) - requires_full_observability = bool( - profile.hand.layout_id == G20_RIGHT_19_LAYOUT - and profile.base_pose_strategy == "full_hand" - ) - if ( - requires_full_observability - and observability_rank < observability_parameter_count - ): - failure_reasons["palm_and_static_zero"] = ( - "zero_observation_jacobian_rank_deficient" - ) - elif ( - requires_full_observability - and observability_condition_number - > maximum_observability_condition_number - ): - failure_reasons["palm_and_static_zero"] = ( - "zero_observation_jacobian_ill_conditioned" - ) - for name in configured_limit_exceeded: - failure_reasons[name] = "zero_offset_exceeds_configured_limit" - for name in diagnostic_bound_hits: - failure_reasons[name] = "zero_offset_reached_diagnostic_bound" - for name in inconsistent_cycles: - failure_reasons[name] = "zero_offset_cycle_difference_too_large" - for name in insignificant_large: - failure_reasons[name] = "zero_offset_not_statistically_significant" - if maximum_confidence_half_width_rad is not None: - for name, half_width in confidence_half_widths.items(): - if half_width > maximum_confidence_half_width_rad: - failure_reasons[name] = ( - "zero_offset_confidence_interval_too_wide" - ) - # This solver publishes rotational encoder zeros only. A post-fit CAD to - # measured axis-line displacement is invariant to the joint's own zero - # and cannot be repaired by changing that rotational parameter. Keep the - # per-joint and aggregate values in ZeroSolveResult for geometry audit, - # but do not misclassify a fixed link-origin/Tag-depth discrepancy as a - # failed rotational holdout. Axis-point *fit* quality is still guarded - # above for every phase observation that actually uses line position. - if not improvement_passed: - for name, value in applied_training.items(): - if ( - name not in fixed_offsets - and value != 0.0 - and improvement_confidence_lower.get(name, 0.0) <= 0.0 - ): - failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" - # Geometry failures are the root cause and must not be hidden by the - # downstream validation symptom produced by the same bad observation. - failure_reasons.update(observation_failures) - passed = bool( - validation_errors.size - == len(profile.direct_zero_joints) - len(fixed_offsets) - and float(np.mean(validation_errors)) <= maximum_validation_mae_rad - and float(np.percentile(validation_errors, 95.0)) - <= maximum_validation_p95_rad - and ( - maximum_validation_error_rad is None - or float(np.max(validation_errors)) - <= maximum_validation_error_rad - ) - and cycle_consistent - and not insignificant_large - and not configured_limit_exceeded - and not diagnostic_bound_hits - and not observation_failures - and ( - not requires_full_observability - or ( - observability_rank == observability_parameter_count - and observability_condition_number - <= maximum_observability_condition_number - ) - ) - and not any( - reason == "zero_offset_confidence_interval_too_wide" - for reason in failure_reasons.values() - ) - and improvement_passed - and palm_orientation_validation_passed - ) - - # Never refit a model that has passed its holdout with the validation - # cycle. The published offsets are exactly the frozen training result - # that produced ``validation_errors`` above. - final_offsets = dict(applied_training) - # Apply independently validated assembly datums only after trajectory - # fitting and holdout validation. A mechanical prior must define the - # written artifact without perturbing downstream yaw/pitch estimates. - final_offsets.update(output_offsets) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT and product_finger_rolls: - # The camera solve observes the four roll axes in a fitted palm frame. - # Rotation of that frame about their shared datum is a gauge, not four - # independent finger assembly errors. The product command 127/CAD - # pose defines the common straight-ahead datum; publish only each - # finger's robust deviation from the four-finger median. Validation - # above remains in the observation gauge, so no measured residual is - # discarded. - for name in product_finger_rolls: - final_offsets[name] = ( - float(final_offsets[name]) - finger_roll_common_mode - ) - - # Every active joint must be present in the runtime payload/URDF writer, - # but absence of an absolute observation is not evidence for the - # reference finger's assembly offset. Preserve the source-CAD zero for - # those independent motors while continuing to share their dynamic curve. - all_offsets = { - name: 0.0 for name in profile.hand.active_joints - } - all_offsets.update(final_offsets) - for target, source in profile.inherited_static_zero_joints.items(): - all_offsets[target] = final_offsets[source] - return ZeroSolveResult( - direct_offsets_rad=final_offsets, - all_active_offsets_rad=all_offsets, - base_translation_xyz_m=tuple(float(value) for value in base_translation), - base_quaternion_xyzw=tuple(float(value) for value in base_rotation.as_quat()), - validation_errors_rad=tuple(float(value) for value in validation_errors), - validation_error_by_joint_rad=validation_error_by_joint, - validation_line_error_by_joint_m=validation_line_error_by_joint, - axis_line_rms_m=axis_line_rms, - passed=passed, - cycle_offsets_rad={ - name: tuple(float(value) for value in values) - for name, values in cycle_values.items() - }, - offset_uncertainty_rad=uncertainties, - offset_confidence_half_width_rad=confidence_half_widths, - training_cycles=training_cycle_ids, - validation_cycle=int(validation_cycle), - validation_original_error_by_joint_rad=original_error_by_joint, - validation_improvement_by_joint_rad=improvement_by_joint, - validation_improvement_confidence_lower_rad=( - improvement_confidence_lower - ), - observability_rank=observability_rank, - observability_parameter_count=observability_parameter_count, - observability_condition_number=observability_condition_number, - offset_covariance_rad2=offset_covariance, - axis_cone_mismatch_by_joint_rad=axis_cone_mismatch_by_joint, - axis_cone_bias_classification_by_joint=( - axis_cone_bias_classification_by_joint - ), - failure_reasons=failure_reasons, - ) - - -def write_zero_corrected_urdf( - *, - source_urdf: str | Path, - output_directory: str | Path, - serial_number: str, - offsets_rad: Mapping[str, float], - endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, - timestamp: str | None = None, - correction_plan: UrdfCorrectionPlan | None = None, -) -> Path: - source = Path(source_urdf).expanduser().resolve() - output = Path(output_directory).expanduser().resolve() - if not source.is_file(): - raise ValueError(f"source URDF does not exist: {source}") - if not offsets_rad: - raise ValueError("offsets_rad must contain at least one joint") - offsets = {str(name): float(value) for name, value in offsets_rad.items()} - endpoint_offsets = { - str(name): float(value) - for name, value in dict(endpoint_anchored_offsets_rad or {}).items() - } - if not set(endpoint_offsets) <= set(offsets): - raise ValueError("endpoint-anchored offsets must be URDF zero targets") - if correction_plan is not None: - correction_plan.verify_source(source) - correction_plan.authorize_offsets(offsets) - if not set(endpoint_offsets).issubset( - correction_plan.endpoint_limit_joints - ): - raise ValueError( - "endpoint offsets are not authorized by the correction plan" - ) - if any( - not math.isfinite(value) or abs(value) > math.radians(90.0) - for value in offsets.values() - ): - raise ValueError("URDF zero offsets must be finite and within +/-90deg") - 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("URDF zero timestamp must use YYYYMMDD_HHMMSS") - safe_serial = "".join( - character if character.isalnum() or character in "_.-" else "_" - for character in str(serial_number) - ) - if not safe_serial: - raise ValueError("serial_number must not be empty") - tree = ET.parse(source) - root = tree.getroot() - replacement_rpy: dict[str, str] = {} - replacement_upper: dict[str, str] = {} - replacement_mimic_offset: dict[str, str] = {} - found: set[str] = set() - for joint in root.findall("joint"): - name = str(joint.get("name")) - if name not in offsets: - mimic = joint.find("mimic") - source_joint = None if mimic is None else str(mimic.get("joint")) - if source_joint in endpoint_offsets: - multiplier = float(mimic.get("multiplier", "1")) - old_offset = float(mimic.get("offset", "0")) - calibrated_mimic_offset = ( - old_offset + multiplier * endpoint_offsets[source_joint] - ) - replacement_mimic_offset[name] = ( - f"{calibrated_mimic_offset:.15g}" - ) - continue - found.add(name) - if name in endpoint_offsets: - limit = joint.find("limit") - if limit is None or limit.get("upper") is None: - raise ValueError(f"joint {name} has no upper limit") - replacement_upper[name] = f"{float(limit.get('upper')) - endpoint_offsets[name]:.15g}" - # A statistically insignificant correction is represented as exact - # zero. Preserve that joint's source text byte-for-byte instead of - # serialising an equivalent Euler triplet. - if abs(offsets[name]) <= 1.0e-15: - continue - if joint.get("type") not in {"revolute", "continuous"}: - raise ValueError(f"joint {name} is not revolute") - axis_node = joint.find("axis") - axis = _parse_triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - axis_norm = float(np.linalg.norm(axis)) - if axis_norm <= 1.0e-12: - raise ValueError(f"joint {name} has a degenerate axis") - axis /= axis_norm - origin_node = joint.find("origin") - if origin_node is None: - raise ValueError( - f"joint {name} has no origin; refusing a non-minimal rewrite" - ) - rpy = _parse_triplet(origin_node.get("rpy", "0 0 0")) - original = Rotation.from_euler("xyz", rpy).as_matrix() - corrected = original @ Rotation.from_rotvec( - axis * offsets[name] - ).as_matrix() - corrected_rpy = Rotation.from_matrix(corrected).as_euler("xyz") - replacement_rpy[name] = " ".join( - f"{float(value):.15g}" for value in corrected_rpy - ) - missing = sorted(set(offsets) - found) - if missing: - raise ValueError("source URDF is missing target joints: " + ",".join(missing)) - patch_names = ( - set(replacement_rpy) - | set(replacement_upper) - | set(replacement_mimic_offset) - ) - joint_patches = { - name: UrdfJointPatch( - origin_rpy=replacement_rpy.get(name), - limit_upper=replacement_upper.get(name), - mimic_offset=replacement_mimic_offset.get(name), - ) - for name in sorted(patch_names) - } - destination = output / ( - f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" - ) - return write_urdf_patches( - source_urdf=source, - destination_urdf=destination, - patches=UrdfPatchSet(joints=joint_patches), - forbidden_source_stem_patterns=( - r"zero_calibrated", - r"calibrated_20\d{6}", - ), - ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py deleted file mode 100644 index 377e41b..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py +++ /dev/null @@ -1,14 +0,0 @@ -"""Registered L6 calibration profiles.""" - -from ..registry import ProfileRegistry - - -def register_profiles(registry: ProfileRegistry) -> None: - from .left_transfer import build_profile as build_left_transfer_profile - from .profile import build_profile - - registry.register(build_profile()) - registry.register(build_left_transfer_profile()) - - -__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py deleted file mode 100644 index b3d54c4..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/fitting.py +++ /dev/null @@ -1,741 +0,0 @@ -"""L6 curve, endpoint-zero, holdout, and mimic fitting.""" - -from __future__ import annotations - -from dataclasses import dataclass -import math -from pathlib import Path -from typing import Mapping, Sequence -import xml.etree.ElementTree as ET - -import numpy as np -from scipy.optimize import least_squares - -from ..g20.profile import ( - HandCalibrationProfile, - JointCurveFit, - JointSpec, -) -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 .profile import ( - CALIBRATED_ACTIVE_JOINTS, - COMMAND_INDEX_BY_JOINT, - COMMAND_NAMES, - COUPLING_MODEL_BY_JOINT, - ENDPOINT_ANCHOR_BY_JOINT, - KEY, - MEASURED_PASSIVE_JOINTS, - MIMIC_SOURCE_BY_JOINT, -) - - -@dataclass(frozen=True) -class MimicFit: - source_joint: str - target_joint: str - model: str - coefficients: tuple[float, ...] - urdf_mimic_multiplier: float - urdf_mimic_policy: str - cycle_coefficients: tuple[tuple[float, ...], ...] - maximum_cycle_range: float - maximum_cycle_prediction_range_rad: float - residual_rms_rad: float - residual_p95_rad: float - residual_max_rad: float - - @property - def multiplier(self) -> float: - """Linear term retained for compatible diagnostics and artifacts.""" - return float(self.coefficients[0]) - - @property - def cycle_multipliers(self) -> tuple[float, ...]: - return tuple(float(values[0]) for values in self.cycle_coefficients) - - @property - def mujoco_polycoef(self) -> tuple[float, ...]: - """MuJoCo q_target = p0 + p1*q_source + ... coefficients.""" - return (0.0, *self.coefficients, *(0.0,) * (5 - len(self.coefficients))) - - -@dataclass(frozen=True) -class L6FitResult: - curves: Mapping[str, JointCurveFit] - zero_offsets_rad: Mapping[str, float] - travels_rad: Mapping[str, float] - mimic_fits: Mapping[str, MimicFit] - holdout_errors_rad: Mapping[str, tuple[float, ...]] - zero_method_by_joint: Mapping[str, str] - zero_fallback_reason_by_joint: Mapping[str, str] - thumb_zero_result: ZeroSolveResult | None = None - - -L6_THUMB_AXIS_JOINTS: tuple[str, ...] = ( - "rh_thumb_cmc_roll", - "rh_thumb_cmc_pitch", - "rh_thumb_dip", - "rh_pinky_mcp_pitch", -) - - -def _l6_thumb_zero_profile() -> ZeroCalibrationProfile: - """Describe the observable L6 thumb/palm axis graph to the G20 solver.""" - joint_specs = { - "rh_thumb_cmc_roll": JointSpec( - "rh_thumb_cmc_roll", 1, True, "top", "top_base", "thumb_roll" - ), - "rh_thumb_cmc_pitch": JointSpec( - "rh_thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_pitch" - ), - "rh_thumb_dip": JointSpec( - "rh_thumb_dip", 0, False, "front", "thumb_pitch", "thumb_dip" - ), - # This root axis fixes the palm-frame phase around the thumb roll axis. - # Its electrical zero is irrelevant because rotating a revolute joint - # does not change its own physical screw axis. - "rh_pinky_mcp_pitch": JointSpec( - "rh_pinky_mcp_pitch", 5, True, "side", "side_base", "pinky_pitch" - ), - } - hand = HandCalibrationProfile( - side="right", - reference_finger="pinky", - view_tags={}, - preflight_view_roles={}, - joint_specs=joint_specs, - sweep_specs=(), - image_trajectory_joints=frozenset(), - roll_clearance_commands={}, - thumb_pitch_clearance_commands={}, - layout_id=KEY.layout, - model="L6", - command_names=COMMAND_NAMES, - baseline_command=(255,) * 6, - directional_zero=True, - isolated_holdout=True, - ) - return ZeroCalibrationProfile( - hand=hand, - direct_zero_joints=( - "rh_thumb_cmc_roll", - "rh_thumb_cmc_pitch", - ), - axis_joints=L6_THUMB_AXIS_JOINTS, - inherited_zero_joints={}, - inherited_static_zero_joints={}, - constrained_circle_joints=frozenset(L6_THUMB_AXIS_JOINTS), - root_anchor_joints=frozenset({"rh_thumb_cmc_roll"}), - axis_parent_joint={ - "rh_thumb_cmc_pitch": "rh_thumb_cmc_roll", - }, - phase_parent_joint={ - "rh_thumb_dip": "rh_thumb_cmc_pitch", - }, - offset_observer_joint={ - "rh_thumb_cmc_roll": "rh_thumb_cmc_pitch", - "rh_thumb_cmc_pitch": "rh_thumb_dip", - }, - same_view_axis_pair_by_offset={}, - fixed_direct_zero_offsets_rad={}, - static_output_zero_offsets_rad={}, - base_pose_strategy="thumb_serial", - orientation_anchor_joint="rh_pinky_mcp_pitch", - ) - - -def curve_travel_rad(fit: JointCurveFit) -> float: - decreasing = float(fit.decreasing_rad[0] - fit.decreasing_rad[255]) - increasing = float(fit.increasing_rad[0] - fit.increasing_rad[255]) - travel = 0.5 * (decreasing + increasing) - if not math.isfinite(travel) or travel <= 0.0: - raise ValueError("L6 fitted travel must be finite and positive") - return travel - - -def derive_endpoint_zero_offsets( - source_urdf: str | Path, - curves: Mapping[str, JointCurveFit], - *, - endpoint_anchor_by_joint: Mapping[str, str] | None = None, - maximum_offset_rad: float = math.radians(15.0), -) -> tuple[dict[str, float], dict[str, float]]: - """Anchor each measured joint to its profile-selected CAD endpoint. - - Every published dynamic curve is zero at feedback 255. ``upper_at_end`` - rotates the static frame by ``source_upper - measured_travel`` so feedback - 0 lands on the CAD upper endpoint. ``lower_at_start`` rotates it by the - source lower value so feedback 255 lands on the CAD lower endpoint. - ``cad_range_center`` splits a source-vs-measured travel discrepancy equally - between the two endpoints when neither source endpoint is a trusted datum. - ``zero_at_start`` keeps the CAD joint frame itself at feedback 255. All - policies publish the normalized corrected coordinate [0, measured travel]. - """ - joints = { - str(joint.get("name")): joint - for joint in ET.parse(Path(source_urdf)).getroot().findall("joint") - } - offsets: dict[str, float] = {} - travels: dict[str, float] = {} - anchors = endpoint_anchor_by_joint or dict(ENDPOINT_ANCHOR_BY_JOINT) - if not anchors or not set(anchors).issubset(CALIBRATED_ACTIVE_JOINTS): - raise ValueError("L6 endpoint anchors must target measured active joints") - for name in sorted(CALIBRATED_ACTIVE_JOINTS): - if name not in curves: - raise ValueError(f"missing measured L6 curve: {name}") - joint = joints.get(name) - limit = None if joint is None else joint.find("limit") - if ( - limit is None - or limit.get("upper") is None - or limit.get("lower") is None - ): - raise ValueError(f"source URDF joint has incomplete limits: {name}") - travel = curve_travel_rad(curves[name]) - travels[name] = travel - if name not in anchors: - continue - policy = str(anchors[name]) - if policy == "upper_at_end": - offset = float(limit.get("upper")) - travel - elif policy == "lower_at_start": - offset = float(limit.get("lower")) - elif policy == "cad_range_center": - offset = 0.5 * ( - float(limit.get("lower")) - + float(limit.get("upper")) - - travel - ) - elif policy == "zero_at_start": - offset = 0.0 - else: - raise ValueError(f"unsupported L6 endpoint anchor: {policy}") - if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: - raise ValueError( - f"{name} endpoint zero offset exceeds 15 degrees: " - f"{math.degrees(offset):.3f}" - ) - offsets[name] = offset - return offsets, travels - - -def _has_complete_axis_geometry( - records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], -) -> bool: - required = { - "relative_translation_xyz_m", - "parent_pose_common", - "child_pose_common", - "view_normal_common_xyz", - "camera_center_common_xyz_m", - "state_u8", - } - return all( - rows and all(required.issubset(row) for row in rows) - for name in L6_THUMB_AXIS_JOINTS - for rows in (records_by_joint.get(name, ()),) - ) - - -def _fit_l6_thumb_axis_zero( - source_urdf: str | Path, - records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], - curves: Mapping[str, JointCurveFit], - *, - fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, - require_passed: bool = True, -) -> ZeroSolveResult: - """Recover thumb roll/pitch zeros from four physical screw axes.""" - profile = _l6_thumb_zero_profile() - measurements = [] - by_key: dict[tuple[str, int], object] = {} - for cycle in range(4): - # Fit the two root/reference axes before the serial passive observer. - for name in ( - "rh_thumb_cmc_roll", - "rh_thumb_cmc_pitch", - "rh_pinky_mcp_pitch", - ): - rows = records_by_joint[name] - view_normal = rows[0]["view_normal_common_xyz"] - measurement = fit_joint_axis_measurement( - name, - rows, - cycle=cycle, - zero_command_u8=255, - constrained_circle_joints=profile.constrained_circle_joints, - view_normal_common_xyz=view_normal, - canonical_zero_direction="decreasing", - ) - measurement = with_depth_free_axis_projection( - measurement, - rows[0]["camera_center_common_xyz_m"], - ) - measurements.append(measurement) - by_key[(name, cycle)] = measurement - - dip_rows = records_by_joint["rh_thumb_dip"] - pitch_axis = by_key[("rh_thumb_cmc_pitch", cycle)] - dip = fit_joint_axis_measurement( - "rh_thumb_dip", - dip_rows, - cycle=cycle, - zero_command_u8=255, - axis_common_constraint=pitch_axis.axis_common_xyz, - constrained_circle_joints=profile.constrained_circle_joints, - view_normal_common_xyz=dip_rows[0]["view_normal_common_xyz"], - canonical_zero_direction="decreasing", - ) - dip = with_depth_free_axis_projection( - dip, - dip_rows[0]["camera_center_common_xyz_m"], - ) - measurements.append(dip) - - result = solve_urdf_zero_offsets( - source_urdf=source_urdf, - measurements=measurements, - curves=curves, - motor_by_joint={ - name: COMMAND_INDEX_BY_JOINT[ - MIMIC_SOURCE_BY_JOINT.get(name, name) - ] - for name in L6_THUMB_AXIS_JOINTS - }, - training_cycles=(0, 1, 2), - validation_cycle=3, - maximum_offset_rad=math.radians(15.0), - finger_maximum_offset_rad=math.radians(15.0), - joint_maximum_offset_rad={ - "rh_thumb_cmc_roll": math.radians(15.0), - "rh_thumb_cmc_pitch": math.radians(15.0), - }, - 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), - maximum_pose_axis_line_rms_m=0.0015, - hand_type="right", - tag_layout=KEY.layout, - fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets_rad, - zero_profile=profile, - ) - if require_passed and not result.passed: - details = ",".join( - f"{name}={reason}" - for name, reason in sorted(result.failure_reasons.items()) - ) - raise ValueError("L6 thumb axis zero solve failed:" + details) - return result - - -def _coupling_regression( - active: Sequence[float], passive: Sequence[float], *, degree: int -) -> tuple[tuple[float, ...], np.ndarray]: - x = np.asarray(active, dtype=float) - y = np.asarray(passive, dtype=float) - if x.shape != y.shape or x.ndim != 1 or x.size < 16: - raise ValueError("mimic curves must be aligned finite vectors") - if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)): - raise ValueError("mimic curves must be finite") - if degree not in {1, 2}: - raise ValueError("L6 coupling degree must be one or two") - if float(x @ x) <= 1.0e-9: - raise ValueError("active mimic source has insufficient travel") - design = np.column_stack([x ** power for power in range(1, degree + 1)]) - initial = np.linalg.lstsq(design, y, rcond=None)[0] - scale = max( - math.radians(0.25), - float(np.median(np.abs(y - design @ initial))), - ) - fitted = least_squares( - lambda value: y - design @ value, - np.asarray(initial, dtype=float), - loss="soft_l1", - f_scale=scale, - ) - if not fitted.success or not np.all(np.isfinite(fitted.x)): - raise ValueError("robust through-origin mimic regression failed") - coefficients = tuple(float(value) for value in fitted.x) - grid = np.linspace(0.0, float(np.max(x)), 256) - derivative = np.full_like(grid, coefficients[0]) - if degree == 2: - derivative += 2.0 * coefficients[1] * grid - if float(np.min(derivative)) < -1.0e-7: - raise ValueError("L6 coupling model is not monotonic") - return coefficients, y - design @ fitted.x - - -def fit_coupling_model( - source_joint: str, - target_joint: str, - active_fit: JointCurveFit, - passive_fit: JointCurveFit, - *, - model: str, - cycle_curve_pairs: Sequence[ - tuple[Sequence[float], Sequence[float]] - ] = (), - minimum_multiplier: float = 0.5, - maximum_multiplier: float = 1.5, - maximum_cycle_range: float = 0.03, - maximum_cycle_prediction_range_rad: float = math.radians(1.0), - 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", "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( - ( - np.asarray(active_fit.decreasing_rad, dtype=float), - np.asarray(active_fit.increasing_rad, dtype=float), - ) - ) - passive = np.concatenate( - ( - np.asarray(passive_fit.decreasing_rad, dtype=float), - np.asarray(passive_fit.increasing_rad, dtype=float), - ) - ) - coefficients, residual = _coupling_regression( - active, passive, degree=degree - ) - multiplier = coefficients[0] - if not minimum_multiplier <= multiplier <= maximum_multiplier: - raise ValueError( - f"{target_joint} coupling linear term is outside [0.5, 1.5]" - ) - cycle_coefficients = tuple( - _coupling_regression( - active_cycle, passive_cycle, degree=degree - )[0] - for active_cycle, passive_cycle in cycle_curve_pairs - ) - cycle_multipliers = tuple(values[0] for values in cycle_coefficients) - cycle_range = ( - 0.0 - if len(cycle_multipliers) < 2 - else float(max(cycle_multipliers) - min(cycle_multipliers)) - ) - if cycle_range > maximum_cycle_range: - raise ValueError( - f"{target_joint} coupling linear-term cycle range exceeds 0.03" - ) - cycle_prediction_range = 0.0 - if len(cycle_coefficients) >= 2: - grid = np.linspace(0.0, float(np.max(active)), 256) - predictions = np.asarray( - [ - sum(value * grid ** (index + 1) for index, value in enumerate(values)) - for values in cycle_coefficients - ] - ) - cycle_prediction_range = float( - np.max(np.ptp(predictions, axis=0)) - ) - if cycle_prediction_range > maximum_cycle_prediction_range_rad: - raise ValueError( - f"{target_joint} coupling cycle prediction range exceeds 1 degree" - ) - absolute = np.abs(residual) - rms = float(np.sqrt(np.mean(np.square(residual)))) - p95 = float(np.percentile(absolute, 95.0)) - maximum = float(np.max(absolute)) - if p95 > maximum_residual_p95_rad or maximum > maximum_residual_rad: - raise ValueError( - "coupling_residual_exceeds:" - f"joint={target_joint}:model={model}:" - f"linear_term={multiplier:.6f}:" - f"p95_deg={math.degrees(p95):.3f}:" - f"maximum_deg={math.degrees(maximum):.3f}:" - f"p95_limit_deg={math.degrees(maximum_residual_p95_rad):.3f}:" - f"maximum_limit_deg={math.degrees(maximum_residual_rad):.3f}" - ) - return MimicFit( - source_joint=source_joint, - target_joint=target_joint, - model=model, - coefficients=coefficients, - # Standard URDF has only a linear mimic. For a nonlinear coupling, - # preserve the familiar editor/RViz linkage with a fallback line that - # is exact at both the open zero and measured closed endpoint. The - # direction-aware runtime curves and MuJoCo polynomial remain exact in - # between those endpoints. - urdf_mimic_multiplier=( - multiplier - if model == "linear_mimic" - else curve_travel_rad(passive_fit) / curve_travel_rad(active_fit) - ), - urdf_mimic_policy=( - "exact_linear" - if model == "linear_mimic" - else "endpoint_linear_fallback" - ), - cycle_coefficients=cycle_coefficients, - maximum_cycle_range=cycle_range, - maximum_cycle_prediction_range_rad=cycle_prediction_range, - residual_rms_rad=rms, - residual_p95_rad=p95, - residual_max_rad=maximum, - ) - - -def fit_mimic_multiplier( - source_joint: str, - target_joint: str, - active_fit: JointCurveFit, - passive_fit: JointCurveFit, - *, - cycle_curve_pairs: Sequence[ - tuple[Sequence[float], Sequence[float]] - ] = (), - minimum_multiplier: float = 0.5, - maximum_multiplier: float = 1.5, - maximum_cycle_range: float = 0.03, - maximum_residual_p95_rad: float = math.radians(2.0), - maximum_residual_rad: float = math.radians(3.0), -) -> MimicFit: - return fit_coupling_model( - source_joint, - target_joint, - active_fit, - passive_fit, - model="linear_mimic", - cycle_curve_pairs=cycle_curve_pairs, - minimum_multiplier=minimum_multiplier, - maximum_multiplier=maximum_multiplier, - maximum_cycle_range=maximum_cycle_range, - maximum_residual_p95_rad=maximum_residual_p95_rad, - maximum_residual_rad=maximum_residual_rad, - ) - - -def fit_l6_session( - source_urdf: str | Path, - records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], - *, - require_thumb_axis_zero: bool = False, -) -> L6FitResult: - """Fit three training cycles and validate the isolated fourth cycle.""" - expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS - if set(records_by_joint) != expected: - raise ValueError("L6 records must contain exactly five measured joints") - curves: dict[str, JointCurveFit] = {} - holdout: dict[str, tuple[float, ...]] = {} - cycle_curves: dict[str, dict[int, JointCurveFit]] = {} - for name in sorted(expected): - rows = [dict(row) for row in records_by_joint[name]] - 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"{name} is missing training or holdout records") - fit = fit_rotation_joint_curve(training, zero_command_u8=255) - errors = rotation_curve_holdout_errors( - fit, validation, zero_command_u8=255 - ) - 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"{name} isolated holdout failed") - correction_limit = math.radians( - 3.0 if name in MEASURED_PASSIVE_JOINTS else 2.0 - ) - if fit.maximum_monotonic_correction_rad > correction_limit: - raise ValueError(f"{name} monotonic correction exceeds limit") - curves[name] = fit - holdout[name] = errors - cycle_curves[name] = { - cycle: fit_rotation_joint_curve( - [row for row in training if int(row["cycle"]) == cycle], - zero_command_u8=255, - ) - for cycle in (0, 1, 2) - } - endpoint_offsets, travels = derive_endpoint_zero_offsets( - source_urdf, - curves, - endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT, - ) - thumb_zero_result: ZeroSolveResult | None = None - offsets = { - "rh_thumb_cmc_roll": 0.0, - "rh_thumb_cmc_pitch": endpoint_offsets["rh_thumb_cmc_pitch"], - **endpoint_offsets, - } - pinky_endpoint_method = { - "upper_at_end": "mechanical_upper_endpoint", - "lower_at_start": "mechanical_lower_endpoint", - "cad_range_center": "cad_range_center", - "zero_at_start": "source_joint_zero", - }[ENDPOINT_ANCHOR_BY_JOINT["rh_pinky_mcp_pitch"]] - zero_methods = { - "rh_thumb_cmc_roll": "source_joint_zero_unpublished", - "rh_thumb_cmc_pitch": "cad_range_center_unpublished", - "rh_pinky_mcp_pitch": pinky_endpoint_method, - } - zero_fallback_reasons: dict[str, str] = {} - has_axis_geometry = _has_complete_axis_geometry(records_by_joint) - if require_thumb_axis_zero and not has_axis_geometry: - raise ValueError( - "L6 thumb absolute zero requires G20-compatible common-frame " - "Tag pose trajectories; this session must be reacquired" - ) - if has_axis_geometry: - geometric_result = _fit_l6_thumb_axis_zero( - source_urdf, - records_by_joint, - curves, - require_passed=False, - ) - thumb_zero_result = geometric_result - pitch_failure = geometric_result.failure_reasons.get( - "rh_thumb_cmc_pitch" - ) - endpoint_fallback_reasons = { - "zero_offset_exceeds_configured_limit", - "zero_offset_reached_diagnostic_bound", - } - if ( - not geometric_result.passed - and set(geometric_result.failure_reasons) - == {"rh_thumb_cmc_pitch"} - and pitch_failure in endpoint_fallback_reasons - ): - # L6_RIGHT_001 demonstrated a stable pitch-to-DIP axis-line phase - # beyond the diagnostic search bound. That phase includes - # physical link geometry and is not a safe encoder-zero observation - # when it contradicts both the reviewed endpoint and the +/-15 deg - # write limit. Keep roll geometric, but freeze pitch to its - # independent measured/CAD range-centre datum. Any roll, - # holdout, cone, confidence, or multi-joint failure remains a hard - # rejection. - zero_fallback_reasons["rh_thumb_cmc_pitch"] = str(pitch_failure) - thumb_zero_result = _fit_l6_thumb_axis_zero( - source_urdf, - records_by_joint, - curves, - fixed_direct_zero_offsets_rad={ - "rh_thumb_cmc_pitch": endpoint_offsets[ - "rh_thumb_cmc_pitch" - ] - }, - ) - elif not geometric_result.passed: - details = ",".join( - f"{name}={reason}" - for name, reason in sorted( - geometric_result.failure_reasons.items() - ) - ) - raise ValueError("L6 thumb axis zero solve failed:" + details) - offsets.update( - { - name: float(thumb_zero_result.direct_offsets_rad[name]) - for name in ( - "rh_thumb_cmc_roll", - "rh_thumb_cmc_pitch", - ) - } - ) - zero_methods.update( - { - "rh_thumb_cmc_roll": "urdf_serial_axis_geometry", - "rh_thumb_cmc_pitch": ( - "cad_range_center_after_geometry_rejection" - if "rh_thumb_cmc_pitch" in zero_fallback_reasons - else "urdf_serial_axis_geometry" - ), - } - ) - mimic_fits: dict[str, MimicFit] = {} - for target in sorted(MEASURED_PASSIVE_JOINTS): - source = MIMIC_SOURCE_BY_JOINT[target] - cycle_pairs = [] - for cycle in (0, 1, 2): - active = cycle_curves[source][cycle] - passive = cycle_curves[target][cycle] - cycle_pairs.append( - ( - tuple(active.decreasing_rad) + tuple(active.increasing_rad), - tuple(passive.decreasing_rad) + tuple(passive.increasing_rad), - ) - ) - mimic_fits[target] = fit_coupling_model( - source, - target, - curves[source], - curves[target], - model=COUPLING_MODEL_BY_JOINT[target], - cycle_curve_pairs=cycle_pairs, - ) - return L6FitResult( - curves=curves, - zero_offsets_rad=offsets, - travels_rad=travels, - mimic_fits=mimic_fits, - holdout_errors_rad=holdout, - zero_method_by_joint=zero_methods, - zero_fallback_reason_by_joint=zero_fallback_reasons, - thumb_zero_result=thumb_zero_result, - ) - - -__all__ = [ - "L6FitResult", - "MimicFit", - "curve_travel_rad", - "derive_endpoint_zero_offsets", - "L6_THUMB_AXIS_JOINTS", - "fit_coupling_model", - "fit_l6_session", - "fit_mimic_multiplier", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py deleted file mode 100644 index 21c194f..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/motion.py +++ /dev/null @@ -1,82 +0,0 @@ -"""Safe six-channel motion helpers for the partial L6 profile.""" - -from __future__ import annotations - -import math -from typing import Sequence - -from ...core import CalibrationProfile, TaskSpec - - -def cosine_position_trajectory_u8( - start_u8: float, - target_u8: float, - elapsed_seconds: float, - full_range_duration_seconds: float, -) -> tuple[float, float, float]: - """Return a bounded, zero-end-velocity L6 command trajectory sample.""" - duration = ( - float(full_range_duration_seconds) - * abs(float(target_u8) - float(start_u8)) - / 255.0 - ) - if full_range_duration_seconds <= 0.0: - raise ValueError("full_range_duration_seconds must be positive") - if duration <= 0.0: - return float(target_u8), 1.0, 0.0 - phase = min(1.0, max(0.0, float(elapsed_seconds) / duration)) - blend = 0.5 - 0.5 * math.cos(math.pi * phase) - value = float(start_u8) + (float(target_u8) - float(start_u8)) * blend - return value, phase, duration - - -def build_calibration_motion_command( - task: TaskSpec, - command_u8: int, - *, - profile: CalibrationProfile, -) -> list[int]: - values = list(profile.command.baseline_u8) - for index, value in task.auxiliary_commands: - values[int(index)] = int(value) - values[int(task.command_index)] = int(command_u8) - return values - - -def build_calibration_preparation_waypoints( - task: TaskSpec, - *, - profile: CalibrationProfile, - current_command: Sequence[int] | None = None, -) -> tuple[tuple[int, ...], ...]: - del current_command - start = build_calibration_motion_command( - task, task.start_u8, profile=profile - ) - return (tuple(start),) - - -def build_calibration_return_waypoints( - target_command: Sequence[int] | None = None, - *, - profile: CalibrationProfile, - current_command: Sequence[int] | None = None, - **_: object, -) -> tuple[tuple[int, ...], ...]: - del current_command - target = ( - tuple(int(value) for value in target_command) - if target_command is not None - else tuple(profile.command.baseline_u8) - ) - if len(target) != profile.command.command_count: - raise ValueError("six-channel return command has the wrong channel count") - return (target,) - - -__all__ = [ - "build_calibration_motion_command", - "build_calibration_preparation_waypoints", - "build_calibration_return_waypoints", - "cosine_position_trajectory_u8", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py deleted file mode 100644 index e82b2dd..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/node.py +++ /dev/null @@ -1,1779 +0,0 @@ -"""Online three-view acquisition node for L6/right/l6_right_8/v1.""" - -from __future__ import annotations - -from collections import deque -from dataclasses import dataclass -import json -import math -from pathlib import Path -import threading -import time -import traceback -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 -from sensor_msgs.msg import CameraInfo, JointState -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from ...acquisition import StateSample, interpolate_state_u8 -from ...extrinsics import ( - load_three_camera_extrinsics, - matrix_payload, - transform_matrix, -) -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 - - -@dataclass(frozen=True) -class MotionStep: - phase: str - task_key: str | None - command_index: int | None - target_u8: float - speed_u8: float - cycle: int | None = None - direction: str | None = None - attempt: int = 1 - target_command: tuple[float, ...] | None = None - - @property - def recording(self) -> bool: - return self.direction in {"decreasing", "increasing"} - - -def _stamp_ns(stamp: Any) -> int: - return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) - - -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, - *, - profile=None, - finalizer=None, - sample_kind: str = "l6_joint_sample", - ) -> None: - self.profile = build_typed_profile() if profile is None else profile - self.model_name = self.profile.key.model - self.command_names = tuple(self.profile.command.names) - self.command_count = self.profile.command.command_count - self.command_unit = self.profile.command.unit - 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) - self.raw_path = self.session_dir / "raw_samples.jsonl" - append_jsonl( - self.raw_path, - { - "kind": "session_start", - "sample_schema_version": self.profile.artifacts.output_schema_version, - "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 - ), - }, - ) - - self.command_publisher = self.create_publisher( - JointState, self.command_topic, 10 - ) - self.setting_publisher = self.create_publisher( - String, self.setting_topic, 10 - ) - self.status_publisher = self.create_publisher( - String, f"{self.profile.namespace}/status", 10 - ) - self.create_subscription( - 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]] = {} - self.last_view_valid_at: dict[str, float] = {} - self.base_corner_observations: dict[str, deque[np.ndarray]] = { - view: deque(maxlen=30) for view in self.profile.vision.view_names - } - self.base_corner_reference: dict[str, np.ndarray] = {} - self.base_drift_counts: dict[str, int] = {} - self.latest_base_drift_px: dict[str, float] = {} - self.trackers = { - view: SquareTagPoseTracker( - maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, - reprojection_tie_px=self.pnp_reprojection_tie_px, - maximum_pose_jump_rad=math.radians(self.pnp_maximum_pose_jump_deg), - maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, - maximum_tag_tilt_rad=math.radians(self.pnp_maximum_tag_tilt_deg), - reset_after_seconds=self.pnp_tracker_reset_seconds, - ) - for view in self.profile.vision.view_names - } - for view in self.profile.vision.view_names: - self.create_subscription( - CameraInfo, - self.camera_info_topics[view], - lambda message, selected=view: self._camera_info_callback( - 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._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, - 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 - self.step_started_at = 0.0 - self.step_hold_since: float | None = None - self.step_command_sent = False - self.step_last_progress_at = 0.0 - self.step_last_feedback = float("nan") - self.step_last_distance_u8 = float("nan") - self.step_initial_distance_u8 = float("nan") - self.step_initial_feedback = float("nan") - self.step_speed_ready_at = 0.0 - self.step_requested_u8 = float("nan") - self.step_start_state_u8: tuple[float, ...] = () - 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 - self.step_total_frames = 0 - self.step_required_roles: tuple[str, ...] = () - self.step_tag_seen_frames: dict[str, int] = {} - self.step_tag_quality_frames: dict[str, int] = {} - self.step_all_tags_quality_frames = 0 - self.step_pnp_valid_frames = 0 - self.step_state_sync_frames = 0 - self.step_rejection_counts: dict[str, int] = {} - self.latest_recognized_tag_ids: tuple[int, ...] = () - self.latest_unrecognized_tag_ids: tuple[int, ...] = () - self.state = "WAIT_DEVICES" - self.reason = "waiting_for_six_channel_feedback_and_three_camera_info" - self.started = False - self.commanded_speed: int | None = None - self.retry_counts: dict[tuple[str, int, str], int] = {} - self.create_timer( - 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) - - def _declare_parameters(self) -> None: - model = self.model_name.lower() - namespace = self.profile.namespace - speed_parameters = self.profile.motion.speed_parameters - defaults: dict[str, Any] = { - "serial_number": "UNSET", - "session_dir": "", - "source_urdf_path": "", - "source_urdf_expected_sha256": "", - "camera_extrinsics_file": "", - "camera_extrinsics_expected_sha256": "", - "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", - "front_camera_info_topic": f"{namespace}/front/camera/camera_info", - "front_detections_topic": f"{namespace}/front/apriltag/detections", - "side_camera_info_topic": f"{namespace}/side/camera/camera_info", - "side_detections_topic": f"{namespace}/side/apriltag/detections", - "top_camera_info_topic": f"{namespace}/top/camera/camera_info", - "top_detections_topic": f"{namespace}/top/apriltag/detections", - "commands_enabled": True, - "baseline_speed_u8": int( - speed_parameters.get( - "baseline_u8", speed_parameters.get("preflight_u8", 1) - ) - ), - "preflight_speed_u8": int(speed_parameters.get("preflight_u8", 1)), - "formal_speed_u8": int(speed_parameters.get("formal_u8", 1)), - "speed_settle_seconds": float( - speed_parameters.get("speed_settle_seconds", 0.2) - ), - "command_trajectory_full_range_seconds": float( - speed_parameters.get("command_trajectory_full_range_seconds", 6.0) - ), - "torque_u8": int(speed_parameters.get("torque_u8", 80)), - "endpoint_tolerance_u8": 2.0, - "endpoint_hold_seconds": 1.0, - "motor_stall_timeout_seconds": 2.0, - "position_timeout_seconds": 30.0, - "sweep_timeout_seconds": 90.0, - "automatic_sweep_retry_limit": 1, - "non_target_motion_tolerance_u8": 3.0, - "minimum_sweep_frames": 40, - "minimum_state_span_u8": 240.0, - "minimum_sweep_bins": 32, - "maximum_bin_gap": 16, - "minimum_detection_rate": 0.95, - # Three independently detected Tags at 95% each can only yield - # 0.95**3 ~= 85.7% fully joined frames. Keep the per-Tag gate at - # 95%, and gate the downstream joined PnP/state samples separately. - "minimum_joint_frame_rate": 0.85, - "minimum_feedback_hz": 25.0, - "maximum_state_image_skew_ms": 50.0, - "maximum_hamming": 0, - "minimum_decision_margin": 30.0, - "minimum_edge_pixels": 30.0, - "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, - "pnp_maximum_pose_jump_deg": 35.0, - "pnp_maximum_translation_jump_m": 0.04, - "pnp_maximum_tag_tilt_deg": 75.0, - "pnp_tracker_reset_seconds": 5.0, - "command_rate_hz": float(speed_parameters.get("command_rate_hz", 100.0)), - "endpoint_tolerance_rad": 0.01, - "non_target_motion_tolerance_rad": 0.015, - "minimum_state_span_fraction": 0.90, - } - for name, default in defaults.items(): - self.declare_parameter(name, default) - - def _load_parameters(self) -> None: - value = lambda name: self.get_parameter(name).value - self.serial_number = str(value("serial_number")) - self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() - self.source_urdf = Path(str(value("source_urdf_path"))).expanduser().resolve() - self.camera_extrinsics_file = Path( - str(value("camera_extrinsics_file")) - ).expanduser().resolve() - if self.serial_number in {"", "UNSET"}: - raise ValueError("serial_number is required") - if not str(value("session_dir")) or not self.source_urdf.is_file(): - raise ValueError("session_dir and immutable source_urdf_path are required") - if not self.camera_extrinsics_file.is_file(): - raise ValueError( - f"camera_extrinsics_file is required for {self.model_name} zero solve" - ) - self.extrinsics = load_three_camera_extrinsics( - self.camera_extrinsics_file, - quality_limits=self.profile.vision.extrinsics_quality_limits, - minimum_capture_counts=self.profile.vision.minimum_capture_counts, - ) - self.protected_inputs = { - "source_urdf_sha256": str(value("source_urdf_expected_sha256")), - "camera_extrinsics_sha256": str( - value("camera_extrinsics_expected_sha256") - ), - "calibration_config_sha256": str( - value("calibration_config_expected_sha256") - ), - "tag_config_sha256": str(value("tag_config_expected_sha256")), - } - if "sdk_config_sha256" in self.profile.artifacts.protected_input_fields: - 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 - or any(len(item) != 64 for item in self.protected_inputs.values()) - ): - raise ValueError("all profile-protected SHA-256 values are required") - self.command_topic = str(value("command_topic")) - self.state_topic = str(value("state_topic")) - self.setting_topic = str(value("setting_topic")) - self.camera_info_topics = { - view: str(value(f"{view}_camera_info_topic")) - for view in self.profile.vision.view_names - } - self.detection_topics = { - view: str(value(f"{view}_detections_topic")) - for view in self.profile.vision.view_names - } - for name in ( - "commands_enabled", "baseline_speed_u8", "preflight_speed_u8", - "formal_speed_u8", - "speed_settle_seconds", "command_trajectory_full_range_seconds", - "torque_u8", "endpoint_tolerance_u8", "endpoint_hold_seconds", - "motor_stall_timeout_seconds", "position_timeout_seconds", - "sweep_timeout_seconds", "automatic_sweep_retry_limit", - "non_target_motion_tolerance_u8", "minimum_sweep_frames", - "minimum_state_span_u8", "minimum_sweep_bins", "maximum_bin_gap", - "minimum_detection_rate", "minimum_joint_frame_rate", - "minimum_feedback_hz", - "maximum_state_image_skew_ms", "maximum_hamming", - "minimum_decision_margin", "minimum_edge_pixels", - "fixed_base_maximum_corner_drift_px", - "fixed_base_movement_confirmation_frames", "tag_size_m", - "pnp_maximum_reprojection_error_px", "pnp_reprojection_tie_px", - "pnp_maximum_pose_jump_deg", "pnp_maximum_translation_jump_m", - "pnp_maximum_tag_tilt_deg", "pnp_tracker_reset_seconds", - "command_rate_hz", "endpoint_tolerance_rad", - "non_target_motion_tolerance_rad", "minimum_state_span_fraction", - ): - setattr(self, name, value(name)) - self.maximum_state_image_skew_ns = int( - float(self.maximum_state_image_skew_ms) * 1_000_000 - ) - if self.command_unit == "u8": - for name in ( - "baseline_speed_u8", "preflight_speed_u8", "formal_speed_u8" - ): - if not 1 <= int(getattr(self, name)) <= 255: - raise ValueError(f"{name} must be in [1, 255]") - else: - self.endpoint_tolerance_u8 = float(self.endpoint_tolerance_rad) - self.non_target_motion_tolerance_u8 = float( - self.non_target_motion_tolerance_rad - ) - self.minimum_state_span_u8 = float(self.minimum_state_span_fraction) - if not 1.0 <= float(self.command_rate_hz) <= 100.0: - raise ValueError("command_rate_hz must be in [1, 100]") - if not 0.0 < float(self.endpoint_tolerance_u8) <= 0.1: - raise ValueError("endpoint_tolerance_rad must be in (0, 0.1]") - if not 0.0 < float(self.minimum_state_span_u8) <= 1.0: - raise ValueError("minimum_state_span_fraction must be in (0, 1]") - if not 0.0 <= float(self.speed_settle_seconds) <= 5.0: - raise ValueError("speed_settle_seconds must be in [0, 5]") - if not 2.0 <= float(self.command_trajectory_full_range_seconds) <= 30.0: - raise ValueError( - "command_trajectory_full_range_seconds must be in [2, 30]" - ) - if not 0.0 < float(self.minimum_detection_rate) <= 1.0: - raise ValueError("minimum_detection_rate must be in (0, 1]") - if not 0.0 < float(self.minimum_joint_frame_rate) <= 1.0: - raise ValueError("minimum_joint_frame_rate must be in (0, 1]") - - def _build_steps(self) -> list[MotionStep]: - steps = [ - MotionStep( - "baseline", None, None, 255, int(self.baseline_speed_u8) - ) - ] - for task in self.profile.motion.tasks: - formal_speed = ( - task.formal_speed - if getattr(self, "command_unit", self.profile.command.unit) == "rad" - else self.formal_speed_u8 - ) - for cycle in (0, 1, 2, 3): - steps.extend( - [ - MotionStep("prepare", task.key, task.command_index, task.start_value, float(formal_speed), cycle), - MotionStep("sweep", task.key, task.command_index, task.end_value, float(formal_speed), cycle, "decreasing"), - MotionStep("prepare", task.key, task.command_index, task.end_value, float(formal_speed), cycle), - MotionStep("sweep", task.key, task.command_index, task.start_value, float(formal_speed), cycle, "increasing"), - ] - ) - return steps - - def _start(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: - if self.started: - response.success = False - response.message = f"{self.model_name} calibration has already started" - return response - if self.state != "READY": - response.success = False - response.message = self.reason - return response - self.steps = self._build_steps() - self.step_index = 0 - self.started = True - self.state = "RUNNING" - self.reason = "moving_to_open_baseline" - self._publish_torque() - response.success = True - response.message = f"{self.model_name} partial calibration started" - return response - - def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: - hold = ( - 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) - self.state = "ABORTED" - self.reason = ( - "operator_abort_holding_current_position" - if self.command_unit == "rad" - else "operator_abort_returning_to_open_baseline" - ) - response.success = True - response.message = self.reason - return response - - def _camera_info_callback(self, view: str, message: CameraInfo) -> None: - 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: - adapter = getattr(self, "sdk_adapter", ProfileSdkAdapter(self.profile.command)) - state = adapter.parse_feedback(message.name, message.position) - if state is None: - return - violation = next(( - (index, value) - for index, value in enumerate(state) - 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: - stamp = int(self.get_clock().now().nanoseconds) - if not self.state_history or stamp > self.state_history[-1].stamp_ns: - self.state_history.append(StateSample(stamp, state)) - self.latest_state_u8 = state - self.state_receive_times.append(time.monotonic()) - # 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) - - def _view(self, name: str): - return next(view for view in self.profile.vision.views if view.name == name) - - def _current_step(self) -> MotionStep | None: - if 0 <= self.step_index < len(self.steps): - return self.steps[self.step_index] - return None - - def _required_roles(self, view: str) -> tuple[str, ...]: - step = self._current_step() - if step is None or step.task_key is None: - return tuple(tag.role for tag in self._view(view).tags) - task = self._task(step.task_key) - if task.view != view: - return () - roles: list[str] = [] - for joint in task.joints: - measurement = self.profile.measurement.measurements[joint] - roles.extend((str(measurement.parent_role), str(measurement.child_role))) - return tuple(dict.fromkeys(roles)) - - def _reset_step_observation_counters( - self, roles: tuple[str, ...] = () - ) -> None: - self.step_required_roles = tuple(roles) - self.step_valid_frames = 0 - self.step_total_frames = 0 - self.step_tag_seen_frames = {role: 0 for role in roles} - self.step_tag_quality_frames = {role: 0 for role in roles} - self.step_all_tags_quality_frames = 0 - self.step_pnp_valid_frames = 0 - self.step_state_sync_frames = 0 - self.step_rejection_counts = {} - required_ids = tuple( - int(tag.tag_id) - for view in self.profile.vision.views - for tag in view.tags - if tag.role in roles - ) - self.latest_recognized_tag_ids = () - self.latest_unrecognized_tag_ids = tuple(sorted(required_ids)) - - def _count_step_rejection(self, reason: str) -> None: - self.step_rejection_counts[reason] = ( - self.step_rejection_counts.get(reason, 0) + 1 - ) - - def _step_observation_metrics(self) -> dict[str, Any]: - total = int(self.step_total_frames) - - def rate(count: int) -> float: - return float(count) / total if total else 0.0 - - seen_by_role = { - role: rate(self.step_tag_seen_frames.get(role, 0)) - for role in self.step_required_roles - } - quality_by_role = { - role: rate(self.step_tag_quality_frames.get(role, 0)) - for role in self.step_required_roles - } - return { - "tag_seen_rate_by_role": seen_by_role, - "tag_detection_rate_by_role": quality_by_role, - # The formal >=95% Tag gate applies to every required Tag, not to - # the product of several independent per-Tag probabilities. - "tag_detection_rate": min(quality_by_role.values(), default=0.0), - "all_tags_quality_rate": rate(self.step_all_tags_quality_frames), - "pnp_valid_rate": rate(self.step_pnp_valid_frames), - "state_sync_rate": rate(self.step_state_sync_frames), - "joint_frame_rate": rate(self.step_valid_frames), - "rejection_counts": dict(sorted(self.step_rejection_counts.items())), - } - - def _detections_callback(self, view: str, message: AprilTagDetectionArray) -> None: - roles = self._required_roles(view) - if not roles or view not in self.camera_matrices: - return - role_by_id = {tag.tag_id: tag.role for tag in self._view(view).tags} - corners: dict[str, np.ndarray] = {} - good: dict[str, bool] = {} - detected_roles: set[str] = set() - quality_failures: 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 is None: - continue - detected_roles.add(role) - points = np.asarray( - [[float(point.x), float(point.y)] for point in detection.corners], - dtype=float, - ) - if points.shape != (4, 2): - quality_failures[role] = ("malformed_corners",) - continue - edges = np.linalg.norm(points - np.roll(points, -1, axis=0), axis=1) - full_border = 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 - ) - failures: list[str] = [] - if int(detection.hamming) > int(self.maximum_hamming): - failures.append("hamming") - if float(detection.decision_margin) < float(self.minimum_decision_margin): - failures.append("decision_margin") - if float(np.min(edges)) < float(self.minimum_edge_pixels): - failures.append("edge_pixels") - if not full_border: - failures.append("image_border") - candidate_good = not failures - # Prefer a good detection if a detector ever emits a duplicate ID. - if role not in corners or candidate_good or not good.get(role, False): - corners[role] = points - good[role] = candidate_good - quality_failures[role] = tuple(failures) - 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 - ) - active_this_view = bool( - step is not None - and step.task_key is not None - and self._task(str(step.task_key)).view == view - ) - if active_this_view: - id_by_role = { - tag.role: int(tag.tag_id) for tag in self._view(view).tags - } - self.latest_recognized_tag_ids = tuple( - sorted(id_by_role[role] for role in roles if good.get(role, False)) - ) - self.latest_unrecognized_tag_ids = tuple( - sorted( - id_by_role[role] - for role in roles - if not good.get(role, False) - ) - ) - if recording_this_view: - self.step_total_frames += 1 - for role in roles: - if role in detected_roles: - self.step_tag_seen_frames[role] = ( - self.step_tag_seen_frames.get(role, 0) + 1 - ) - else: - self._count_step_rejection(f"tag:{role}:missing") - if good.get(role, False): - self.step_tag_quality_frames[role] = ( - self.step_tag_quality_frames.get(role, 0) + 1 - ) - else: - reasons = quality_failures.get( - role, - ("malformed_corners",) if role in detected_roles else (), - ) - for reason in reasons: - self._count_step_rejection(f"tag:{role}:{reason}") - if all(role in corners and good.get(role, False) for role in roles): - self.step_all_tags_quality_frames += 1 - base_role = next( - tag.role for tag in self._view(view).tags if tag.fixed_reference - ) - if base_role in corners and good.get(base_role, False): - if not recording_this_view: - observations = self.base_corner_observations[view] - observations.append(corners[base_role].copy()) - if len(observations) >= 5: - self.base_corner_reference[view] = np.median( - np.asarray(observations, dtype=float), axis=0 - ) - self.base_drift_counts[view] = 0 - self.latest_base_drift_px[view] = 0.0 - elif view in self.base_corner_reference: - reference = self.base_corner_reference[view] - drift = float( - np.max( - np.linalg.norm(corners[base_role] - reference, 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": next( - tag.tag_id - for tag in self._view(view).tags - if tag.fixed_reference - ), - "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 - # 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] = 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], - tag_size_m=float(self.tag_size_m), - camera_matrix=self.camera_matrices[view], - stamp_ns=stamp, - ) - if pose is None: - if recording_this_view: - self._count_step_rejection( - f"pnp:{role}:{pnp_reason or 'rejected'}" - ) - 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() - preflight_hook = getattr(self, "_observe_preflight_pose", None) - if preflight_hook is not None and step is not None and step.phase == "preflight": - preflight_hook(view, selected, step) - if step is None or not step.recording or self._task(step.task_key).view != view: - return - matched = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) - if matched is None: - self._count_step_rejection("state_sync:no_sample_within_limit") - return - state_u8, skew_ns = matched - self.step_state_sync_frames += 1 - task = self._task(step.task_key) - feedback = float(state_u8[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_feedback_domain") - return - 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)] - child = selected[str(measurement.child_role)] - 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) - ) - 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]) - ) - record = { - "kind": self.sample_kind, - "profile_id": self.profile.key.profile_id, - "task_name": task.key, - "view": view, - "joint": joint, - "sdk_channel": self.command_names[task.command_index], - "motor_index": task.command_index, - "cycle": int(step.cycle), - "direction": str(step.direction), - "attempt": int(step.attempt), - # Keep the established schema contract: this field labels the - # requested sweep endpoint. The live shaped set-point is a - # separate diagnostic and is never used as the fit domain. - "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() - ], - # G20-compatible geometric observations. The fitted screw - # axes, rather than an electrical endpoint assumption, define - # the two thumb CMC static zeros. - "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), - "view_normal_common_xyz": [ - float(value) - for value in ( - common_from_view[:3, :3] - @ np.asarray([0.0, 0.0, 1.0], dtype=float) - ) - ], - "camera_center_common_xyz_m": [ - float(value) for value in common_from_view[:3, 3] - ], - "pnp_reprojection_error_px": round( - max(parent.reprojection_error_px, child.reprojection_error_px), 6 - ), - "image_stamp_ns": stamp, - } - if self.command_unit == "u8": - record.update({ - "requested_command_u8": int(step.target_u8), - "trajectory_command_u8": round(self.step_requested_u8, 6), - "feedback_u8": round(feedback, 6), - "state_u8": [round(float(value), 6) for value in state_u8], - }) - else: - record.update({ - "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), - "state_rad": [round(float(value), 9) for value in state_u8], - "progress_01": round(progress, 9), - }) - 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 - message = String() - message.data = json.dumps( - { - "setting_cmd": "set_max_torque_limits", - "params": {"hand_type": "right", "torque": [int(self.torque_u8)] * 6}, - } - ) - self.setting_publisher.publish(message) - - def _publish_speed(self, speed: float) -> None: - if self.command_unit != "u8": - self.commanded_speed = float(speed) - return - if not self.commands_enabled or self.commanded_speed == int(speed): - return - message = String() - message.data = json.dumps( - { - "setting_cmd": "set_speed", - "params": {"hand_type": "right", "speed": [int(speed)] * 6}, - } - ) - self.setting_publisher.publish(message) - self.commanded_speed = int(speed) - - def _publish_command(self, values: list[float]) -> None: - if not self.commands_enabled: - return - if len(values) != self.command_count: - raise ValueError("command has the wrong channel count") - bounded = [ - float(np.clip(value, self.command_lower[index], self.command_upper[index])) - for index, value in enumerate(values) - ] - message = JointState() - message.header.stamp = self.get_clock().now().to_msg() - message.name = ( - [] if self.profile.command.feedback_by_index else list(self.command_names) - ) - 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: - if len(step.target_command) != self.command_count: - raise ValueError("motion-step target has the wrong channel count") - return tuple(float(value) for value in step.target_command) - target = [float(value) for value in self.baseline_command] - if step.task_key is not None: - for index, value in self._task(step.task_key).auxiliary_commands: - target[int(index)] = float(value) - if step.command_index is not None: - target[step.command_index] = float(step.target_u8) - 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) - self.step_speed_ready_at = now + float(self.speed_settle_seconds) - self.reason = f"setting_speed:{step.speed_u8}" - return - if now < self.step_speed_ready_at: - return - if len(self.latest_state_u8) != self.command_count: - return - 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 = ( - float(self.latest_state_u8[step.command_index]) - if step.command_index is not None and len(self.latest_state_u8) == self.command_count - else ( - float(np.mean(self.latest_state_u8)) - if len(self.latest_state_u8) == self.command_count - else float("nan") - ) - ) - self.step_initial_feedback = self.step_last_feedback - self.step_requested_u8 = self.step_initial_feedback - target = list(self._target_command(step)) - errors = [ - abs(end - start) - for start, end in zip(self.step_start_state_u8, target) - ] - self.step_moving_indices = frozenset( - index for index, error in enumerate(errors) - if error > float(self.non_target_motion_tolerance_u8) - ) - command_distance = ( - float(sum(errors)) - if step.command_index is None - else float(errors[step.command_index]) - ) - 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 = ( - self._radian_trajectory_fraction( - maximum_distance, 0.0, float(step.speed_u8) - ) - ) - else: - _, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8( - 0.0, maximum_distance, 0.0, - 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, ...] = () - if step.recording and step.task_key is not None: - roles = self._required_roles(self._task(step.task_key).view) - self._reset_step_observation_counters(roles) - self.step_command_sent = True - self._advance_step_trajectory(step, now) - self.reason = ( - f"{step.phase}:{step.task_key or 'all'}:cycle={step.cycle}:" - 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) - else: - # Compatibility for the isolated legacy trajectory unit harness. - target = [255.0] * len(self.step_start_state_u8) - if step.command_index is not None: - target[step.command_index] = float(step.target_u8) - target_values = tuple(target) - elapsed = max(0.0, float(now) - self.step_started_at) - 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) - ) - 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) - ] - command = tuple( - int(np.clip(round(value), 0, 255)) - if getattr(self, "command_unit", "u8") == "u8" - else float(np.clip(value, self.command_lower[index], self.command_upper[index])) - 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]) - ) - # 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 - - def _qualify_recording_step(self, step: MotionStep) -> None: - task = self._task(str(step.task_key)) - rows = [ - row for row in self.raw_records - if row["task_name"] == task.key - and int(row["cycle"]) == int(step.cycle) - and row["direction"] == step.direction - and int(row["attempt"]) == step.attempt - and row["joint"] == task.joints[0] - ] - command_unit = getattr(self, "command_unit", "u8") - feedback_field = "feedback_u8" if command_unit == "u8" else "feedback_rad" - feedback = np.asarray([float(row[feedback_field]) for row in rows]) - if command_unit == "u8": - bins = sorted(set(int(round(value)) for value in feedback)) - span = float(np.ptp(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=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( - normalized_bin_count - 1, - max( - 0, - int( - self.profile.command.normalize( - task.command_index, value - ) * normalized_bin_count - ), - ), - ) - for value in feedback - ) - ) - bins = normalized - span = ( - float(np.ptp([self.profile.command.normalize(task.command_index, value) for value in feedback])) - if feedback.size else 0.0 - ) - 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"]) - 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, - { - "kind": getattr( - self, "sweep_quality_kind", "l6_sweep_observation_quality" - ), - "task_name": step.task_key, - "cycle": step.cycle, - "direction": step.direction, - "attempt": step.attempt, - "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, - }, - ) - if failures: - raise ValueError(",".join(failures)) - - 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 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 - 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 = [ - 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( - self.raw_path, - { - "kind": "automatic_rescan", - "task_name": step.task_key, - "cycle": step.cycle, - "direction": step.direction, - "failed_attempt": step.attempt, - "next_attempt": attempt, - "reason": reason, - }, - ) - 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) - except ValueError as error: - if not self._retry_step(step, str(error)): - self._pause(f"sweep_quality_failed:{step.task_key}:{error}") - return - self.step_index += 1 - self.step_command_sent = False - 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 - state_publisher_count = self.count_publishers(self.state_topic) - if state_publisher_count > 1: - self._pause( - f"multiple_state_publishers:count={state_publisher_count}" - ) - return - command_publisher_count = self.count_publishers(self.command_topic) - if command_publisher_count > 1: - self._pause( - f"multiple_command_publishers:count={command_publisher_count}" - ) - return - if not self.started: - if len(self.latest_state_u8) == self.command_count and set(self.camera_matrices) == set( - self.profile.vision.view_names - ): - self.state = "READY" - self.reason = "ready_for_operator_start" - return - step = self._current_step() - if step is None: - return - if not self.step_command_sent: - self._begin_step(step) - return - now = time.monotonic() - self._advance_step_trajectory(step, now) - # 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]) - for index, value in enumerate(self.latest_state_u8) - ] - actual = ( - max(endpoint_errors) - if step.command_index is None - else endpoint_errors[step.command_index] - ) - progress_distance = ( - float(sum(endpoint_errors)) - if step.command_index is None - else float(endpoint_errors[step.command_index]) - ) - feedback = ( - float(np.mean(self.latest_state_u8)) - if step.command_index is None - else float(self.latest_state_u8[step.command_index]) - ) - self.step_last_feedback = feedback - # Baseline recovery moves all six channels at once. Using their mean - # as the progress signal divides a one-count move by six and can turn - # slow, valid O6 motion into a false two-second stall. Distance to the - # final target also rejects movement in the wrong direction. - if ( - not math.isfinite(self.step_last_distance_u8) - or progress_distance <= self.step_last_distance_u8 - ( - 0.5 if self.command_unit == "u8" else 0.002 - ) - ): - self.step_last_distance_u8 = progress_distance - self.step_last_progress_at = now - if ( - actual > float(self.endpoint_tolerance_u8) - and now - self.step_started_at > 1.0 - and now - self.step_last_progress_at > float(self.motor_stall_timeout_seconds) - ): - self._pause(f"mechanical_stall:{self.reason}") - return - if actual > float(self.endpoint_tolerance_u8): - self.step_hold_since = None - return - if self.step_trajectory_phase < 1.0: - self.step_hold_since = None - 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 _finalize(self) -> None: - try: - payload, _fit, correction = self.finalize_session( - session_dir=self.session_dir, - serial_number=self.serial_number, - source_urdf=self.source_urdf, - protected_inputs=self.protected_inputs, - records=self.raw_records, - publish=True, - ) - except BaseException as error: - atomic_write_json( - self.session_dir / "failure_diagnostic.json", - { - "profile_id": self.profile.key.profile_id, - "reason": str(error), - "traceback": traceback.format_exc(), - "source_urdf_unchanged": True, - }, - ) - self._pause(f"fit_or_publication_failed:{error}") - return - self._publish_command(list(self.baseline_command)) - self.state = "PASSED" - self.reason = ( - "partial_calibration_passed" - if self.profile.scope.default_scope != "full" - else "full_calibration_passed" - ) - self.final_json = str( - self.session_dir - / self.profile.artifacts.calibration_filename.format( - serial_number=self.serial_number - ) - ) - self.final_urdf = str(correction.path) - self.final_quality = payload["quality"] - - def _pause(self, reason: str) -> None: - if self.state in {"PAUSED", "ABORTED", "PASSED"}: - return - self.state = "PAUSED" - self.reason = str(reason) - # 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)) - if self.command_unit == "u8" else float(value) - for value in self.latest_state_u8 - ] - ) - 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() - feedback = float("nan") - if len(self.latest_state_u8) == self.command_count: - feedback = ( - float(np.mean(self.latest_state_u8)) - if step is None or step.command_index is None - else float(self.latest_state_u8[step.command_index]) - ) - target_state: list[float] = [] - channel_errors: list[float] = [] - if step is not None: - target_state = list(self._target_command(step)) - if len(self.latest_state_u8) == self.command_count: - channel_errors = [ - abs(value - target_state[index]) - for index, value in enumerate(self.latest_state_u8) - ] - step_fraction = 0.0 - if step is not None and math.isfinite(feedback): - 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 - observation = self._step_observation_metrics() - return { - "profile_id": self.profile.key.profile_id, - "state": self.state, - "reason": self.reason, - "serial_number": self.serial_number, - "session_dir": str(self.session_dir), - "step_index": self.step_index, - "step_count": len(self.steps), - "task_name": None if step is None else step.task_key, - "phase": None if step is None else step.phase, - "cycle": None if step is None else step.cycle, - "direction": None if step is None else step.direction, - "attempt": None if step is None else step.attempt, - "motor_index": None if step is None else step.command_index, - "command_unit": self.command_unit, - "speed_u8": ( - None if step is None or self.command_unit != "u8" else int(step.speed_u8) - ), - "speed_rad_s": ( - None if step is None or self.command_unit != "rad" else round(float(step.speed_u8), 6) - ), - "target_u8": ( - None if step is None or self.command_unit != "u8" else step.target_u8 - ), - "target_rad": ( - None if step is None or self.command_unit != "rad" else round(float(step.target_u8), 9) - ), - "current_command_u8": ( - None - if step is None or not math.isfinite(self.step_requested_u8) - else round(self.step_requested_u8, 3) - ), - "actual_u8": None if not math.isfinite(feedback) else round(feedback, 3), - "step_fraction": round(step_fraction, 6), - "valid_frames": self.step_valid_frames, - "total_frames": self.step_total_frames, - "tag_detection_rate": round( - float(observation["tag_detection_rate"]), 6 - ), - "tag_detection_rate_by_role": { - role: round(float(rate), 6) - for role, rate in observation["tag_detection_rate_by_role"].items() - }, - "tag_seen_rate_by_role": { - role: round(float(rate), 6) - for role, rate in observation["tag_seen_rate_by_role"].items() - }, - "recognized_tag_ids": list(self.latest_recognized_tag_ids), - "unrecognized_tag_ids": list(self.latest_unrecognized_tag_ids), - "all_tags_quality_rate": round( - float(observation["all_tags_quality_rate"]), 6 - ), - "pnp_valid_rate": round(float(observation["pnp_valid_rate"]), 6), - "state_sync_rate": round(float(observation["state_sync_rate"]), 6), - "joint_frame_rate": round(float(observation["joint_frame_rate"]), 6), - "observation_rejection_counts": observation["rejection_counts"], - "preflight_speed_u8": int(self.preflight_speed_u8), - "baseline_speed_u8": int(self.baseline_speed_u8), - "formal_speed_u8": int(self.formal_speed_u8), - "command_trajectory_full_range_seconds": float( - self.command_trajectory_full_range_seconds - ), - "command_trajectory_phase": round(self.step_trajectory_phase, 6), - "command_trajectory_duration_seconds": round( - self.step_trajectory_duration_seconds, 6 - ), - "base_corner_drift_px": { - view: round(value, 4) - for view, value in self.latest_base_drift_px.items() - }, - "base_corner_drift_limit_px": float( - 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), - "command_names": list(self.command_names), - f"latest_state_{self.command_unit}": list(self.latest_state_u8), - f"target_state_{self.command_unit}": [round(value, 6) for value in target_state], - f"current_command_state_{self.command_unit}": ( - [] - if self.step_last_command_u8 is None - else list(self.step_last_command_u8) - ), - f"channel_errors_{self.command_unit}": [round(value, 6) for value in channel_errors], - "maximum_error_channel": ( - None - if not channel_errors - else self.command_names[int(np.argmax(channel_errors))] - ), - f"maximum_error_{self.command_unit}": ( - None if not channel_errors else round(max(channel_errors), 3) - ), - "final_json": getattr(self, "final_json", ""), - "final_urdf": getattr(self, "final_urdf", ""), - "quality": getattr(self, "final_quality", {}), - } - - def _publish_status(self) -> None: - message = String() - message.data = json.dumps(self._status(), ensure_ascii=False) - self.status_publisher.publish(message) - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - node: L6ThreeCameraCalibrationNode | None = None - try: - node = L6ThreeCameraCalibrationNode() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -__all__ = ["L6ThreeCameraCalibrationNode", "MotionStep", "main"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py deleted file mode 100644 index e66d348..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/pipeline.py +++ /dev/null @@ -1,295 +0,0 @@ -"""Shared online/offline finalization path for one L6 partial session.""" - -from __future__ import annotations - -from datetime import datetime -import json -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, - build_l6_runtime_payload, - build_l6_urdf_input_payload, - load_l6_urdf_input, - publish_partial_session, -) -from .fitting import L6FitResult, fit_l6_session -from .profile import ( - CALIBRATED_ACTIVE_JOINTS, - CORRECTED_PASSIVE_JOINTS, - MEASURED_PASSIVE_JOINTS, - TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, - TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, - build_typed_profile, -) -from .urdf import L6UrdfCorrection, write_l6_corrected_urdf - - -MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS -ENDPOINT_SNAP_TOLERANCE_U8 = 2 - - -def canonical_feedback_command_u8(feedback_u8: float) -> int: - """Map a reached L6 feedback endpoint onto the curve's 0/255 domain.""" - value = int(round(float(feedback_u8))) - if value <= ENDPOINT_SNAP_TOLERANCE_U8: - return 0 - if value >= 255 - ENDPOINT_SNAP_TOLERANCE_U8: - return 255 - return value - - -def accepted_records_by_joint( - records: Sequence[Mapping[str, Any]], -) -> dict[str, list[dict[str, Any]]]: - """Select the newest attempt for every task/cycle/direction.""" - samples = [ - dict(row) - for row in records - if row.get("kind") == "l6_joint_sample" - and str(row.get("joint", "")) in MEASURED_JOINTS - ] - latest_attempt: dict[tuple[str, int, str], int] = {} - for row in samples: - key = ( - str(row["task_name"]), - int(row["cycle"]), - str(row["direction"]), - ) - latest_attempt[key] = max( - latest_attempt.get(key, 0), int(row.get("attempt", 1)) - ) - result = {name: [] for name in MEASURED_JOINTS} - for row in samples: - key = ( - str(row["task_name"]), - int(row["cycle"]), - str(row["direction"]), - ) - if int(row.get("attempt", 1)) != latest_attempt[key]: - continue - accepted = { - "cycle": int(row["cycle"]), - "direction": str(row["direction"]), - # The reusable fitter calls the independent variable - # command_u8; schema v6 deliberately supplies measured SDK - # feedback here, never the requested controller set-point. - "command_u8": canonical_feedback_command_u8( - float(row["feedback_u8"]) - ), - "feedback_u8": float(row["feedback_u8"]), - "relative_quaternion_xyzw": list( - row["relative_quaternion_xyzw"] - ), - } - # Schema-v6.1 adds the G20-compatible pose trajectory required for - # absolute thumb CMC zero recovery. Keep the projection here so the - # online and offline finalizers consume byte-equivalent fitting rows. - geometric_fields = ( - "relative_translation_xyz_m", - "parent_pose_common", - "child_pose_common", - "view_normal_common_xyz", - "camera_center_common_xyz_m", - "state_u8", - ) - if any(field in row for field in geometric_fields): - missing = [field for field in geometric_fields if field not in row] - if missing: - raise ValueError( - "L6 geometric sample is incomplete: " + ",".join(missing) - ) - for field in geometric_fields: - value = row[field] - accepted[field] = ( - dict(value) if isinstance(value, Mapping) else list(value) - ) - result[str(row["joint"])].append(accepted) - return result - - -def load_l6_raw_samples(path: str | Path) -> list[dict[str, Any]]: - source = Path(path).expanduser().resolve() - if not source.is_file(): - raise ValueError(f"raw L6 sample file does not exist: {source}") - rows: list[dict[str, Any]] = [] - with source.open("r", encoding="utf-8") as stream: - for line_number, line in enumerate(stream, 1): - if not line.strip(): - continue - try: - value = json.loads(line) - except json.JSONDecodeError as error: - raise ValueError( - f"invalid L6 JSONL record at line {line_number}" - ) from error - if not isinstance(value, Mapping): - raise ValueError(f"L6 JSONL line {line_number} is not an object") - rows.append(dict(value)) - return rows - - -def finalize_l6_session( - *, - session_dir: str | Path, - serial_number: str, - source_urdf: str | Path, - protected_inputs: Mapping[str, str], - records: Sequence[Mapping[str, Any]], - publish: bool = True, - timestamp: str | None = None, -) -> tuple[dict[str, Any], L6FitResult, L6UrdfCorrection]: - directory = Path(session_dir).expanduser().resolve() - directory.mkdir(parents=True, exist_ok=True) - result = fit_l6_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, - }, - ) - # 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. - payload = build_l6_runtime_payload( - serial_number=serial_number, - source_urdf=source_urdf, - result=result, - protected_inputs=protected_inputs, - passed=True, - ) - urdf_input_path = ( - directory / f"l6_right_{serial_number}_urdf_correction_input.json" - ) - atomic_write_json( - urdf_input_path, - build_l6_urdf_input_payload( - serial_number=serial_number, - source_urdf=source_urdf, - result=result, - ), - ) - urdf_result = load_l6_urdf_input( - urdf_input_path, - source_urdf=source_urdf, - serial_number=serial_number, - ) - stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") - correction = write_l6_corrected_urdf( - source_urdf=source_urdf, - output_directory=directory, - serial_number=serial_number, - result=urdf_result, - timestamp=stamp, - ) - json_path = directory / f"l6_right_{serial_number}_partial_calibration.json" - atomic_write_json(json_path, payload) - summary = { - "schema_version": 1, - "profile_id": "L6/right/l6_right_8/v1", - "serial_number": str(serial_number), - "result": "PARTIAL_PASS", - "publication_pointer": "latest_partial_passed", - "calibrated_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS), - "active_zero_methods": dict(sorted(result.zero_method_by_joint.items())), - "active_zero_fallback_reasons": dict( - sorted(result.zero_fallback_reason_by_joint.items()) - ), - "thumb_axis_zero": ( - None - if result.thumb_zero_result is None - else { - "offsets_rad": { - name: round(float(value), 10) - for name, value in sorted( - result.thumb_zero_result.direct_offsets_rad.items() - ) - }, - "axis_line_rms_m": round( - float(result.thumb_zero_result.axis_line_rms_m), 10 - ), - "validation_error_by_joint_rad": { - name: round(float(value), 10) - for name, value in sorted( - result.thumb_zero_result.validation_error_by_joint_rad.items() - ) - }, - } - ), - "measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS), - "transferred_active_joints": dict( - sorted(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items()) - ), - "transferred_passive_joints": dict( - sorted(TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items()) - ), - "passive_coupling": { - name: { - "model": fit.model, - "transferred_from_joint": ( - donor if donor != name else None - ), - "coefficients": [ - round(float(value), 10) for value in fit.coefficients - ], - "urdf_mimic_enabled": True, - "urdf_mimic_multiplier": round( - float(fit.urdf_mimic_multiplier), 10 - ), - "urdf_mimic_policy": fit.urdf_mimic_policy, - "residual_p95_deg": round( - math.degrees(float(fit.residual_p95_rad)), 6 - ), - "residual_max_deg": round( - math.degrees(float(fit.residual_max_rad)), 6 - ), - "cycle_prediction_range_deg": round( - math.degrees( - float(fit.maximum_cycle_prediction_range_rad) - ), - 6, - ), - } - for name, donor in sorted( - { - target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get( - target, target - ) - for target in CORRECTED_PASSIVE_JOINTS - }.items() - ) - for fit in (result.mimic_fits[donor],) - }, - "explicit_runtime_joints": sorted( - correction.explicit_runtime_joints - ), - "artifacts": { - "json": json_path.name, - "urdf": correction.path.name, - **artifact_hashes(json_path, correction.path), - }, - } - atomic_write_json(directory / "calibration_summary_zh.json", summary) - if publish: - publish_partial_session(directory.parent, directory) - return payload, result, correction - - -__all__ = [ - "ENDPOINT_SNAP_TOLERANCE_U8", - "MEASURED_JOINTS", - "accepted_records_by_joint", - "canonical_feedback_command_u8", - "finalize_l6_session", - "load_l6_raw_samples", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py deleted file mode 100644 index 580acdd..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/profile.py +++ /dev/null @@ -1,391 +0,0 @@ -"""Reviewed partial-calibration profile for the right L6 eight-Tag rig.""" - -from __future__ import annotations - -from ...core import ( - ArtifactPolicy, - CalibrationProfile, - CommandLayout, - MeasurementPolicy, - MeasurementSpec, - MotionPolicy, - ProfileKey, - QualityPolicy, - ScopePolicy, - TagSpec, - TaskSpec, - ViewSpec, - VisionRigSpec, - ZeroSolvePolicy, -) -from ..registry import EngineBindings, RegisteredProfile -from .motion import ( - build_calibration_motion_command, - build_calibration_preparation_waypoints, - build_calibration_return_waypoints, -) - - -KEY = ProfileKey("L6", "right", "l6_right_8", 1) - -COMMAND_NAMES: tuple[str, ...] = ( - "thumb_cmc_pitch", - "thumb_cmc_roll", - "index_mcp_pitch", - "middle_mcp_pitch", - "ring_mcp_pitch", - "pinky_mcp_pitch", -) - -ACTIVE_JOINTS: tuple[str, ...] = ( - "rh_thumb_cmc_pitch", - "rh_thumb_cmc_roll", - "rh_index_mcp_pitch", - "rh_middle_mcp_pitch", - "rh_ring_mcp_pitch", - "rh_pinky_mcp_pitch", -) -PASSIVE_JOINTS: tuple[str, ...] = ( - "rh_thumb_dip", - "rh_index_dip", - "rh_middle_dip", - "rh_ring_dip", - "rh_pinky_dip", -) -CALIBRATED_ACTIVE_JOINTS = frozenset( - { - "rh_thumb_cmc_pitch", - "rh_thumb_cmc_roll", - "rh_pinky_mcp_pitch", - } -) -MEASURED_PASSIVE_JOINTS = frozenset( - {"rh_thumb_dip", "rh_pinky_dip"} -) - -# The four L6 fingers use the same six-channel mechanism. This profile has -# visual Tags only on the pinky, so the remaining three fingers deliberately -# inherit the pinky's measured travel, feedback curves, and passive coupling -# while retaining their own CAD frames and geometry. The shared MCP zero is -# anchored at the observed open endpoint; it is not inferred by forcing the -# measured closed travel back onto the shorter CAD upper limit. -TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { - "rh_index_mcp_pitch": "rh_pinky_mcp_pitch", - "rh_middle_mcp_pitch": "rh_pinky_mcp_pitch", - "rh_ring_mcp_pitch": "rh_pinky_mcp_pitch", -} -TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = { - "rh_index_dip": "rh_pinky_dip", - "rh_middle_dip": "rh_pinky_dip", - "rh_ring_dip": "rh_pinky_dip", -} -CORRECTED_ACTIVE_JOINTS = frozenset( - CALIBRATED_ACTIVE_JOINTS | TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.keys() -) -CORRECTED_PASSIVE_JOINTS = frozenset( - MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys() -) - -ENDPOINT_ANCHOR_BY_JOINT = { - # Thumb pitch normally uses serial-axis geometry. If that observation is - # incompatible with the source link geometry, align the centre of the - # measured physical range with the centre of the source CAD range. Real - # endpoint comparison showed lower_at_start slightly under-corrected and - # upper_at_end over-corrected this serial, so neither endpoint is an - # independently trustworthy absolute datum. - "rh_thumb_cmc_pitch": "cad_range_center", - # Feedback 255 is the repeatable open/lower endpoint. The measured travel - # is about 70.55 deg while the source CAD upper is 65 deg. Anchoring the - # closed endpoint therefore introduced a -5.55 deg offset into all four - # fingers and made intermediate pinch poses systematically under-flexed. - "rh_pinky_mcp_pitch": "lower_at_start", -} - -COMMAND_INDEX_BY_JOINT = { - "rh_thumb_cmc_pitch": 0, - "rh_thumb_cmc_roll": 1, - "rh_index_mcp_pitch": 2, - "rh_middle_mcp_pitch": 3, - "rh_ring_mcp_pitch": 4, - "rh_pinky_mcp_pitch": 5, -} - -MIMIC_SOURCE_BY_JOINT = { - "rh_thumb_dip": "rh_thumb_cmc_pitch", - "rh_index_dip": "rh_index_mcp_pitch", - "rh_middle_dip": "rh_middle_mcp_pitch", - "rh_ring_dip": "rh_ring_mcp_pitch", - "rh_pinky_dip": "rh_pinky_mcp_pitch", -} - -COUPLING_MODEL_BY_JOINT = { - "rh_thumb_dip": "linear_mimic", - # The L6 pinky transmission has a repeatable changing ratio over its - # travel. Keep the exact direction-aware lookup at runtime and use the - # quadratic centre relation only for MuJoCo's equality constraint. - "rh_pinky_dip": "quadratic_runtime", - "rh_index_dip": "quadratic_runtime", - "rh_middle_dip": "quadratic_runtime", - "rh_ring_dip": "quadratic_runtime", -} - - -def build_typed_profile() -> CalibrationProfile: - active = frozenset(ACTIVE_JOINTS) - passive = frozenset(PASSIVE_JOINTS) - frozen_active = active - CALIBRATED_ACTIVE_JOINTS - measurements = { - "rh_thumb_cmc_roll": MeasurementSpec( - "rh_thumb_cmc_roll", - "relative_rotation", - "top", - "top_base", - "thumb_roll", - ), - "rh_thumb_cmc_pitch": MeasurementSpec( - "rh_thumb_cmc_pitch", - "relative_rotation", - "front", - "front_base", - "thumb_pitch", - ), - "rh_thumb_dip": MeasurementSpec( - "rh_thumb_dip", - "relative_rotation", - "front", - "thumb_pitch", - "thumb_dip", - ), - "rh_pinky_mcp_pitch": MeasurementSpec( - "rh_pinky_mcp_pitch", - "relative_rotation", - "side", - "side_base", - "pinky_pitch", - ), - "rh_pinky_dip": MeasurementSpec( - "rh_pinky_dip", - "relative_rotation", - "side", - "pinky_pitch", - "pinky_dip", - ), - } - tasks = ( - TaskSpec( - "thumb_roll_top", - "top", - 1, - ("rh_thumb_cmc_roll",), - auxiliary_commands=((0, 255),), - preflight_speed_u8=1, - formal_speed_u8=1, - ), - TaskSpec( - "thumb_pitch_dip_front", - "front", - 0, - ("rh_thumb_cmc_pitch", "rh_thumb_dip"), - auxiliary_commands=((1, 255),), - preflight_speed_u8=1, - formal_speed_u8=1, - ), - TaskSpec( - "pinky_pitch_dip_side", - "side", - 5, - ("rh_pinky_mcp_pitch", "rh_pinky_dip"), - preflight_speed_u8=1, - formal_speed_u8=1, - ), - ) - coverage = { - **{ - name: ( - "measured_static_dynamic" - if name in CALIBRATED_ACTIVE_JOINTS - else "transferred_static_dynamic" - if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT - else "cad_nominal" - ) - for name in active - }, - **{ - name: ( - "measured_dynamic_cad_static" - if name in MEASURED_PASSIVE_JOINTS - else "transferred_dynamic_cad_static" - if name in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT - else "mimic_nominal" - ) - for name in passive - }, - } - return CalibrationProfile( - key=KEY, - namespace="/l6_calibration", - command=CommandLayout( - names=COMMAND_NAMES, - baseline_u8=(255,) * 6, - command_index_by_joint=COMMAND_INDEX_BY_JOINT, - urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS}, - feedback_name_aliases={"thumb_cmc_yaw": "thumb_cmc_roll"}, - speed_slot_by_command_index={index: index for index in range(6)}, - ), - vision=VisionRigSpec( - views=( - ViewSpec( - "front", - ( - TagSpec("front_base", 0, fixed_reference=True), - TagSpec("thumb_pitch", 1), - TagSpec("thumb_dip", 2), - ), - ), - ViewSpec( - "side", - ( - TagSpec("side_base", 3, fixed_reference=True), - TagSpec("pinky_pitch", 4), - TagSpec("pinky_dip", 5), - ), - ), - ViewSpec( - "top", - ( - TagSpec("top_base", 6, fixed_reference=True), - TagSpec("thumb_roll", 7), - ), - ), - ), - common_frame="calibration_common", - extrinsic_reference_view="front", - extrinsics_quality_limits={ - "reprojection_rms_px": 1.2, - "maximum_rotation_repeatability_deg": 0.3, - "maximum_translation_repeatability_m": 0.0015, - }, - minimum_capture_counts={ - "front_side_captures": 15, - "front_top_captures": 15, - }, - ), - motion=MotionPolicy( - tasks=tasks, - precheck_sweeps=False, - steady_command_checkpoints=False, - speed_parameters={ - "preflight_u8": 1, - "formal_u8": 1, - "speed_settle_seconds": 0.2, - "command_trajectory_full_range_seconds": 6.0, - "torque_u8": 80, - "endpoint_hold_seconds": 1.0, - "stall_timeout_seconds": 2.0, - }, - ), - measurement=MeasurementPolicy( - measurements=measurements, - directional_zero=True, - ), - 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(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: ENDPOINT_ANCHOR_BY_JOINT[name] - for name in ENDPOINT_ANCHOR_BY_JOINT - }, - fitted_mimic_joints=MEASURED_PASSIVE_JOINTS, - coupling_model_by_joint=COUPLING_MODEL_BY_JOINT, - ), - quality=QualityPolicy( - training_cycles=(0, 1, 2), - holdout_cycle=3, - hard_threshold_keys=frozenset( - { - "minimum_detection_rate", - "maximum_state_image_skew_ms", - "maximum_validation_error_rad", - "maximum_mimic_residual_rad", - } - ), - isolated_holdout=True, - ), - scope=ScopePolicy( - calibrate_joints={"partial": CALIBRATED_ACTIVE_JOINTS}, - frozen_joints={"partial": frozen_active}, - default_scope="partial", - ), - artifacts=ArtifactPolicy( - output_schema_version=6, - calibration_filename=( - "l6_right_{serial_number}_partial_calibration.json" - ), - corrected_urdf_filename=( - "linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf" - ), - protected_input_fields=frozenset( - { - "source_urdf_sha256", - "camera_extrinsics_sha256", - "calibration_config_sha256", - "tag_config_sha256", - } - ), - publication_pointer="latest_partial_passed", - session_compatibility_tokens=frozenset( - {"l6_partial_v1", "feedback_curves_v6"} - ), - publish_corrected_urdf=True, - ), - joint_coverage=coverage, - ) - - -def _run_cli(args: list[str] | None = None) -> None: - from .runner import main - - main(args) - - -def _run_node(args: list[str] | None = None) -> None: - from .node import main - - main(args) - - -def build_profile() -> RegisteredProfile: - typed = build_typed_profile() - return RegisteredProfile( - profile=typed, - engine=EngineBindings( - hand_profile=typed, - zero_profile=typed.zero, - motion_command=build_calibration_motion_command, - preparation_waypoints=build_calibration_preparation_waypoints, - return_waypoints=build_calibration_return_waypoints, - cli_main=_run_cli, - node_main=_run_node, - ), - ) - - -__all__ = [ - "ACTIVE_JOINTS", - "CALIBRATED_ACTIVE_JOINTS", - "COMMAND_NAMES", - "KEY", - "MEASURED_PASSIVE_JOINTS", - "MIMIC_SOURCE_BY_JOINT", - "PASSIVE_JOINTS", - "build_profile", - "build_typed_profile", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py deleted file mode 100644 index 8fcda1e..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/runner.py +++ /dev/null @@ -1,716 +0,0 @@ -"""One-command online runner and deterministic offline replay for L6 right.""" - -from __future__ import annotations - -import argparse -from datetime import datetime -import hashlib -import json -import os -from pathlib import Path -import re -import signal -import subprocess -import sys -import time -from typing import Any, Callable, Mapping - -import rclpy -from rclpy.node import Node -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from ...operator_report import ( - ProgressEstimator, - render_compact_progress_header_zh, -) -from ...product import ProductConfig, load_product_config -from ...storage import atomic_write_json -from .pipeline import finalize_l6_session, load_l6_raw_samples - - -_STATE_LABELS = { - "WAIT_DEVICES": "等待六通道反馈和三相机内参", - "READY": "设备就绪", - "RUNNING": "标定中", - "FINALIZING": "拟合、验证并生成 URDF", - "PASSED": "通过", - "PAUSED": "已暂停", - "ABORTED": "已中止", -} -_TASK_LABELS = { - "thumb_roll_top": "拇指 CMC roll（上方机位，ID6→ID7）", - "thumb_pitch_dip_front": "拇指 CMC pitch / DIP（正面机位，ID0→ID1→ID2）", - "pinky_pitch_dip_side": "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）", -} -_PHASE_LABELS = { - "baseline": "安全恢复基准形态", - "preflight": "任务运动预检", - "prepare": "扫描起点准备", - "retry_prepare": "自动重扫起点准备", - "resume_prepare": "断点恢复起点准备", - "sweep": "正式扫描", - "clearance": "手指避让", - "clearance_outer": "小指/无名指避让", - "clearance_splay": "侧摆避让", - "return_splay_zero": "侧摆回零", - "return_middle_open": "展开中指", - "return_outer_open": "展开小指/无名指", -} -_DIRECTION_LABELS = { - "decreasing": "递减", - "increasing": "递增", -} - - -def _progress_fraction(status: Mapping[str, Any]) -> tuple[float, int, int]: - state = str(status.get("state", "")) - step_count = max(0, int(status.get("step_count", 0) or 0)) - step_index = int(status.get("step_index", -1) or 0) - step_fraction = float(status.get("step_fraction", 0.0) or 0.0) - if state == "PASSED": - overall = 1.0 - elif step_count and step_index >= 0: - overall = min(1.0, max(0.0, (step_index + step_fraction) / step_count)) - else: - overall = 0.0 - current_step = min(step_count, max(0, step_index + 1)) if step_count else 0 - return overall, current_step, step_count - - -def _duration_zh(seconds: float | None) -> str: - if seconds is None or seconds < 0.0: - return "计算中" - value = int(round(seconds)) - return f"{value // 60}分{value % 60:02d}秒" - - -def _l6_reason_zh( - status: Mapping[str, Any], *, model_name: str = "L6" -) -> tuple[str, str, str]: - reason = str(status.get("reason", "unknown")) - if reason.startswith("fixed_base_tag_moved:"): - fields = reason.split(":") - view = fields[1] if len(fields) > 1 else "unknown" - labels = {"front": "正面", "side": "侧面", "top": "上方"} - match = re.search(r"drift_px=([0-9.]+)", reason) - drift = match.group(1) if match else "未知" - limit = float(status.get("base_corner_drift_limit_px", 2.0) or 2.0) - return ( - "OBS-BASE-DRIFT-105", - f"{labels.get(view, view)}机位的掌心固定基准 Tag 相对本方向扫描前" - f"锁定位置连续漂移，最大角点位移 {drift} px，超过 {limit:g} px。", - "检查手掌支架、相机和掌心基准 Tag 是否松动或被碰触；固定后重新开始。" - "程序已禁止发布本次结果。", - ) - if reason.startswith("sweep_quality_failed:"): - fields = reason.split(":", 2) - details = fields[2] if len(fields) > 2 else "未提供明细" - return ( - "OBS-SWEEP-QUALITY-104", - "当前方向经过自动重扫后仍未满足采集门限；具体未通过项：" - f"{details}。", - "查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate；先处理遮挡或" - "反馈采样问题，再重新开始。程序已禁止发布本次结果。", - ) - if reason.startswith("multiple_state_publishers:"): - count = status.get("state_publisher_count", "?") - return ( - "DEVICE-DUPLICATE-SDK-203", - f"检测到 {count} 个 {model_name} 状态发布者；这通常表示已有 SDK/GUI 未退出。", - "先停止单独启动的 linker_hand_sdk 和 GUI，只保留本标定命令自动拉起的 SDK，" - "再重新开始。程序已禁止同时控制同一只手。", - ) - if reason.startswith("multiple_command_publishers:"): - count = status.get("command_publisher_count", "?") - return ( - "DEVICE-COMMAND-CONFLICT-204", - f"检测到 {count} 个 {model_name} 控制命令发布者，标定节点之外还有程序在控制手。", - "停止 GUI、手动控制节点或其他标定进程，只保留当前标定命令后重新开始。", - ) - if reason.startswith("mechanical_stall:"): - return ( - "MOTION-STALL-303", - "目标电机连续两秒没有向目标推进，程序已保持当前位置。", - "检查碰撞、摩擦和机械端点；不要连续重启强推。", - ) - if reason.startswith("motion_timeout:"): - return ( - "MOTION-TIMEOUT-302", - "当前运动在规定时间内没有到达目标位置。", - "检查 CAN 反馈、电机状态和机械阻挡后重新开始。", - ) - if reason.startswith("fit_or_publication_failed:"): - if ( - "mimic_residual_exceeds:" in reason - or "coupling_residual_exceeds:" in reason - ): - joint_match = re.search(r"joint=([^:]+)", reason) - model_match = re.search(r"model=([^:]+)", reason) - multiplier_match = re.search( - r"(?:multiplier|linear_term)=([0-9.]+)", reason - ) - p95_match = re.search(r"p95_deg=([0-9.]+)", reason) - maximum_match = re.search(r"maximum_deg=([0-9.]+)", reason) - joint = joint_match.group(1) if joint_match else "未知关节" - model = model_match.group(1) if model_match else "linear_mimic" - multiplier = multiplier_match.group(1) if multiplier_match else "未知" - p95 = p95_match.group(1) if p95_match else "未知" - maximum = maximum_match.group(1) if maximum_match else "未知" - return ( - "FIT-MIMIC-503", - f"{joint} 的 {model} 耦合模型未达到精度门限：" - f"线性项 {multiplier}，" - f"残差 P95={p95}°、最大={maximum}°。", - "原始视觉曲线已保留；不要放宽门限或发布错误 URDF。请检查 Tag 刚性、" - "遮挡与机械重复性后重新采集。", - ) - detail = reason.split(":", 1)[1] if ":" in reason else "未知" - return ( - "FIT-PUBLISH-501", - f"采集完成后的拟合、质量验证或 URDF 安全写回失败：{detail}。", - "保留本会话，不要修改源 URDF；复制下方诊断块给开发者。", - ) - if reason.startswith("operator_abort"): - return "OPERATOR-ABORT-001", "操作员主动中止了本次标定。", "排除现场问题后重新开始。" - return ( - f"{model_name}-CAL-500", - f"{model_name} 标定因未分类保护条件停止。", - "保留会话目录和运行日志，并复制下方诊断块给开发者。", - ) - - -def render_six_channel_progress_zh( - status: Mapping[str, Any], - *, - task_labels: Mapping[str, str], - reason_renderer: Callable[ - [Mapping[str, Any]], tuple[str, str, str] - ], - estimator: ProgressEstimator | None = None, -) -> str: - """Render one six-channel profile in the operator-oriented G20 layout.""" - state = str(status.get("state", "")) - overall, _current_step, _step_count = _progress_fraction(status) - task = status.get("task_name") - phase = status.get("phase") - if task: - task_text = task_labels.get(str(task), str(task)) - elif phase == "baseline": - task_text = "全手基准姿态" - elif state == "WAIT_DEVICES": - task_text = "等待设备连接" - elif state == "READY": - task_text = "等待开始" - else: - task_text = "无" - eta = _duration_zh(estimator.remaining(overall) if estimator else None) - cycle = status.get("cycle") - cycle_text = "-" if cycle is None else str(int(cycle) + 1) - direction = status.get("direction") - direction_text = _DIRECTION_LABELS.get(str(direction), "-") - command_unit = str(status.get("command_unit", "u8")) - target = status.get("target_rad") if command_unit == "rad" else status.get("target_u8") - requested = status.get("current_command_u8", target) - actual = status.get("actual_u8") - if command_unit == "rad": - requested = "未知" if requested is None else f"{float(requested):.3f} rad" - actual_text = "未知" if actual is None else f"{float(actual):.3f} rad" - else: - actual_text = "未知" if actual is None else f"{float(actual):.1f}" - valid = int(status.get("valid_frames", 0) or 0) - total = int(status.get("total_frames", 0) or 0) - rate = float(status.get("tag_detection_rate", 0.0) or 0.0) - joint_rate = float( - status.get( - "joint_frame_rate", - (float(valid) / total) if total else 0.0, - ) - or 0.0 - ) - recognized = [int(value) for value in status.get("recognized_tag_ids", [])] - unrecognized = [ - int(value) for value in status.get("unrecognized_tag_ids", []) - ] - recognized_text = "/".join(f"ID{value}" for value in recognized) or "无" - unrecognized_text = "/".join(f"ID{value}" for value in unrecognized) or "无" - attempt = int(status.get("attempt", 1) or 1) - lines = render_compact_progress_header_zh( - serial_number=str(status.get("serial_number", "?")), - progress=overall, - eta=eta, - stage=_PHASE_LABELS.get(str(phase), _STATE_LABELS.get(state, state)), - cycle=cycle_text, - repetitions=4, - task=task_text, - requested=requested, - actual=actual_text, - direction=direction_text, - tag_status=( - f"已识别 {recognized_text}；未识别/不合格 {unrecognized_text}；" - f"本方向各Tag最低 {rate:.1%}；联合 {valid}/{total} 帧" - f"（{joint_rate:.1%}）" - ), - ready_cameras=3, - feedback_hz=float(status.get("feedback_hz", 0.0) or 0.0), - valid_frames=valid, - automatic_retry_count=max(0, attempt - 1), - ) - speed_u8 = status.get("speed_u8") - if speed_u8 is not None: - trajectory_seconds = float( - status.get("command_trajectory_full_range_seconds", 0.0) or 0.0 - ) - lines.append( - f"运动：速度档 {int(speed_u8)}；全行程 {trajectory_seconds:.1f} 秒余弦轨迹" - ) - speed_rad_s = status.get("speed_rad_s") - if speed_rad_s is not None: - trajectory_seconds = float( - status.get("command_trajectory_duration_seconds", 0.0) or 0.0 - ) - lines.append( - f"运动：峰值 {float(speed_rad_s):.3f} rad/s；" - f"本段 {trajectory_seconds:.1f} 秒平滑限速轨迹" - ) - 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) - 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):.{digits}f}{suffix}" - for name, value in zip(command_names, latest_state) - ) - maximum_error_channel = status.get("maximum_error_channel") - 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}=" - 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}" - ) - if state in {"PAUSED", "ABORTED"}: - _code, problem, suggestion = reason_renderer(status) - lines.extend((f"原因：{problem}", f"建议：{suggestion}")) - return "\n".join(lines) - - -def render_l6_progress_zh( - status: Mapping[str, Any], - estimator: ProgressEstimator | None = None, -) -> str: - """Render L6 progress in the same operator-oriented layout as G20.""" - return render_six_channel_progress_zh( - status, - task_labels=_TASK_LABELS, - reason_renderer=_l6_reason_zh, - estimator=estimator, - ) - - -class _ProgressConsole: - def __init__( - self, - renderer: Callable[ - [Mapping[str, Any], ProgressEstimator | None], str - ] = render_l6_progress_zh, - ) -> None: - self.last_text = "" - self.estimator = ProgressEstimator.start() - self.renderer = renderer - - def update(self, status: Mapping[str, Any]) -> None: - text = self.renderer(status, self.estimator) - if text == self.last_text: - return - self.last_text = text - if sys.stdout.isatty(): - sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") - sys.stdout.flush() - else: - print(text, flush=True) - - -class _Monitor(Node): - def __init__(self, progress: _ProgressConsole) -> None: - super().__init__("l6_calibration_runner") - self.status: dict[str, Any] = {} - self.progress = progress - self.create_subscription( - String, "/l6_calibration/status", self._status_callback, 10 - ) - self.start_client = self.create_client(Trigger, "/l6_calibration/start") - self.abort_client = self.create_client(Trigger, "/l6_calibration/abort") - - def _status_callback(self, message: String) -> None: - try: - value = json.loads(message.data) - except json.JSONDecodeError: - return - if isinstance(value, dict): - self.status = value - self.progress.update(value) - - -def _launch_command( - config: ProductConfig, - session: Path, - *, - record_bag: bool, - commands_enabled: bool, - sdk_startup_speed_u8: int = 1, - resume_from: Path | None = None, -) -> list[str]: - arguments = { - "model": config.model, - "hand_type": config.side, - "tag_layout": config.tag_layout, - "serial_number": config.serial_number, - "source_urdf_path": str(config.source_urdf), - "source_urdf_expected_sha256": config.source_urdf_sha256, - "camera_extrinsics_file": str(config.camera_extrinsics), - "camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256, - "calibration_config": str(config.calibration_config), - "calibration_config_expected_sha256": config.calibration_config_sha256, - "tag_config": str(config.tag_config), - "tag_config_expected_sha256": config.tag_config_sha256, - "vendor_sdk_config": "" if config.sdk_config is None else str(config.sdk_config), - "sdk_config_expected_sha256": config.sdk_config_sha256, - "output_root": str(config.output_root), - "session_dir": str(session), - "corrected_urdf_output_dir": str(session), - "recalibration_scope": config.calibration_contract.typed_profile.scope.default_scope, - "calibration_speed": str(int(sdk_startup_speed_u8)), - "index_roll_calibration_speed": "1", - "index_flex_calibration_speed": "1", - "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. - if config.can_interface: - arguments["can_interface"] = config.can_interface - for view, camera in config.cameras.items(): - arguments[f"{view}_camera_serial"] = camera["serial_number"] - arguments[f"{view}_camera_name"] = camera["camera_name"] - arguments[f"{view}_camera_info_url"] = camera["camera_info"] - return [ - "ros2", "launch", "linkerhand_calibration", - "three_camera_calibration.launch.py", - *(f"{name}:={value}" for name, value in arguments.items()), - ] - - -def _wait_until( - monitor: _Monitor, - process: subprocess.Popen[Any], - predicate, - *, - timeout: float | None, -) -> bool: - 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 - if timeout is not None and time.monotonic() - started > timeout: - return False - return False - - -def _stop_stack(process: subprocess.Popen[Any]) -> None: - """Stop the whole launch process group without leaking child-node noise.""" - if process.poll() is not None: - return - try: - os.killpg(process.pid, signal.SIGINT) - except ProcessLookupError: - return - try: - process.wait(timeout=15.0) - except subprocess.TimeoutExpired: - try: - os.killpg(process.pid, signal.SIGTERM) - except ProcessLookupError: - return - try: - process.wait(timeout=5.0) - except subprocess.TimeoutExpired: - try: - os.killpg(process.pid, signal.SIGKILL) - except ProcessLookupError: - return - process.wait(timeout=5.0) - - -def _l6_failure_report( - config: ProductConfig, - session: Path, - status: Mapping[str, Any], - log_path: Path, -) -> str: - code, problem, suggestion = _l6_reason_zh(status) - task_key = status.get("task_name") - task = "无" if task_key is None else _TASK_LABELS.get(str(task_key), str(task_key)) - metrics = { - "task": task, - "cycle": status.get("cycle"), - "direction": status.get("direction"), - "valid_frames": status.get("valid_frames", 0), - "detection_frames": status.get("total_frames", 0), - "detection_rate": status.get("tag_detection_rate", 0.0), - "detection_rate_by_role": status.get("tag_detection_rate_by_role", {}), - "tag_seen_rate_by_role": status.get("tag_seen_rate_by_role", {}), - "recognized_tag_ids": status.get("recognized_tag_ids", []), - "unrecognized_tag_ids": status.get("unrecognized_tag_ids", []), - "joint_frame_rate": status.get("joint_frame_rate", 0.0), - "all_tags_quality_rate": status.get("all_tags_quality_rate", 0.0), - "pnp_valid_rate": status.get("pnp_valid_rate", 0.0), - "state_sync_rate": status.get("state_sync_rate", 0.0), - "observation_rejection_counts": status.get( - "observation_rejection_counts", {} - ), - "actual_u8": status.get("actual_u8"), - "current_command_u8": status.get("current_command_u8"), - "base_corner_drift_px": status.get("base_corner_drift_px", {}), - "state_publisher_count": status.get("state_publisher_count"), - "command_publisher_count": status.get("command_publisher_count"), - "failure_reason": status.get("reason"), - } - payload = { - "schema_version": 1, - "profile_id": status.get("profile_id"), - "serial_number": config.serial_number, - "result": "FAIL", - "error_code": code, - "stage": status.get("phase") - or ("fit" if str(status.get("reason", "")).startswith( - "fit_or_publication_failed:" - ) else status.get("state", "unknown")), - "reason": status.get("reason", "unknown"), - "problem_zh": problem, - "automatic_action_zh": "已停止运动、保持当前位置并禁止发布标定 JSON/URDF", - "suggestion_zh": suggestion, - "metrics": metrics, - "feedback_hz": status.get("feedback_hz", 0.0), - "hashes": { - "product_config_sha256": hashlib.sha256( - config.path.read_bytes() - ).hexdigest(), - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - "source_urdf_sha256": config.source_urdf_sha256, - }, - "session_dir": str(session), - "log_path": str(log_path), - "quality": {"passed": False}, - } - atomic_write_json(session / "calibration_summary_zh.json", payload) - return "\n".join( - [ - "========== 请复制以下内容给开发者 ==========", - f"会话编号：{config.serial_number}_{session.name}", - "结果：FAIL", - f"错误代码：{code}", - f"失败阶段：{payload['stage']}", - f"问题：{problem}", - f"自动处理：{payload['automatic_action_zh']}", - "关键指标：" - + json.dumps(metrics, ensure_ascii=False, separators=(",", ":")), - f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz", - f"配置哈希：{payload['hashes']['product_config_sha256']}", - f"外参哈希：{config.camera_extrinsics_sha256}", - f"源 URDF 哈希：{config.source_urdf_sha256}", - f"会话目录：{session}", - f"运行日志：{log_path}", - f"建议：{suggestion}", - "========== 复制结束 ==========", - ] - ) - - -def _run_online( - config: ProductConfig, - *, - record_bag: bool, - commands_enabled: bool, -) -> int: - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session = config.session_root / stamp - while session.exists(): - time.sleep(1.0) - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session = config.session_root / stamp - session.mkdir(parents=True) - command = _launch_command( - config, session, record_bag=record_bag, commands_enabled=commands_enabled - ) - log_path = session / "calibration.log" - log_stream = log_path.open("a", encoding="utf-8", buffering=1) - process = subprocess.Popen( - command, - cwd=config.workspace, - stdout=log_stream, - stderr=subprocess.STDOUT, - text=True, - start_new_session=True, - ) - rclpy.init() - monitor = _Monitor(_ProgressConsole()) - try: - ready = _wait_until( - monitor, - process, - lambda status: status.get("state") - in {"READY", "PAUSED", "ABORTED"}, - timeout=120.0, - ) - if ready and monitor.status.get("state") in {"PAUSED", "ABORTED"}: - print( - _l6_failure_report(config, session, monitor.status, log_path), - flush=True, - ) - return 3 - if not ready: - print("L6 启动失败：120秒内未收到六通道反馈和三相机内参。", flush=True) - return 2 - if not monitor.start_client.wait_for_service(timeout_sec=10.0): - print("L6 标定 /start 服务不可用。", flush=True) - return 2 - future = monitor.start_client.call_async(Trigger.Request()) - while rclpy.ok() and not future.done(): - rclpy.spin_once(monitor, timeout_sec=0.2) - response = future.result() - if response is None or not response.success: - print(f"L6 标定未启动：{getattr(response, 'message', '')}", flush=True) - return 2 - print( - "L6右手标定已启动：通道0/1/5，预检/正式速度1，" - "全行程6秒余弦缓入缓出，torque 80。", - flush=True, - ) - finished = _wait_until( - monitor, - process, - lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"}, - timeout=None, - ) - if not finished: - print("L6 标定进程意外退出。", flush=True) - return 2 - status = monitor.status - if status.get("state") != "PASSED": - print(_l6_failure_report(config, session, status, log_path), flush=True) - return 3 - print( - "\n".join( - [ - "PASS：L6右手三主动关节与两条DIP实测通过；" - "小指结果已迁移到食指、中指和无名指。", - f"部分结果：{config.session_root / 'latest_partial_passed'}", - f"JSON：{status.get('final_json')}", - f"URDF：{status.get('final_urdf')}", - ] - ), - flush=True, - ) - return 0 - except KeyboardInterrupt: - if monitor.abort_client.wait_for_service(timeout_sec=2.0): - monitor.abort_client.call_async(Trigger.Request()) - rclpy.spin_once(monitor, timeout_sec=1.0) - return 130 - finally: - monitor.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - _stop_stack(process) - log_stream.flush() - os.fsync(log_stream.fileno()) - log_stream.close() - - -def main(args: list[str] | None = None) -> None: - parser = argparse.ArgumentParser( - description="L6 right partial three-camera calibration" - ) - parser.add_argument("--config", required=True) - parser.add_argument("--workspace", default=None) - parser.add_argument("--record-bag", action="store_true") - parser.add_argument("--commands-disabled", action="store_true") - parser.add_argument("--validate-only", action="store_true") - parser.add_argument("--offline-raw", default="") - parser.add_argument("--offline-output", default="") - parser.add_argument("--publish-offline", action="store_true") - selected = parser.parse_args(args) - config = load_product_config( - selected.config, - workspace=selected.workspace, - check_can=not bool(selected.validate_only or selected.offline_raw), - ) - if selected.validate_only: - print( - f"配置有效：{config.profile_key.profile_id}，源URDF " - f"{config.source_urdf_sha256}", - flush=True, - ) - return - if selected.offline_raw: - output = ( - Path(selected.offline_output).expanduser().resolve() - if selected.offline_output - else config.session_root - / (datetime.now().strftime("%Y%m%d_%H%M%S") + "_offline") - ) - output.mkdir(parents=True, exist_ok=False) - payload, _fit, correction = finalize_l6_session( - 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, - }, - records=load_l6_raw_samples(selected.offline_raw), - publish=selected.publish_offline, - ) - print( - f"离线回放PASS：schema {payload['schema_version']}，URDF {correction.path}", - flush=True, - ) - return - raise SystemExit( - _run_online( - config, - record_bag=selected.record_bag, - commands_enabled=not selected.commands_disabled, - ) - ) - - -__all__ = [ - "main", "render_l6_progress_zh", "render_six_channel_progress_zh" -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py deleted file mode 100644 index 26fbdf3..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/__init__.py +++ /dev/null @@ -1,11 +0,0 @@ -"""Registered O12 calibration profiles.""" - -from ..registry import ProfileRegistry - - -def register_profiles(registry: ProfileRegistry) -> None: - from .profile import build_profile - registry.register(build_profile()) - - -__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py deleted file mode 100644 index 6271f2d..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/fitting.py +++ /dev/null @@ -1,688 +0,0 @@ -"""Direction-aware fitting for O12 feedback expressed in radians.""" - -from __future__ import annotations - -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 ( - 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] - zero_offsets_rad: Mapping[str, float] - 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]: - return { - joint: task - for task in build_typed_profile().motion.tasks - for joint in task.joints - } - - -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)] - 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) - 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, - 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( - lower, - upper, - int(count), - ) - ) - - -def curve_values_at_rad( - 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, feedback_domain_rad), - indices, - values, - )) - for value in inputs_rad - ) - - -def _virtual_records( - 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"]), 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]) - + float(fit.increasing_rad[0] - fit.increasing_rad[255]) - ) - - -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: - # 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)) - extra = sorted(set(records_by_joint) - expected) - raise ValueError(f"O12 records differ from profile: missing={missing} extra={extra}") - 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_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", - require_observed_domain_endpoints=False, - zero_reference_maximum_distance_u8=None, - ) - 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) - or float(np.percentile(absolute, 95.0)) > math.radians(2.0) - or float(np.max(absolute)) > math.radians(3.0) - ): - raise ValueError(f"{joint} isolated holdout failed") - curves[joint] = fit - holdout[joint] = tuple(float(value) for value in errors) - cycle_curves[joint] = { - cycle: fit_rotation_joint_curve( - [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) - } - - mimic_fits: dict[str, MimicFit] = {} - for target in sorted(MEASURED_PASSIVE_JOINTS): - source = MIMIC_SOURCE_BY_JOINT[target] - cycle_pairs = [ - ( - tuple(cycle_curves[source][cycle].decreasing_rad) - + tuple(cycle_curves[source][cycle].increasing_rad), - tuple(cycle_curves[target][cycle].decreasing_rad) - + tuple(cycle_curves[target][cycle].increasing_rad), - ) - for cycle in (0, 1, 2) - ] - mimic_fits[target] = fit_coupling_model( - source, - target, - curves[source], - curves[target], - # 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: visual_arcs[joint] for joint in CALIBRATED_ACTIVE_JOINTS - } - for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): - travels[target] = travels[donor] - 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", "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/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py deleted file mode 100644 index 4f06d6f..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/motion.py +++ /dev/null @@ -1,133 +0,0 @@ -"""Physical-radian motion helpers for O12 right calibration.""" - -from __future__ import annotations - -import math -from typing import Sequence - -from ...core import CalibrationProfile, TaskSpec - - -def cosine_position_trajectory_rad( - start_rad: float, - target_rad: float, - elapsed_seconds: float, - maximum_speed_rad_s: float, -) -> tuple[float, float, float]: - """Cosine trajectory whose peak velocity does not exceed the limit.""" - speed = float(maximum_speed_rad_s) - if not math.isfinite(speed) or speed <= 0.0: - raise ValueError("maximum_speed_rad_s must be positive") - distance = abs(float(target_rad) - float(start_rad)) - if distance <= 0.0: - return float(target_rad), 1.0, 0.0 - duration = math.pi * distance / (2.0 * speed) - phase = min(1.0, max(0.0, float(elapsed_seconds) / duration)) - blend = 0.5 - 0.5 * math.cos(math.pi * phase) - return ( - float(start_rad) + (float(target_rad) - float(start_rad)) * blend, - phase, - duration, - ) - - -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, - *, - profile: CalibrationProfile, -) -> list[float]: - values = list(profile.command.baseline_values) - for index, value in task.auxiliary_commands: - values[int(index)] = float(value) - values[int(task.command_index)] = float(command_rad) - return values - - -def build_calibration_preparation_waypoints( - task: TaskSpec, - *, - profile: CalibrationProfile, - current_command: Sequence[float] | None = None, -) -> tuple[tuple[float, ...], ...]: - del current_command - return (tuple(build_calibration_motion_command(task, task.start_value, profile=profile)),) - - -def build_calibration_return_waypoints( - target_command: Sequence[float] | None = None, - *, - profile: CalibrationProfile, - current_command: Sequence[float] | None = None, - **_: object, -) -> tuple[tuple[float, ...], ...]: - del current_command - target = tuple( - profile.command.baseline_values if target_command is None else target_command - ) - if len(target) != profile.command.command_count: - raise ValueError("O12 return command has the wrong channel count") - return (tuple(float(value) for value in target),) - - -__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 deleted file mode 100644 index 217fd7f..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/node.py +++ /dev/null @@ -1,1758 +0,0 @@ -"""Safety-gated online acquisition node for O12 right.""" - -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 ...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_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(), - finalizer=finalize_o12_session, - sample_kind="o12_joint_sample", - ) - self.mode_verified = False - self.error_verified = False - self.temperature_verified = False - 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 - ) - self.error_query_publisher = self.create_publisher( - Empty, "/o12/right/joint_error_cmd", 10 - ) - self.temperature_query_publisher = self.create_publisher( - Empty, "/o12/right/joint_temperature_cmd", 10 - ) - self.create_subscription( - Int8MultiArray, - "/o12/right/joint_control_mode_states", - self._mode_callback, - 10, - ) - self.create_subscription( - Int16MultiArray, - "/o12/right/joint_error_states", - self._error_callback, - 10, - ) - self.create_subscription( - Int16MultiArray, - "/o12/right/joint_temperature_states", - self._temperature_callback, - 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() - self.maximum_temperature_c = int( - self.get_parameter("maximum_temperature_c").value - ) - self.temperature_report_required = bool( - self.get_parameter("temperature_report_required").value - ) - self.temperature_fallback_after_seconds = float( - self.get_parameter("temperature_fallback_after_seconds").value - ) - 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 <= 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 - - def _activate_temperature_fallback_if_allowed(self, now: float) -> None: - if ( - self.temperature_report_required - or self.temperature_verified - or self.temperature_fallback_active - or not self.error_verified - or now - self.health_check_started_at - < self.temperature_fallback_after_seconds - ): - return - self.temperature_fallback_active = True - append_jsonl(self.raw_path, { - "kind": "o12_temperature_capability_fallback", - "temperature_report_received": False, - "fallback_protection": "joint_error_states_bit1_overheat", - "sdk_config_sha256": self.protected_inputs["sdk_config_sha256"], - }) - self.get_logger().warning( - "O12 temperature report unavailable; continuing with error-code " - "bit1 overheat protection" - ) - - def _mode_callback(self, message: Int8MultiArray) -> None: - values = tuple(int(value) for value in message.data) - if len(values) != 12 or any(value != POSITION_MODE for value in values): - self._pause("control_mode_is_not_position") - return - self.mode_verified = True - - def _error_callback(self, message: Int16MultiArray) -> None: - 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") - 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: - # 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: - self.temperature_verified = True - self.temperature_fallback_active = False - - def _query_health(self) -> None: - if not self.commands_enabled: - return - mode = Int8MultiArray() - mode.data = [POSITION_MODE] * 12 - self.control_mode_publisher.publish(mode) - self.error_query_publisher.publish(Empty()) - 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, ...]: - index = { - "thumb_roll": 0, "thumb_abad": 1, "thumb_mcp": 2, - "thumb_pip": 3, "index_abad": 4, "index_mcp": 5, - "index_pip": 6, "middle_abad": 7, "middle_mcp": 8, - "middle_pip": 9, "ring_mcp": 10, "pinky_mcp": 11, - } - target = [0.0] * 12 - for name, value in values.items(): - target[index[name]] = float(value) - return tuple(target) - - def _build_steps(self) -> list[MotionStep]: - def scaled(speed: float, maximum: float | None = None) -> float: - value = float(speed) * self.motion_speed_scale - return value if maximum is None else min(value, maximum) - - steps: list[MotionStep] = [ - MotionStep( - "baseline", None, None, 0.0, scaled(0.10, 0.20), - target_command=self._full_target(), - ) - ] - previous_group = "thumb" - for task in self.profile.motion.tasks: - group = task.key.split("_", 1)[0] - if group != previous_group: - if group == "middle": - outer = self._full_target( - 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.20), target_command=outer, - required_endpoint_indices=(10, 11), - )) - target = self._full_target( - index_abad=INDEX_CLEARANCE_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.12), target_command=target, - required_endpoint_indices=(4, 5, 10, 11), - )) - elif group == "index": - splay_zero = self._full_target( - 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.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_RING_MCP_RAD, - pinky_mcp=PARK_PINKY_MCP_RAD, - ) - 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 - 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.06), - relative_feedback_delta=relative_delta, - )) - for cycle in (0, 1, 2, 3): - formal_speed = scaled( - float(task.formal_speed), - FORMAL_SPEED_CAP_RAD_S[int(task.command_index)], - ) - steps.extend(( - MotionStep("sweep", task.key, task.command_index, task.end_value, formal_speed, cycle, "decreasing"), - 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_RING_MCP_RAD, pinky_mcp=PARK_PINKY_MCP_RAD - ) - outer_parked = self._full_target( - 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.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() - and self._feedback_hz() >= float(self.minimum_feedback_hz) - ): - response.success = False - response.message = ( - "O12 POSITION/health/control-stream precheck is incomplete" - ) - return response - return super()._start(request, response) - - def _observe_preflight_pose(self, view, selected, step: MotionStep) -> None: - task = self._task(str(step.task_key)) - if task.view != view: - return - measurement = self.profile.measurement.measurements[task.joints[0]] - parent = Rotation.from_quat( - selected[str(measurement.parent_role)].quaternion_xyzw - ) - child = Rotation.from_quat( - selected[str(measurement.child_role)].quaternion_xyzw - ) - relative = parent.inv() * child - if abs(float(step.target_u8) - task.start_value) <= 1.0e-9: - self.preflight_reference_by_task.setdefault(task.key, relative) - return - reference = self.preflight_reference_by_task.get(task.key) - if reference is None: - return - vector = (reference.inv() * relative).as_rotvec() - magnitude = float(np.linalg.norm(vector)) - if magnitude > self.preflight_maximum_rotation_by_task.get(task.key, 0.0): - self.preflight_maximum_rotation_by_task[task.key] = magnitude - if magnitude > 1.0e-9: - axis = vector / magnitude - self.measured_direction_axis_by_task[task.key] = tuple( - 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 - 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 - or next_step.phase.startswith("clearance") - or (previous_task and next_step.task_key and previous_task.split("_", 1)[0] != next_step.task_key.split("_", 1)[0]) - ): - 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. 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 self._feedback_hz() >= float(self.minimum_feedback_hz) - ) - ): - self.state = "WAIT_DEVICES" - 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": ( - "direct_temperature_report" - if self.temperature_verified - else "error_bitmask_bit1_overheat" - if self.temperature_fallback_active - else "waiting_for_temperature_report" - ), - "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 - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - node: O12ThreeCameraCalibrationNode | None = None - try: - node = O12ThreeCameraCalibrationNode() - # 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: - if node is not None: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -__all__ = ["O12ThreeCameraCalibrationNode", "POSITION_MODE", "main"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py deleted file mode 100644 index 86f2bf1..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/pipeline.py +++ /dev/null @@ -1,336 +0,0 @@ -"""Shared online/offline O12 session finalization.""" - -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, - validate_o12_runtime_payload_against_urdf, -) -from .fitting import O12FitResult, fit_o12_session -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 - - -def accepted_records_by_joint( - records: Sequence[Mapping[str, Any]], -) -> dict[str, list[dict[str, Any]]]: - samples = [ - dict(row) for row in records - if row.get("kind") == "o12_joint_sample" - and str(row.get("joint", "")) in MEASURED_JOINTS - ] - 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 = {name: [] for name in MEASURED_JOINTS} - for row in samples: - key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"])) - if int(row.get("attempt", 1)) != latest[key]: - continue - accepted: dict[str, Any] = { - "cycle": int(row["cycle"]), - "direction": str(row["direction"]), - "feedback_rad": float(row["feedback_rad"]), - "relative_quaternion_xyzw": list(row["relative_quaternion_xyzw"]), - } - for field in ( - "relative_translation_xyz_m", "parent_pose_common", - "child_pose_common", "view_normal_common_xyz", - "camera_center_common_xyz_m", "state_rad", - ): - if field in row: - value = row[field] - accepted[field] = dict(value) if isinstance(value, Mapping) else list(value) - result[str(row["joint"])].append(accepted) - 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(): - raise ValueError(f"raw O12 sample file does not exist: {source}") - rows: list[dict[str, Any]] = [] - with source.open("r", encoding="utf-8") as stream: - for line_number, line in enumerate(stream, 1): - if not line.strip(): - continue - try: - item = json.loads(line) - except json.JSONDecodeError as error: - raise ValueError(f"invalid O12 JSONL record at line {line_number}") from error - if not isinstance(item, Mapping): - raise ValueError(f"O12 JSONL line {line_number} is not an object") - rows.append(dict(item)) - return rows - - -def _publish(serial_root: Path, session: Path) -> Path: - if session.parent != serial_root or not session.is_dir(): - raise ValueError("O12 session must be a direct existing child") - destination = serial_root / "latest_passed" - temporary = serial_root / f".latest_passed.{os.getpid()}.tmp" - if temporary.exists() or temporary.is_symlink(): - temporary.unlink() - os.symlink(session.name, temporary, target_is_directory=True) - os.replace(temporary, destination) - return destination - - -def finalize_o12_session( - *, - session_dir: str | Path, - serial_number: str, - source_urdf: str | Path, - protected_inputs: Mapping[str, str], - records: Sequence[Mapping[str, Any]], - publish: bool = True, - timestamp: str | None = None, -) -> tuple[dict[str, Any], O12FitResult, O12UrdfCorrection]: - directory = Path(session_dir).expanduser().resolve() - directory.mkdir(parents=True, exist_ok=True) - 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, - result=result, - 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, - serial_number=serial_number, - 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 = { - "schema_version": 1, - "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, - "calibration_json_sha256": sha256_file(json_path), - "corrected_urdf_sha256": sha256_file(correction.path), - }, - } - atomic_write_json(directory / "calibration_summary_zh.json", summary) - if publish: - _publish(directory.parent, directory) - return payload, result, correction - - -__all__ = [ - "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/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py deleted file mode 100644 index 4943466..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/profile.py +++ /dev/null @@ -1,517 +0,0 @@ -"""Reviewed O12 right-hand 16-Tag calibration profile.""" - -from __future__ import annotations - -from dataclasses import dataclass -import math - -from ...core import ( - AcquisitionPolicy, - ArtifactPolicy, - CalibrationProfile, - CommandLayout, - MeasurementPolicy, - MeasurementSpec, - MotionPolicy, - ProfileKey, - QualityPolicy, - ScopePolicy, - TagSpec, - TaskSpec, - ViewSpec, - VisionRigSpec, - ZeroSolvePolicy, -) -from ..registry import EngineBindings, RegisteredProfile -from .motion import ( - build_calibration_motion_command, - build_calibration_preparation_waypoints, - build_calibration_return_waypoints, -) - - -KEY = ProfileKey("O12", "right", "o12_right_16", 1) - -# sensor_msgs/JointState.position active-angle order from API_CPP_O12.md. -COMMAND_NAMES: tuple[str, ...] = ( - "thumb_roll", - "thumb_abad", - "thumb_mcp", - "thumb_pip", - "index_abad", - "index_mcp", - "index_pip", - "middle_abad", - "middle_mcp", - "middle_pip", - "ring_mcp", - "pinky_mcp", -) - -SDK_LOWER_RAD = ( - 0.0, -1.387536755335492, -0.8272860654453121, -1.2915436464758039, - -0.2617993877991494, 0.0, 0.0, -0.2617993877991494, - 0.0, 0.0, 0.0, 0.0, -) -SDK_UPPER_RAD = ( - 0.9424777960769379, 0.0, 0.0, 0.0, - 0.2617993877991494, 1.3526301702956054, 1.530653753999027, - 0.2617993877991494, 1.3578661580515883, 1.8151424220741028, - 1.53588974175501, 1.53588974175501, -) - -# 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 -) -FEEDBACK_UPPER_RAD = tuple( - value + FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_UPPER_RAD -) - -SDK_TO_URDF_JOINT: tuple[str, ...] = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "index_mcp_roll", - "index_mcp_pitch", - "index_pip", - "middle_mcp_roll", - "middle_mcp_pitch", - "middle_pip", - "ring_mcp_pitch", - "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", - "index_dip", - "middle_dip", - "ring_pip", - "ring_dip", - "pinky_pip", - "pinky_dip", -) -CALIBRATED_ACTIVE_JOINTS = frozenset(ACTIVE_JOINTS) - {"ring_mcp_pitch"} -MEASURED_PASSIVE_JOINTS = frozenset( - {"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"} -) -TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { - "ring_mcp_pitch": "pinky_mcp_pitch", -} -MIMIC_SOURCE_BY_JOINT = { - "thumb_dip": "thumb_mcp", - "index_dip": "index_pip", - "middle_dip": "middle_pip", - "ring_pip": "ring_mcp_pitch", - "ring_dip": "ring_pip", - "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) -} - -# 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( - key: str, - view: str, - index: int, - joints: tuple[str, ...], - *, - start: float, - end: float, - speed: float, - auxiliary: tuple[tuple[int, float], ...] = (), -) -> TaskSpec: - return TaskSpec( - key, - view, - index, - joints, - auxiliary_commands=auxiliary, - start=start, - end=end, - preflight_speed=min(speed, 0.02), - formal_speed=speed, - ) - - -def build_typed_profile() -> CalibrationProfile: - middle_clearance = ( - (4, INDEX_CLEARANCE_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_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), - _task("thumb_roll_front", "front", 0, ("thumb_cmc_roll",), start=0.0, end=SAFE_UPPER_RAD[0], speed=0.04), - _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_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_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), - ) - measurements = { - "thumb_cmc_pitch": MeasurementSpec("thumb_cmc_pitch", "relative_rotation", "front", "front_base", "thumb_cmc"), - "thumb_cmc_roll": MeasurementSpec("thumb_cmc_roll", "relative_rotation", "front", "front_base", "thumb_cmc"), - "thumb_mcp": MeasurementSpec("thumb_mcp", "relative_rotation", "front", "thumb_cmc", "thumb_mcp"), - "thumb_dip": MeasurementSpec("thumb_dip", "relative_rotation", "front", "thumb_mcp", "thumb_dip", pose_axis_line_required=False), - "thumb_cmc_yaw": MeasurementSpec("thumb_cmc_yaw", "relative_rotation", "top", "top_base", "thumb_yaw"), - "pinky_mcp_pitch": MeasurementSpec("pinky_mcp_pitch", "relative_rotation", "side", "side_base", "pinky_mcp"), - "pinky_pip": MeasurementSpec("pinky_pip", "relative_rotation", "side", "pinky_mcp", "pinky_pip", pose_axis_line_required=False), - "pinky_dip": MeasurementSpec("pinky_dip", "relative_rotation", "side", "pinky_pip", "pinky_dip", pose_axis_line_required=False), - "middle_mcp_roll": MeasurementSpec("middle_mcp_roll", "relative_rotation", "front", "front_base", "middle_roll"), - "middle_mcp_pitch": MeasurementSpec("middle_mcp_pitch", "relative_rotation", "side", "side_base", "middle_pip"), - "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_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), - } - active = frozenset(ACTIVE_JOINTS) - passive = frozenset(PASSIVE_JOINTS) - coverage = { - **{ - name: ( - "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 - }, - **{ - name: ( - "measured_dynamic_cad_static" - if name in MEASURED_PASSIVE_JOINTS - else "mimic_nominal" - ) - for name in passive - }, - } - return CalibrationProfile( - key=KEY, - namespace="/o12_calibration", - command=CommandLayout( - names=COMMAND_NAMES, - baseline_u8=(), - baseline=(0.0,) * 12, - # 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, - urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS}, - ), - vision=VisionRigSpec( - views=( - ViewSpec("front", ( - TagSpec("front_base", 0, True), TagSpec("thumb_cmc", 1), - TagSpec("thumb_mcp", 2), TagSpec("thumb_dip", 3), - TagSpec("middle_roll", 12), TagSpec("index_roll", 13), - )), - ViewSpec("side", ( - TagSpec("side_base", 4, True), TagSpec("pinky_mcp", 5), - TagSpec("pinky_pip", 6), TagSpec("pinky_dip", 7), - TagSpec("middle_pip", 8), TagSpec("middle_dip", 9), - TagSpec("index_pip", 10), TagSpec("index_dip", 11), - )), - ViewSpec("top", ( - TagSpec("top_base", 14, True), TagSpec("thumb_yaw", 15), - )), - ), - common_frame="calibration_common", - extrinsic_reference_view="front", - extrinsics_quality_limits={ - "reprojection_rms_px": MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, - "maximum_rotation_repeatability_deg": 0.3, - "maximum_translation_repeatability_m": 0.0015, - }, - minimum_capture_counts={"front_side_captures": 15, "front_top_captures": 15}, - ), - motion=MotionPolicy( - tasks=tasks, - # 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": 50.0, - "clearance_flex_rad_s": 0.10, - "clearance_splay_rad_s": 0.04, - "probe_travel_rad": math.radians(3.0), - "probe_speed_rad_s": 0.02, - "endpoint_hold_seconds": 1.0, - "stall_timeout_seconds": 2.0, - }, - ), - measurement=MeasurementPolicy(measurements=measurements, directional_zero=True), - zero=ZeroSolvePolicy( - active_joints=active, - passive_joints=passive, - 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 | (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_validation_error_rad", - "maximum_mimic_residual_rad", - }), - isolated_holdout=True, - ), - scope=ScopePolicy( - calibrate_joints={"full": active}, - frozen_joints={"full": frozenset()}, - default_scope="full", - ), - artifacts=ArtifactPolicy( - output_schema_version=7, - calibration_filename="o12_right_{serial_number}_calibration.json", - corrected_urdf_filename="linkerhand_o12_right_{serial_number}_zero_calibrated.urdf", - protected_input_fields=frozenset({ - "source_urdf_sha256", "camera_extrinsics_sha256", - "calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256", - }), - publication_pointer="latest_passed", - 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, - ) - - -def _run_cli(args: list[str] | None = None) -> None: - from .runner import main - main(args) - - -def _run_node(args: list[str] | None = None) -> None: - from .node import main - main(args) - - -def build_profile() -> RegisteredProfile: - typed = build_typed_profile() - return RegisteredProfile( - typed, - EngineBindings( - hand_profile=typed, - zero_profile=typed.zero, - motion_command=build_calibration_motion_command, - preparation_waypoints=build_calibration_preparation_waypoints, - return_waypoints=build_calibration_return_waypoints, - cli_main=_run_cli, - node_main=_run_node, - ), - ) - - -__all__ = [ - "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT", - "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 deleted file mode 100644 index 421099f..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/quality.py +++ /dev/null @@ -1,115 +0,0 @@ -"""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 deleted file mode 100644 index 30c9a5e..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/resume.py +++ /dev/null @@ -1,384 +0,0 @@ -"""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 deleted file mode 100644 index b038106..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/runner.py +++ /dev/null @@ -1,576 +0,0 @@ -"""One-command O12 right runner with vendor Jazzy overlay loading.""" - -from __future__ import annotations - -import argparse -from datetime import datetime -import json -import os -from pathlib import Path -import re -import subprocess -import time -from typing import Any - -import rclpy -from rclpy.node import Node -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from ...product import ProductConfig, load_product_config -from ..l6.runner import ( - _ProgressConsole, - _l6_reason_zh, - _launch_command, - _stop_stack, - 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 = { - "thumb_pitch_front": "拇指 CMC pitch（正面 ID0→ID1）", - "thumb_roll_front": "拇指 CMC roll（正面 ID0→ID1）", - "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 + 侧面 ID4→ID8）", - "middle_mcp_side": "中指 MCP pitch（侧面 ID4→ID8）", - "middle_pip_dip_side": "中指 PIP / 被动 DIP（侧面 ID8→ID9）", - "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 ( - "O12-MAPPING-301", - f"固定 SDK 映射点动时没有观察到目标关节运动：{task}。", - "检查对应 Tag、通道映射和机械连接；不要交换通道后强行继续。", - ) - 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('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") - - -def render_o12_progress_zh(status, estimator=None) -> str: - """Render O12 in the same operator-oriented layout as G20 and L6.""" - text = render_six_channel_progress_zh( - status, - task_labels=_TASK_LABELS, - reason_renderer=_o12_reason_zh, - estimator=estimator, - ) - temperature = ( - "直接温度回读" - if status.get("temperature_report_verified") - else "错误码 bit1 过热保护" - if status.get("temperature_fallback_active") - else "等待温度能力确认" - ) - 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"（{error_health_text}）；" - f"温度策略={temperature}；速度倍率=" - f"{float(status.get('motion_speed_scale', 1.0)):.1f}x；" - f"控制发布={float(status.get('command_publish_hz', 0.0)):.1f} Hz" - ) - ) - - -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") - self.abort_client = self.create_client(Trigger, "/o12_calibration/abort") - - def _status(self, message: String) -> None: - try: - value = json.loads(message.data) - except json.JSONDecodeError: - 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)], - check=True, - stdout=subprocess.PIPE, - ) - environment = dict(os.environ) - for item in completed.stdout.split(b"\0"): - if b"=" in item: - key, value = item.split(b"=", 1) - environment[key.decode()] = value.decode(errors="surrogateescape") - 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, - 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) - rclpy.init() - monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh)) - process: subprocess.Popen[str] | None = None - try: - # 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 模式、错误/温度回读、" - f"12路反馈和三相机。日志：{log_path}", flush=True, - ) - return 2 - if not monitor.start_client.wait_for_service(timeout_sec=10.0): - print("O12 标定 /start 服务不可用。", flush=True) - return 2 - future = monitor.start_client.call_async(Trigger.Request()) - while rclpy.ok() and not future.done(): - rclpy.spin_once(monitor, timeout_sec=0.2) - response = future.result() - if response is None or not response.success: - print(f"O12 标定未启动：{getattr(response, 'message', '')}", flush=True) - return 2 - 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 已通过；" - "thumb_mcp 静态零位保留原始 CAD。", - f"发布结果：{config.session_root / 'latest_passed'}", - f"JSON：{monitor.status.get('final_json')}", - f"URDF：{monitor.status.get('final_urdf')}", - )), flush=True) - return 0 - except KeyboardInterrupt: - if monitor.abort_client.wait_for_service(timeout_sec=2.0): - monitor.abort_client.call_async(Trigger.Request()) - rclpy.spin_once(monitor, timeout_sec=1.0) - return 130 - finally: - monitor.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - if process is not None: - _stop_stack(process) - log_stream.flush() - os.fsync(log_stream.fileno()) - log_stream.close() - - -def main(args: list[str] | None = None) -> None: - parser = argparse.ArgumentParser(description="O12 right 16-Tag calibration") - parser.add_argument("--config", required=True) - parser.add_argument("--workspace", default=None) - parser.add_argument("--record-bag", action="store_true") - parser.add_argument("--commands-disabled", action="store_true") - parser.add_argument("--validate-only", action="store_true") - 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, - workspace=selected.workspace, - check_can=False, - ) - if config.profile_key.profile_id != "O12/right/o12_right_16/v1": - raise ValueError("O12 runner received a different product profile") - if selected.validate_only: - print(f"配置有效：{config.profile_key.profile_id}，SDK {config.sdk_config_sha256}") - return - if selected.offline_raw: - output = ( - Path(selected.offline_output).expanduser().resolve() - if selected.offline_output else config.session_root / - (datetime.now().strftime("%Y%m%d_%H%M%S") + "_offline") - ) - output.mkdir(parents=True, exist_ok=False) - payload, _fit, correction = finalize_o12_session( - session_dir=output, - serial_number=config.serial_number, - source_urdf=config.source_urdf, - protected_inputs=_protected_inputs(config), - records=load_o12_raw_samples(selected.offline_raw), - publish=selected.publish_offline, - ) - 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, - )) - - -__all__ = ["main", "render_o12_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py deleted file mode 100644 index a1c1595..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o12/urdf.py +++ /dev/null @@ -1,351 +0,0 @@ -"""Auditable O12 URDF correction from radian-domain calibration results.""" - -from __future__ import annotations - -from dataclasses import dataclass -from datetime import datetime -import math -from pathlib import Path -import re -from typing import Mapping -import xml.etree.ElementTree as ET - -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"}) - - -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, ...] - - -def write_o12_corrected_urdf( - *, - source_urdf: str | Path, - output_directory: str | Path, - serial_number: str, - result: O12FitResult, - timestamp: str | None = None, -) -> O12UrdfCorrection: - """Write full-range visual joint limits and equivalent endpoint mimics. - - 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(): - raise ValueError(f"source URDF does not exist: {source}") - if "calibrated" in source.stem.lower(): - raise ValueError("O12 source URDF must be immutable original CAD") - if set(result.travels_rad) != ( - CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT) - ): - 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 = { - str(node.get("name")): node - for node in root.findall("joint") - if node.get("type") == "revolute" - } - required = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS | { - "ring_mcp_pitch", "ring_pip", "ring_dip" - } - if missing := required - set(joints): - raise ValueError("source URDF is missing O12 joints: " + ",".join(sorted(missing))) - - corrected_limits: dict[str, tuple[float, float]] = {} - patches: dict[str, UrdfJointPatch] = {} - 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}") - 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 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 = 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(mimic_multipliers[name]) - if not math.isfinite(multiplier) or not 0.5 <= multiplier <= 2.2: - raise ValueError(f"invalid O12 mimic multiplier: {name}") - 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") - safe_serial = "".join( - char if char.isalnum() or char in "_.-" else "_" - for char in str(serial_number) - ) - if not safe_serial: - raise ValueError("serial number must not be empty") - output = Path(output_directory).expanduser().resolve() - output.mkdir(parents=True, exist_ok=True) - destination = output / f"{source.stem}_calibrated_{safe_serial}_{stamp}.urdf" - write_urdf_patches( - source_urdf=source, - destination_urdf=destination, - patches=UrdfPatchSet(joints=patches), - 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.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", - "o12_active_ranges", - "o12_endpoint_mimic_contract", - "write_o12_corrected_urdf", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py deleted file mode 100644 index bffbc3e..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/__init__.py +++ /dev/null @@ -1,14 +0,0 @@ -"""Registered O6 calibration profiles.""" - -from ..registry import ProfileRegistry - - -def register_profiles(registry: ProfileRegistry) -> None: - from .left_transfer import build_profile as build_left_transfer_profile - from .profile import build_profile - - registry.register(build_profile()) - registry.register(build_left_transfer_profile()) - - -__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py deleted file mode 100644 index 8386b0f..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/fitting.py +++ /dev/null @@ -1,334 +0,0 @@ -"""O6 curve, geometric-zero, holdout, and linear mimic fitting.""" - -from __future__ import annotations - -import math -from pathlib import Path -from typing import Mapping, Sequence -import xml.etree.ElementTree as ET - -import numpy as np - -from ..g20.profile import HandCalibrationProfile, JointCurveFit, JointSpec -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 L6FitResult, MimicFit, fit_coupling_model -from .profile import ( - CALIBRATED_ACTIVE_JOINTS, - GEOMETRIC_ZERO_JOINTS, - COMMAND_INDEX_BY_JOINT, - COMMAND_NAMES, - COUPLING_MODEL_BY_JOINT, - COUPLING_RESIDUAL_MAX_DEG, - COUPLING_RESIDUAL_P95_DEG, - ENDPOINT_ANCHOR_BY_JOINT, - KEY, - MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M, - MAXIMUM_HYSTERESIS_DEG, - MEASURED_PASSIVE_JOINTS, - MIMIC_SOURCE_BY_JOINT, -) - - -O6FitResult = L6FitResult -O6_THUMB_AXIS_JOINTS: tuple[str, ...] = ( - "rh_thumb_cmc_yaw", - "rh_thumb_cmc_pitch", - "rh_thumb_ip", - "rh_pinky_mcp_pitch", -) - - -def _zero_profile() -> ZeroCalibrationProfile: - specs = { - "rh_thumb_cmc_yaw": JointSpec( - "rh_thumb_cmc_yaw", 1, True, "top", "top_base", "thumb_yaw" - ), - "rh_thumb_cmc_pitch": JointSpec( - "rh_thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_pitch" - ), - "rh_thumb_ip": JointSpec( - "rh_thumb_ip", 0, False, "front", "thumb_pitch", "thumb_ip" - ), - "rh_pinky_mcp_pitch": JointSpec( - "rh_pinky_mcp_pitch", 5, True, "side", "side_base", "pinky_pitch" - ), - } - 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=KEY.layout, - model="O6", - command_names=COMMAND_NAMES, - baseline_command=(255,) * 6, - directional_zero=True, - isolated_holdout=True, - ) - return ZeroCalibrationProfile( - hand=hand, - direct_zero_joints=("rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch"), - axis_joints=O6_THUMB_AXIS_JOINTS, - inherited_zero_joints={}, - inherited_static_zero_joints={}, - constrained_circle_joints=frozenset(O6_THUMB_AXIS_JOINTS), - root_anchor_joints=frozenset({"rh_thumb_cmc_yaw"}), - axis_parent_joint={"rh_thumb_cmc_pitch": "rh_thumb_cmc_yaw"}, - phase_parent_joint={"rh_thumb_ip": "rh_thumb_cmc_pitch"}, - offset_observer_joint={ - "rh_thumb_cmc_yaw": "rh_thumb_cmc_pitch", - "rh_thumb_cmc_pitch": "rh_thumb_ip", - }, - same_view_axis_pair_by_offset={}, - fixed_direct_zero_offsets_rad={}, - static_output_zero_offsets_rad={}, - base_pose_strategy="thumb_serial", - orientation_anchor_joint="rh_pinky_mcp_pitch", - directed_base_axis_joints=frozenset({ - "rh_thumb_cmc_yaw", "rh_pinky_mcp_pitch" - }), - ) - - -def _travel(fit: JointCurveFit) -> float: - value = 0.5 * ( - float(fit.decreasing_rad[0] - fit.decreasing_rad[255]) - + float(fit.increasing_rad[0] - fit.increasing_rad[255]) - ) - if not math.isfinite(value) or value <= 0.0: - raise ValueError("O6 fitted travel must be finite and positive") - return value - - -def _endpoint_offsets( - source_urdf: str | Path, - curves: Mapping[str, JointCurveFit], -) -> tuple[dict[str, float], dict[str, float]]: - joints = { - str(joint.get("name")): joint - for joint in ET.parse(Path(source_urdf)).getroot().findall("joint") - } - offsets: dict[str, float] = {} - travels: dict[str, float] = {} - for name in sorted(CALIBRATED_ACTIVE_JOINTS): - joint = joints.get(name) - limit = None if joint is None else joint.find("limit") - if limit is None or limit.get("lower") is None: - raise ValueError(f"O6 source joint has incomplete limits: {name}") - travel = _travel(curves[name]) - offset = float(limit.get("lower")) - if ENDPOINT_ANCHOR_BY_JOINT[name] != "lower_at_start": - raise ValueError(f"unsupported O6 endpoint anchor: {name}") - if not math.isfinite(offset) or abs(offset) > math.radians(15.0): - raise ValueError(f"O6 endpoint zero offset exceeds 15 degrees: {name}") - offsets[name] = offset - travels[name] = travel - return offsets, travels - - -def _has_complete_geometry( - records: Mapping[str, Sequence[Mapping[str, object]]], -) -> bool: - required = { - "relative_translation_xyz_m", "parent_pose_common", "child_pose_common", - "view_normal_common_xyz", "camera_center_common_xyz_m", "state_u8", - } - return all( - rows and all(required.issubset(row) for row in rows) - for name in O6_THUMB_AXIS_JOINTS - for rows in (records.get(name, ()),) - ) - - -def _fit_thumb_zero( - source_urdf: str | Path, - records: Mapping[str, Sequence[Mapping[str, object]]], - curves: Mapping[str, JointCurveFit], -) -> ZeroSolveResult: - profile = _zero_profile() - measurements = [] - by_key: dict[tuple[str, int], object] = {} - for cycle in range(4): - for name in ( - "rh_thumb_cmc_yaw", "rh_thumb_cmc_pitch", "rh_pinky_mcp_pitch" - ): - rows = records[name] - 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", - ) - measurement = with_depth_free_axis_projection( - measurement, rows[0]["camera_center_common_xyz_m"] - ) - measurements.append(measurement) - by_key[(name, cycle)] = measurement - rows = records["rh_thumb_ip"] - pitch_axis = by_key[("rh_thumb_cmc_pitch", cycle)] - measurement = fit_joint_axis_measurement( - "rh_thumb_ip", - rows, - cycle=cycle, - zero_command_u8=255, - axis_common_constraint=pitch_axis.axis_common_xyz, - 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[MIMIC_SOURCE_BY_JOINT.get(name, name)] - for name in O6_THUMB_AXIS_JOINTS - }, - training_cycles=(0, 1, 2), - validation_cycle=3, - maximum_offset_rad=math.radians(15.0), - finger_maximum_offset_rad=math.radians(15.0), - joint_maximum_offset_rad={ - "rh_thumb_cmc_yaw": math.radians(15.0), - "rh_thumb_cmc_pitch": math.radians(15.0), - }, - 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), - maximum_pose_axis_line_rms_m=0.0015, - hand_type="right", - tag_layout=KEY.layout, - zero_profile=profile, - ) - if not result.passed: - details = ",".join( - f"{name}={reason}" for name, reason in sorted(result.failure_reasons.items()) - ) - raise ValueError("O6 thumb axis zero solve failed:" + details) - if ( - not math.isfinite(result.axis_line_rms_m) - or result.axis_line_rms_m > MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M - ): - raise ValueError( - "O6 thumb cross-view axis line RMS exceeds " - f"{MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M * 1000.0:.1f} mm: " - f"actual={result.axis_line_rms_m * 1000.0:.3f} mm; " - "recalibrate camera extrinsics or check the rigid checkerboard" - ) - return result - - -def fit_o6_session( - source_urdf: str | Path, - records_by_joint: Mapping[str, Sequence[Mapping[str, object]]], - *, - require_thumb_axis_zero: bool = False, -) -> O6FitResult: - expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS - if set(records_by_joint) != expected: - raise ValueError("O6 records must contain exactly five measured joints") - curves: dict[str, JointCurveFit] = {} - holdout: dict[str, tuple[float, ...]] = {} - cycle_curves: dict[str, dict[int, JointCurveFit]] = {} - for name in sorted(expected): - rows = [dict(row) for row in records_by_joint[name]] - 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"{name} is missing training or holdout records") - fit = fit_rotation_joint_curve(training, zero_command_u8=255) - errors = rotation_curve_holdout_errors(fit, validation, zero_command_u8=255) - 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"{name} isolated holdout failed") - correction_limit = math.radians( - 3.0 if name in MEASURED_PASSIVE_JOINTS else 2.0 - ) - if fit.maximum_monotonic_correction_rad > correction_limit: - raise ValueError(f"{name} monotonic correction exceeds limit") - curves[name] = fit - holdout[name] = errors - cycle_curves[name] = { - cycle: fit_rotation_joint_curve( - [row for row in training if int(row["cycle"]) == cycle], - zero_command_u8=255, - ) - for cycle in (0, 1, 2) - } - offsets, travels = _endpoint_offsets(source_urdf, curves) - if require_thumb_axis_zero and not _has_complete_geometry(records_by_joint): - raise ValueError( - "O6 thumb absolute zero requires common-frame Tag pose trajectories" - ) - thumb_result = None - zero_methods = { - name: "mechanical_lower_endpoint" for name in CALIBRATED_ACTIVE_JOINTS - } - if _has_complete_geometry(records_by_joint): - thumb_result = _fit_thumb_zero(source_urdf, records_by_joint, curves) - for name in GEOMETRIC_ZERO_JOINTS: - offsets[name] = float(thumb_result.direct_offsets_rad[name]) - zero_methods[name] = "urdf_serial_axis_geometry" - mimic_fits: dict[str, MimicFit] = {} - for target in sorted(MEASURED_PASSIVE_JOINTS): - source = MIMIC_SOURCE_BY_JOINT[target] - pairs = [] - for cycle in (0, 1, 2): - active = cycle_curves[source][cycle] - passive = cycle_curves[target][cycle] - pairs.append(( - tuple(active.decreasing_rad) + tuple(active.increasing_rad), - tuple(passive.decreasing_rad) + tuple(passive.increasing_rad), - )) - mimic_fits[target] = fit_coupling_model( - source, - target, - curves[source], - curves[target], - model=COUPLING_MODEL_BY_JOINT[target], - cycle_curve_pairs=pairs, - maximum_multiplier=2.2, - maximum_residual_p95_rad=math.radians( - COUPLING_RESIDUAL_P95_DEG - ), - maximum_residual_rad=math.radians(COUPLING_RESIDUAL_MAX_DEG), - ) - return O6FitResult( - curves=curves, - zero_offsets_rad=offsets, - travels_rad=travels, - mimic_fits=mimic_fits, - holdout_errors_rad=holdout, - zero_method_by_joint=zero_methods, - zero_fallback_reason_by_joint={}, - thumb_zero_result=thumb_result, - ) - - -__all__ = ["MimicFit", "O6FitResult", "O6_THUMB_AXIS_JOINTS", "fit_o6_session"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py deleted file mode 100644 index 95e9219..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/node.py +++ /dev/null @@ -1,38 +0,0 @@ -"""Online three-view acquisition node for O6/right/o6_right_8/v1.""" - -from __future__ import annotations - -import rclpy - -from ..l6.node import L6ThreeCameraCalibrationNode, MotionStep -from .pipeline import finalize_o6_session -from .profile import build_typed_profile - - -class O6ThreeCameraCalibrationNode(L6ThreeCameraCalibrationNode): - """O6 specialization of the shared six-channel acquisition node.""" - - def __init__(self) -> None: - super().__init__( - profile=build_typed_profile(), - finalizer=finalize_o6_session, - sample_kind="o6_joint_sample", - ) - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - node: O6ThreeCameraCalibrationNode | None = None - try: - node = O6ThreeCameraCalibrationNode() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -__all__ = ["MotionStep", "O6ThreeCameraCalibrationNode", "main"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py deleted file mode 100644 index 2210b65..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/pipeline.py +++ /dev/null @@ -1,189 +0,0 @@ -"""Shared online/offline finalization path for O6 right.""" - -from __future__ import annotations - -from datetime import datetime -import json -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, - build_o6_runtime_payload, - build_o6_urdf_input_payload, - load_o6_urdf_input, - publish_partial_session, -) -from .fitting import O6FitResult, fit_o6_session -from .profile import ( - CALIBRATED_ACTIVE_JOINTS, - CORRECTED_PASSIVE_JOINTS, - KEY, - MEASURED_PASSIVE_JOINTS, - TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, - TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, - build_typed_profile, -) -from .urdf import O6UrdfCorrection, write_o6_corrected_urdf - - -MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS - - -def _canonical_feedback_u8(value: float) -> int: - command = int(round(float(value))) - if command <= 2: - return 0 - if command >= 253: - return 255 - return command - - -def accepted_records_by_joint( - records: Sequence[Mapping[str, Any]], -) -> dict[str, list[dict[str, Any]]]: - samples = [ - dict(row) for row in records - if row.get("kind") == "o6_joint_sample" - and str(row.get("joint", "")) in MEASURED_JOINTS - ] - 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 = {name: [] for name in MEASURED_JOINTS} - geometric = ( - "relative_translation_xyz_m", "parent_pose_common", "child_pose_common", - "view_normal_common_xyz", "camera_center_common_xyz_m", "state_u8", - ) - for row in samples: - key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"])) - if int(row.get("attempt", 1)) != latest[key]: - continue - accepted: dict[str, Any] = { - "cycle": int(row["cycle"]), - "direction": str(row["direction"]), - "command_u8": _canonical_feedback_u8(float(row["feedback_u8"])), - "feedback_u8": float(row["feedback_u8"]), - "relative_quaternion_xyzw": list(row["relative_quaternion_xyzw"]), - } - if any(field in row for field in geometric): - missing = [field for field in geometric if field not in row] - if missing: - raise ValueError("O6 geometric sample is incomplete: " + ",".join(missing)) - for field in geometric: - accepted[field] = dict(row[field]) if isinstance(row[field], Mapping) else list(row[field]) - result[str(row["joint"])].append(accepted) - return result - - -def load_o6_raw_samples(path: str | Path) -> list[dict[str, Any]]: - source = Path(path).expanduser().resolve() - if not source.is_file(): - raise ValueError(f"raw O6 sample file does not exist: {source}") - rows = [] - for number, line in enumerate(source.read_text(encoding="utf-8").splitlines(), 1): - if not line.strip(): - continue - try: - value = json.loads(line) - except json.JSONDecodeError as error: - raise ValueError(f"invalid O6 JSONL record at line {number}") from error - if not isinstance(value, Mapping): - raise ValueError(f"O6 JSONL line {number} is not an object") - rows.append(dict(value)) - return rows - - -def finalize_o6_session( - *, - session_dir: str | Path, - serial_number: str, - source_urdf: str | Path, - protected_inputs: Mapping[str, str], - records: Sequence[Mapping[str, Any]], - publish: bool = True, - timestamp: str | None = None, -) -> tuple[dict[str, Any], O6FitResult, O6UrdfCorrection]: - directory = Path(session_dir).expanduser().resolve() - directory.mkdir(parents=True, exist_ok=True) - 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, - result=result, - protected_inputs=protected_inputs, - passed=True, - ) - correction_input = directory / f"o6_right_{serial_number}_urdf_correction_input.json" - atomic_write_json(correction_input, build_o6_urdf_input_payload( - serial_number=serial_number, source_urdf=source_urdf, result=result - )) - authenticated = load_o6_urdf_input( - correction_input, source_urdf=source_urdf, serial_number=serial_number - ) - correction = write_o6_corrected_urdf( - source_urdf=source_urdf, - output_directory=directory, - serial_number=serial_number, - result=authenticated, - timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"), - ) - json_path = directory / f"o6_right_{serial_number}_partial_calibration.json" - atomic_write_json(json_path, payload) - summary = { - "schema_version": 1, - "profile_id": KEY.profile_id, - "serial_number": str(serial_number), - "result": "PARTIAL_PASS", - "publication_pointer": "latest_partial_passed", - "calibrated_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS), - "measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS), - "transferred_active_joints": dict(sorted(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items())), - "transferred_passive_joints": dict(sorted(TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items())), - "passive_coupling": { - name: { - "model": result.mimic_fits[donor].model, - "coefficients": [ - round(float(value), 10) - for value in result.mimic_fits[donor].coefficients - ], - "urdf_mimic_enabled": True, - "urdf_mimic_policy": result.mimic_fits[donor].urdf_mimic_policy, - "mimic_multiplier": round( - float(result.mimic_fits[donor].urdf_mimic_multiplier), 10 - ), - "residual_p95_deg": round(math.degrees(float(result.mimic_fits[donor].residual_p95_rad)), 6), - "residual_max_deg": round(math.degrees(float(result.mimic_fits[donor].residual_max_rad)), 6), - } - for name, donor in sorted({ - target: TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(target, target) - for target in CORRECTED_PASSIVE_JOINTS - }.items()) - }, - "explicit_runtime_joints": sorted(correction.explicit_runtime_joints), - "artifacts": artifact_hashes(json_path, correction.path), - "quality": {"passed": True}, - } - atomic_write_json(directory / "calibration_summary.json", summary) - if publish: - publish_partial_session(directory.parent, directory) - return payload, result, correction - - -__all__ = [ - "accepted_records_by_joint", "finalize_o6_session", "load_o6_raw_samples" -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py deleted file mode 100644 index 84112c2..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/profile.py +++ /dev/null @@ -1,321 +0,0 @@ -"""Reviewed partial-calibration profile for the right O6 eight-Tag rig.""" - -from __future__ import annotations - -from ...core import ( - ArtifactPolicy, - CalibrationProfile, - CommandLayout, - MeasurementPolicy, - MeasurementSpec, - MotionPolicy, - ProfileKey, - QualityPolicy, - ScopePolicy, - TagSpec, - TaskSpec, - ViewSpec, - VisionRigSpec, - ZeroSolvePolicy, -) -from ..l6.motion import ( - build_calibration_motion_command, - build_calibration_preparation_waypoints, - build_calibration_return_waypoints, -) -from ..registry import EngineBindings, RegisteredProfile - - -KEY = ProfileKey("O6", "right", "o6_right_8", 1) - -BASELINE_SPEED_U8 = 80 -PREFLIGHT_SPEED_U8 = 60 -FORMAL_SPEED_U8 = 40 -MAXIMUM_HYSTERESIS_DEG = 3.5 -COUPLING_RESIDUAL_P95_DEG = 2.2 -COUPLING_RESIDUAL_MAX_DEG = 3.0 -MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX = 1.5 -MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M = 0.020 - -COMMAND_NAMES: tuple[str, ...] = ( - "thumb_cmc_pitch", - "thumb_cmc_yaw", - "index_mcp_pitch", - "middle_mcp_pitch", - "ring_mcp_pitch", - "pinky_mcp_pitch", -) - -ACTIVE_JOINTS: tuple[str, ...] = ( - "rh_thumb_cmc_pitch", - "rh_thumb_cmc_yaw", - "rh_index_mcp_pitch", - "rh_middle_mcp_pitch", - "rh_ring_mcp_pitch", - "rh_pinky_mcp_pitch", -) -PASSIVE_JOINTS: tuple[str, ...] = ( - "rh_thumb_ip", - "rh_index_dip", - "rh_middle_dip", - "rh_ring_dip", - "rh_pinky_dip", -) -CALIBRATED_ACTIVE_JOINTS = frozenset( - {"rh_thumb_cmc_pitch", "rh_thumb_cmc_yaw", "rh_pinky_mcp_pitch"} -) -# Field validation on O6_RIGHT_001 showed that the yaw zero is reliably -# observable from cross-view axis directions, while the pitch phase depends -# on two monocular axis-line centres and can acquire a repeatable systematic -# bias. Keep the mechanically established CAD lower endpoint for pitch. -GEOMETRIC_ZERO_JOINTS = frozenset({"rh_thumb_cmc_yaw"}) -CAD_ENDPOINT_ZERO_JOINTS = frozenset( - {"rh_thumb_cmc_pitch", "rh_pinky_mcp_pitch"} -) -MEASURED_PASSIVE_JOINTS = frozenset({"rh_thumb_ip", "rh_pinky_dip"}) - -TRANSFERRED_ACTIVE_SOURCE_BY_JOINT = { - "rh_index_mcp_pitch": "rh_pinky_mcp_pitch", - "rh_middle_mcp_pitch": "rh_pinky_mcp_pitch", - "rh_ring_mcp_pitch": "rh_pinky_mcp_pitch", -} -TRANSFERRED_PASSIVE_SOURCE_BY_JOINT = { - "rh_index_dip": "rh_pinky_dip", - "rh_middle_dip": "rh_pinky_dip", - "rh_ring_dip": "rh_pinky_dip", -} -CORRECTED_ACTIVE_JOINTS = frozenset( - CALIBRATED_ACTIVE_JOINTS | TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.keys() -) -CORRECTED_PASSIVE_JOINTS = frozenset( - MEASURED_PASSIVE_JOINTS | TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.keys() -) - -ENDPOINT_ANCHOR_BY_JOINT = { - "rh_thumb_cmc_pitch": "lower_at_start", - "rh_thumb_cmc_yaw": "lower_at_start", - "rh_pinky_mcp_pitch": "lower_at_start", -} - -COMMAND_INDEX_BY_JOINT = { - "rh_thumb_cmc_pitch": 0, - "rh_thumb_cmc_yaw": 1, - "rh_index_mcp_pitch": 2, - "rh_middle_mcp_pitch": 3, - "rh_ring_mcp_pitch": 4, - "rh_pinky_mcp_pitch": 5, -} - -MIMIC_SOURCE_BY_JOINT = { - "rh_thumb_ip": "rh_thumb_cmc_pitch", - "rh_index_dip": "rh_index_mcp_pitch", - "rh_middle_dip": "rh_middle_mcp_pitch", - "rh_ring_dip": "rh_ring_mcp_pitch", - "rh_pinky_dip": "rh_pinky_mcp_pitch", -} - -COUPLING_MODEL_BY_JOINT = { - name: "quadratic_runtime" for name in PASSIVE_JOINTS -} - - -def build_typed_profile() -> CalibrationProfile: - active = frozenset(ACTIVE_JOINTS) - passive = frozenset(PASSIVE_JOINTS) - measurements = { - "rh_thumb_cmc_yaw": MeasurementSpec( - "rh_thumb_cmc_yaw", "relative_rotation", "top", "top_base", "thumb_yaw" - ), - "rh_thumb_cmc_pitch": MeasurementSpec( - "rh_thumb_cmc_pitch", "relative_rotation", "front", "front_base", "thumb_pitch" - ), - "rh_thumb_ip": MeasurementSpec( - "rh_thumb_ip", "relative_rotation", "front", "thumb_pitch", "thumb_ip" - ), - "rh_pinky_mcp_pitch": MeasurementSpec( - "rh_pinky_mcp_pitch", "relative_rotation", "side", "side_base", "pinky_pitch" - ), - "rh_pinky_dip": MeasurementSpec( - "rh_pinky_dip", "relative_rotation", "side", "pinky_pitch", "pinky_dip" - ), - } - tasks = ( - TaskSpec( - "thumb_yaw_top", "top", 1, ("rh_thumb_cmc_yaw",), - preflight_speed_u8=PREFLIGHT_SPEED_U8, - formal_speed_u8=FORMAL_SPEED_U8, - ), - TaskSpec( - "thumb_pitch_ip_front", "front", 0, - ("rh_thumb_cmc_pitch", "rh_thumb_ip"), - preflight_speed_u8=PREFLIGHT_SPEED_U8, - formal_speed_u8=FORMAL_SPEED_U8, - ), - TaskSpec( - "pinky_pitch_dip_side", "side", 5, - ("rh_pinky_mcp_pitch", "rh_pinky_dip"), - preflight_speed_u8=PREFLIGHT_SPEED_U8, - formal_speed_u8=FORMAL_SPEED_U8, - ), - ) - coverage = { - **{ - name: ( - "measured_static_dynamic" - if name in CALIBRATED_ACTIVE_JOINTS - else "transferred_static_dynamic" - ) - for name in active - }, - **{ - name: ( - "measured_dynamic_cad_static" - if name in MEASURED_PASSIVE_JOINTS - else "transferred_dynamic_cad_static" - ) - for name in passive - }, - } - return CalibrationProfile( - key=KEY, - namespace="/o6_calibration", - command=CommandLayout( - names=COMMAND_NAMES, - baseline_u8=(255,) * 6, - command_index_by_joint=COMMAND_INDEX_BY_JOINT, - urdf_joint_by_joint={name: name for name in ACTIVE_JOINTS}, - speed_slot_by_command_index={index: index for index in range(6)}, - ), - vision=VisionRigSpec( - views=( - ViewSpec("front", ( - TagSpec("front_base", 0, fixed_reference=True), - TagSpec("thumb_pitch", 1), - TagSpec("thumb_ip", 2), - )), - ViewSpec("side", ( - TagSpec("side_base", 3, fixed_reference=True), - TagSpec("pinky_pitch", 4), - TagSpec("pinky_dip", 5), - )), - ViewSpec("top", ( - TagSpec("top_base", 6, fixed_reference=True), - TagSpec("thumb_yaw", 7), - )), - ), - common_frame="calibration_common", - extrinsic_reference_view="front", - extrinsics_quality_limits={ - "reprojection_rms_px": MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX, - "maximum_rotation_repeatability_deg": 0.3, - "maximum_translation_repeatability_m": 0.0015, - }, - minimum_capture_counts={ - "front_side_captures": 15, - "front_top_captures": 15, - }, - ), - motion=MotionPolicy( - tasks=tasks, - precheck_sweeps=False, - steady_command_checkpoints=False, - speed_parameters={ - "baseline_u8": BASELINE_SPEED_U8, - "preflight_u8": PREFLIGHT_SPEED_U8, - "formal_u8": FORMAL_SPEED_U8, - "speed_settle_seconds": 0.2, - "command_trajectory_full_range_seconds": 6.0, - "torque_u8": 80, - "endpoint_hold_seconds": 1.0, - "stall_timeout_seconds": 2.0, - }, - ), - measurement=MeasurementPolicy( - measurements=measurements, - directional_zero=True, - ), - 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(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=ENDPOINT_ANCHOR_BY_JOINT, - fitted_mimic_joints=MEASURED_PASSIVE_JOINTS, - coupling_model_by_joint=COUPLING_MODEL_BY_JOINT, - ), - quality=QualityPolicy( - training_cycles=(0, 1, 2), - holdout_cycle=3, - hard_threshold_keys=frozenset({ - "minimum_detection_rate", - "maximum_state_image_skew_ms", - "maximum_validation_error_rad", - "maximum_mimic_residual_rad", - }), - isolated_holdout=True, - ), - scope=ScopePolicy( - calibrate_joints={"partial": CALIBRATED_ACTIVE_JOINTS}, - frozen_joints={"partial": active - CALIBRATED_ACTIVE_JOINTS}, - default_scope="partial", - ), - artifacts=ArtifactPolicy( - output_schema_version=6, - calibration_filename="o6_right_{serial_number}_partial_calibration.json", - corrected_urdf_filename="linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf", - protected_input_fields=frozenset({ - "source_urdf_sha256", - "camera_extrinsics_sha256", - "calibration_config_sha256", - "tag_config_sha256", - }), - publication_pointer="latest_partial_passed", - session_compatibility_tokens=frozenset({ - "o6_partial_v1", "feedback_curves_v6" - }), - publish_corrected_urdf=True, - ), - joint_coverage=coverage, - ) - - -def _run_cli(args: list[str] | None = None) -> None: - from .runner import main - - main(args) - - -def _run_node(args: list[str] | None = None) -> None: - from .node import main - - main(args) - - -def build_profile() -> RegisteredProfile: - typed = build_typed_profile() - return RegisteredProfile( - profile=typed, - engine=EngineBindings( - hand_profile=typed, - zero_profile=typed.zero, - motion_command=build_calibration_motion_command, - preparation_waypoints=build_calibration_preparation_waypoints, - return_waypoints=build_calibration_return_waypoints, - cli_main=_run_cli, - node_main=_run_node, - ), - ) - - -__all__ = [ - "ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_NAMES", - "CORRECTED_ACTIVE_JOINTS", "CORRECTED_PASSIVE_JOINTS", - "COUPLING_MODEL_BY_JOINT", "ENDPOINT_ANCHOR_BY_JOINT", "KEY", - "MEASURED_PASSIVE_JOINTS", "MIMIC_SOURCE_BY_JOINT", "PASSIVE_JOINTS", - "TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", - "TRANSFERRED_PASSIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile", -] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py deleted file mode 100644 index 8c96a6d..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/models/o6/runner.py +++ /dev/null @@ -1,225 +0,0 @@ -"""One-command online runner and deterministic offline replay for O6 right.""" - -from __future__ import annotations - -import argparse -from datetime import datetime -import json -import os -from pathlib import Path -import subprocess -import time -from typing import Any - -import rclpy -from rclpy.node import Node -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from ...product import ProductConfig, load_product_config -from ..l6.runner import ( - _ProgressConsole, - _l6_reason_zh, - _launch_command, - _stop_stack, - _wait_until, - render_six_channel_progress_zh, -) -from ...operator_report import ProgressEstimator -from .pipeline import finalize_o6_session, load_o6_raw_samples -from .profile import BASELINE_SPEED_U8 - - -_TASK_LABELS = { - "thumb_yaw_top": "拇指 CMC yaw（上面机位，ID6→ID7）", - "thumb_pitch_ip_front": "拇指 CMC pitch / IP（正面机位，ID0→ID1→ID2）", - "pinky_pitch_dip_side": "小指 MCP pitch / DIP（侧面机位，ID3→ID4→ID5）", -} - - -def render_o6_progress_zh( - status: dict[str, Any], - estimator: ProgressEstimator | None = None, -) -> str: - return render_six_channel_progress_zh( - status, - task_labels=_TASK_LABELS, - reason_renderer=lambda value: _l6_reason_zh( - value, model_name="O6" - ), - estimator=estimator, - ) - - -class _Monitor(Node): - def __init__(self) -> None: - super().__init__("o6_calibration_runner") - self.status: dict[str, Any] = {} - self.progress = _ProgressConsole(render_o6_progress_zh) - self.create_subscription(String, "/o6_calibration/status", self._status, 10) - self.start_client = self.create_client(Trigger, "/o6_calibration/start") - self.abort_client = self.create_client(Trigger, "/o6_calibration/abort") - - def _status(self, message: String) -> None: - try: - value = json.loads(message.data) - except json.JSONDecodeError: - return - if isinstance(value, dict): - self.status = value - self.progress.update(value) - - -def _run_online( - config: ProductConfig, *, record_bag: bool, commands_enabled: bool -) -> int: - 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) - log_path = session / "calibration.log" - log_stream = log_path.open("a", encoding="utf-8", buffering=1) - print( - f"O6 标定环境正在启动（相机、SDK、标定节点）；日志：{log_path}", - flush=True, - ) - process = subprocess.Popen( - _launch_command( - config, - session, - record_bag=record_bag, - commands_enabled=commands_enabled, - sdk_startup_speed_u8=BASELINE_SPEED_U8, - ), - cwd=config.workspace, - stdout=log_stream, - stderr=subprocess.STDOUT, - text=True, - start_new_session=True, - ) - rclpy.init() - monitor = _Monitor() - try: - ready = _wait_until( - monitor, process, - lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"}, - timeout=120.0, - ) - if not ready or monitor.status.get("state") != "READY": - detail = "" - try: - lines = log_path.read_text( - encoding="utf-8", errors="replace" - ).splitlines() - significant = [ - line for line in lines - if "[ERROR]" in line - or "Traceback" in line - or "ValueError:" in line - or "RuntimeError:" in line - ] - if significant: - detail = "\n启动日志摘要：\n" + "\n".join(significant[-6:]) - except OSError: - pass - print( - "O6 启动失败；请检查六通道反馈、三相机内参和状态信息。" - + detail - + f"\n完整日志：{log_path}", - flush=True, - ) - return 2 - if not monitor.start_client.wait_for_service(timeout_sec=10.0): - print("O6 标定 /start 服务不可用。", flush=True) - return 2 - future = monitor.start_client.call_async(Trigger.Request()) - while rclpy.ok() and not future.done(): - rclpy.spin_once(monitor, timeout_sec=0.2) - response = future.result() - if response is None or not response.success: - print(f"O6 标定未启动：{getattr(response, 'message', '')}", flush=True) - return 2 - finished = _wait_until( - monitor, process, - lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"}, - timeout=None, - ) - if not finished or monitor.status.get("state") != "PASSED": - print("O6 标定失败：" + str(monitor.status.get("reason", "process_exit")), flush=True) - return 3 - print( - "\n".join([ - "PASS：O6 右手三主动关节与两条被动关节实测通过；" - "小指结果已迁移到食指、中指和无名指。", - f"部分结果：{config.session_root / 'latest_partial_passed'}", - f"JSON：{monitor.status.get('final_json')}", - f"URDF：{monitor.status.get('final_urdf')}", - ]), - flush=True, - ) - return 0 - except KeyboardInterrupt: - if monitor.abort_client.wait_for_service(timeout_sec=2.0): - monitor.abort_client.call_async(Trigger.Request()) - rclpy.spin_once(monitor, timeout_sec=1.0) - return 130 - finally: - monitor.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - _stop_stack(process) - log_stream.flush() - os.fsync(log_stream.fileno()) - log_stream.close() - - -def main(args: list[str] | None = None) -> None: - parser = argparse.ArgumentParser(description="O6 right partial calibration") - parser.add_argument("--config", required=True) - parser.add_argument("--workspace", default=None) - parser.add_argument("--record-bag", action="store_true") - parser.add_argument("--commands-disabled", action="store_true") - parser.add_argument("--validate-only", action="store_true") - parser.add_argument("--offline-raw", default="") - parser.add_argument("--offline-output", default="") - parser.add_argument("--publish-offline", action="store_true") - selected = parser.parse_args(args) - config = load_product_config( - selected.config, - workspace=selected.workspace, - check_can=not bool(selected.validate_only or selected.offline_raw), - ) - if selected.validate_only: - print(f"配置有效：{config.profile_key.profile_id}，源URDF {config.source_urdf_sha256}") - return - if selected.offline_raw: - output = ( - Path(selected.offline_output).expanduser().resolve() - if selected.offline_output else config.session_root / - (datetime.now().strftime("%Y%m%d_%H%M%S") + "_offline") - ) - output.mkdir(parents=True, exist_ok=False) - payload, _fit, correction = finalize_o6_session( - 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, - }, - records=load_o6_raw_samples(selected.offline_raw), - publish=selected.publish_offline, - ) - print(f"离线回放PASS：schema {payload['schema_version']}，URDF {correction.path}") - return - raise SystemExit(_run_online( - config, - record_bag=selected.record_bag, - commands_enabled=not selected.commands_disabled, - )) - - -__all__ = ["main", "render_o6_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/node.py b/src/linkerhand_calibration/linkerhand_calibration/node.py deleted file mode 100644 index 07570a3..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/node.py +++ /dev/null @@ -1,2891 +0,0 @@ -"""ROS 2 hardware calibration node for a front-facing industrial camera.""" - -from __future__ import annotations - -from collections import deque -from dataclasses import replace -from datetime import datetime, timezone -import hashlib -import json -import math -from pathlib import Path -import random -import re -import time -from typing import Any - -import cv2 -from cv_bridge import CvBridge -import numpy as np -import rclpy -from apriltag_msgs.msg import AprilTagDetectionArray -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from sensor_msgs.msg import CameraInfo, Image, JointState -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from .acquisition import ( - ContinuousSweepCollector, - Observation, - PointCollector, - StateSample, - TAG_PAIR_ROLES, - TagQuality, - aggregate_sweep_observations, - interpolate_state_u8, - tag_quality_is_valid, -) -from .compat.legacy.thumb_core import ( - BASELINE_COMMAND, - COMMAND_NAMES, - DIRECTION_DECREASING, - DIRECTION_INCREASING, - PAIR_IP, - PAIR_MCP, - PAIR_NAMES, - PAIR_ROOT, - PHASE_ROOT, - PHASE_TIP, - FitResult, - build_command, - create_final_payload, - fit_calibration_curves, - image_plane_tag_quaternion_xyzw, - maximum_non_target_drift_rad, - relative_quaternion_xyzw, - rotation_inlier_fraction, - rotation_rms_rad, - scan_targets, -) -from .diagnostics import ( - STATE_NAMES_ZH, - build_tag_quality_diagnostics, - pnp_rejection_zh, - render_status_text_zh, - status_guidance_zh, -) -from .pnp import ( - SquareTagGroupPoseTracker, - SquareTagPose, - SquareTagPoseTracker, - rotation_distance_rad, - select_rigid_group_trajectory, -) -from .storage import ( - append_jsonl, - atomic_write_json, - completed_scan_keys, - load_jsonl, -) -from .trajectory import ( - fit_center_trajectory_curves, - maximum_center_non_target_drift_rad, - measure_center_trajectory_phase_angles, -) - - -STATE_PREFLIGHT = "PREFLIGHT" -STATE_WAIT_ROOT = "WAIT_ROOT_CONFIRM" -STATE_SCAN_ROOT = "SCAN_ROOT" -STATE_WAIT_TIP = "WAIT_TIP_CONFIRM" -STATE_SCAN_TIP = "SCAN_TIP" -STATE_VALIDATING = "VALIDATING" -STATE_PAUSED = "PAUSED" -STATE_ABORTED = "ABORTED" -STATE_COMPLETE = "COMPLETE" - - -def _stamp_ns(stamp: Any) -> int: - return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) - - -def _safe_name(value: str) -> str: - safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) - return safe or "UNSET" - - -class G20ThumbCalibrationNode(Node): - """Drive isolated G20 thumb sweeps and aggregate AprilTag rotations.""" - - def __init__(self) -> None: - super().__init__("g20_thumb_calibration") - self._declare_parameters() - self._load_parameters() - - self.session_dir.mkdir(parents=True, exist_ok=True) - self.raw_path = self.session_dir / "raw_samples.jsonl" - self.checkpoint_path = self.session_dir / "checkpoint.json" - self.validation_path = self.session_dir / "validation.json" - self.manifest_path = self.session_dir / "session_manifest.json" - self.final_path = self.session_dir / ( - f"g20_left_{_safe_name(self.serial_number)}_thumb_angle.json" - ) - - self._validate_existing_session() - self.records = load_jsonl(self.raw_path) - self.validation_records: list[dict[str, Any]] = [] - self.state = STATE_PREFLIGHT - self.state_reason = "waiting_for_camera_tags_and_sdk" - self.paused_from: str | None = None - self.current_phase: str | None = None - self.task_queue: deque[dict[str, Any]] = deque() - self.active_task: dict[str, Any] | None = None - self.pending_aggregate: dict[str, Any] | None = None - self.pending_sweep_observations: list[Observation] | None = None - self.fit: FitResult | None = None - self.validation_zero: dict[str, dict[str, Any]] = {} - self.validation_errors: list[float] = [] - - self.latest_state_u8: tuple[float, ...] = () - self.state_history: deque[StateSample] = deque(maxlen=600) - self.latest_hand_info: dict[str, Any] = {} - self.camera_intrinsics: dict[str, Any] = {} - self.pnp_camera_matrix: np.ndarray | None = None - self.camera_info_valid = False - self.camera_frame = "" - self.preflight_detection_flags: deque[bool] = deque( - maxlen=self.preflight_frames - ) - self.preflight_detection_times: deque[float] = deque( - maxlen=self.preflight_frames - ) - self.preflight_pnp_flags: deque[bool] = deque( - maxlen=self.preflight_frames - ) - self.preflight_observations: deque[Observation] = deque( - maxlen=self.preflight_frames - ) - - self.quality_cache: dict[int, dict[str, TagQuality]] = {} - self.corner_cache: dict[int, dict[str, np.ndarray]] = {} - self.transform_cache: dict[int, dict[str, tuple[float, ...]]] = {} - self.translation_cache: dict[ - int, dict[str, tuple[float, float, float]] - ] = {} - self.pnp_candidate_cache: dict[ - int, dict[str, tuple[SquareTagPose, ...]] - ] = {} - self.emitted_stamps: set[int] = set() - self.latest_corners: dict[str, np.ndarray] = {} - self.latest_tag_qualities: dict[str, TagQuality] = {} - self.latest_pnp_rejections: dict[str, str] = {} - self.latest_pnp_errors_px: dict[str, float] = {} - self.latest_trajectory_branch_quality: dict[str, Any] = {} - self.trajectory_branch_reports: list[dict[str, Any]] = [] - self.pnp_tracker = SquareTagPoseTracker( - maximum_reprojection_error_px=( - self.pnp_maximum_reprojection_error_px - ), - reprojection_tie_px=self.pnp_reprojection_tie_px, - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=( - self.pnp_maximum_translation_jump_m - ), - maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, - reset_after_seconds=self.pnp_tracker_reset_seconds, - ) - self.pnp_group_tracker = SquareTagGroupPoseTracker( - roles=("t0", "t3", "t4", "t5"), - adjacent_pairs=( - ("t0", "t3"), - ("t3", "t4"), - ("t4", "t5"), - ), - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=( - self.pnp_maximum_translation_jump_m - ), - relative_rotation_scale_rad=( - self.pnp_group_relative_rotation_scale_rad - ), - relative_translation_scale_m=( - self.pnp_group_relative_translation_scale_m - ), - reprojection_scale_px=( - self.pnp_trajectory_reprojection_scale_px - ), - reprojection_weight=self.pnp_group_reprojection_weight, - reset_after_seconds=self.pnp_tracker_reset_seconds, - ) - - self.collector = PointCollector( - stable_frames=self.stable_frames, - capture_frames=self.capture_frames, - minimum_settle_seconds=self.minimum_settle_seconds, - maximum_stable_spread_rad=self.maximum_stable_spread_rad, - stability_mode=( - "translation" - if self.angle_estimation_mode == "trajectory_center_3d" - else "rotation" - ), - maximum_stable_translation_spread_m=( - self.maximum_stable_translation_spread_m - ), - settle_timeout_seconds=self.settle_timeout_seconds, - capture_timeout_seconds=self.capture_timeout_seconds, - ) - self.sweep_collector = ContinuousSweepCollector( - endpoint_tolerance_u8=self.continuous_endpoint_tolerance_u8, - endpoint_hold_seconds=self.continuous_endpoint_hold_seconds, - timeout_seconds=self.continuous_timeout_seconds, - invalid_timeout_seconds=self.continuous_invalid_timeout_seconds, - minimum_valid_frames=self.continuous_minimum_valid_frames, - minimum_state_span_u8=self.continuous_minimum_state_span_u8, - ) - - self.command_publisher = self.create_publisher( - JointState, self.command_topic, 1 - ) - self.status_publisher = self.create_publisher( - String, "~/status", 10 - ) - self.status_text_publisher = self.create_publisher( - String, "~/status_text", 10 - ) - self.debug_publisher = None - self.last_debug_publish = 0.0 - if self.publish_debug_image: - self.debug_publisher = self.create_publisher( - Image, "~/debug_image", qos_profile_sensor_data - ) - self.bridge = CvBridge() - - self.create_subscription( - JointState, self.state_topic, self._state_callback, 10 - ) - self.create_subscription( - String, self.info_topic, self._info_callback, 10 - ) - self.create_subscription( - CameraInfo, self.camera_info_topic, self._camera_info_callback, 10 - ) - self.create_subscription( - AprilTagDetectionArray, - self.detections_topic, - self._detections_callback, - qos_profile_sensor_data, - ) - if self.publish_debug_image: - self.create_subscription( - Image, - self.image_topic, - self._image_callback, - qos_profile_sensor_data, - ) - - self.create_service( - Trigger, "~/start", self._start_callback - ) - self.create_service( - Trigger, "~/confirm_root_full_range", self._confirm_root_callback - ) - self.create_service( - Trigger, "~/confirm_tip_full_range", self._confirm_tip_callback - ) - self.create_service(Trigger, "~/pause", self._pause_callback) - self.create_service(Trigger, "~/resume", self._resume_callback) - self.create_service(Trigger, "~/abort", self._abort_callback) - - self.last_status_publish = 0.0 - self.timer = self.create_timer(0.05, self._timer_callback) - self._write_manifest() - self.get_logger().info( - f"Calibration session: {self.session_dir}; commands_enabled={self.commands_enabled}" - ) - - def _declare_parameters(self) -> None: - self.declare_parameter("serial_number", "UNSET") - self.declare_parameter("session_dir", "calibration_output/session") - self.declare_parameter("commands_enabled", True) - self.declare_parameter("calibration_speed", 15) - self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") - self.declare_parameter("state_topic", "/g20/cb_left_hand_state") - self.declare_parameter("info_topic", "/g20/cb_left_hand_info") - self.declare_parameter( - "camera_info_topic", "/camera/camera/color/camera_info" - ) - self.declare_parameter( - "image_topic", "/camera/camera/color/image_rect" - ) - self.declare_parameter("detections_topic", "/apriltag/detections") - self.declare_parameter("tf_topic", "/tf") - self.declare_parameter( - "angle_estimation_mode", "trajectory_center_3d" - ) - self.declare_parameter("camera_serial_number", "") - self.declare_parameter("rosbag_path", "") - self.declare_parameter("publish_debug_image", False) - self.declare_parameter("debug_max_rate_hz", 10.0) - self.declare_parameter("debug_scale", 0.5) - self.declare_parameter("tag_roles", ["t0", "t3", "t4", "t5"]) - self.declare_parameter("tag_ids", [0, 1, 2, 3]) - self.declare_parameter("tag_frames", ["tag_t0", "tag_t3", "tag_t4", "tag_t5"]) - self.declare_parameter("tag_sizes_m", [0.02, 0.02, 0.02, 0.02]) - self.declare_parameter("repetitions", 1) - self.declare_parameter("command_step", 8) - self.declare_parameter("scan_mode", "continuous") - self.declare_parameter("continuous_motion_mode", "endpoint") - self.declare_parameter("auto_start_tip", True) - self.declare_parameter("maximum_state_image_skew_ms", 150.0) - self.declare_parameter("continuous_endpoint_tolerance_u8", 2.0) - self.declare_parameter("continuous_endpoint_hold_seconds", 1.0) - self.declare_parameter("continuous_timeout_seconds", 90.0) - self.declare_parameter("continuous_invalid_timeout_seconds", 3.0) - self.declare_parameter("continuous_minimum_valid_frames", 40) - self.declare_parameter("continuous_minimum_state_span_u8", 240.0) - self.declare_parameter("continuous_minimum_bins", 32) - self.declare_parameter("continuous_maximum_bin_gap", 16) - self.declare_parameter("continuous_segment_minimum_seconds", 0.1) - self.declare_parameter("continuous_segment_timeout_seconds", 5.0) - self.declare_parameter("continuous_prepare_timeout_seconds", 30.0) - self.declare_parameter("preflight_frames", 150) - self.declare_parameter("minimum_detection_rate", 0.95) - self.declare_parameter("minimum_detection_hz", 15.0) - self.declare_parameter("maximum_hamming", 0) - self.declare_parameter("minimum_decision_margin", 30.0) - self.declare_parameter("minimum_edge_pixels", 40.0) - self.declare_parameter("maximum_static_std_deg", 0.5) - self.declare_parameter("pose_outlier_threshold_deg", 5.0) - self.declare_parameter("minimum_pose_inlier_rate", 0.95) - self.declare_parameter("pnp_minimum_valid_rate", 0.95) - self.declare_parameter("pnp_maximum_reprojection_error_px", 1.5) - self.declare_parameter("pnp_reprojection_tie_px", 1.5) - self.declare_parameter("pnp_maximum_pose_jump_deg", 35.0) - self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) - self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) - self.declare_parameter("pnp_tracker_reset_seconds", 5.0) - self.declare_parameter( - "pnp_group_relative_rotation_scale_deg", 5.0 - ) - self.declare_parameter( - "pnp_group_relative_translation_scale_m", 0.01 - ) - self.declare_parameter("pnp_group_reprojection_weight", 0.05) - self.declare_parameter( - "pnp_trajectory_reprojection_scale_px", 0.1 - ) - self.declare_parameter("pnp_rigid_rotation_scale_deg", 5.0) - self.declare_parameter("pnp_rigid_translation_scale_m", 0.01) - self.declare_parameter( - "pnp_rigid_p95_accepted_drift_deg", 8.0 - ) - self.declare_parameter( - "pnp_rigid_maximum_accepted_drift_deg", 15.0 - ) - self.declare_parameter( - "pnp_rigid_p95_accepted_distance_drift_m", 0.003 - ) - self.declare_parameter( - "pnp_rigid_maximum_accepted_distance_drift_m", 0.006 - ) - self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) - self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) - self.declare_parameter("trajectory_minimum_radius_m", 0.005) - self.declare_parameter("trajectory_minimum_arc_deg", 15.0) - self.declare_parameter( - "trajectory_maximum_root_role_disagreement_deg", 5.0 - ) - self.declare_parameter( - "trajectory_maximum_anchor_drift_m", 0.005 - ) - self.declare_parameter("passive_ip_multiplier", 1.02) - self.declare_parameter( - "trajectory_static_translation_outlier_m", 0.005 - ) - self.declare_parameter( - "trajectory_maximum_static_translation_rms_m", 0.002 - ) - self.declare_parameter("stable_frames", 5) - self.declare_parameter("capture_frames", 8) - self.declare_parameter("minimum_settle_seconds", 0.4) - self.declare_parameter("maximum_stable_spread_deg", 1.0) - self.declare_parameter( - "maximum_stable_translation_spread_m", 0.003 - ) - self.declare_parameter("settle_timeout_seconds", 5.0) - self.declare_parameter("capture_timeout_seconds", 5.0) - self.declare_parameter("validation_command_count", 5) - self.declare_parameter("validation_approach_minimum_seconds", 0.2) - self.declare_parameter("validation_approach_timeout_seconds", 10.0) - self.declare_parameter("validation_position_tolerance_u8", 2.0) - self.declare_parameter("validation_seed", 20260727) - self.declare_parameter("maximum_validation_mae_deg", 2.0) - self.declare_parameter("maximum_validation_p95_deg", 3.0) - self.declare_parameter("maximum_coupling_drift_deg", 2.0) - self.declare_parameter("minimum_ip_coupling_r_squared", 0.98) - self.declare_parameter("maximum_monotonic_correction_deg", 2.0) - self.declare_parameter("maximum_hysteresis_deg", 5.0) - - def _load_parameters(self) -> None: - value = lambda name: self.get_parameter(name).value - self.serial_number = str(value("serial_number")) - if self.serial_number == "UNSET": - raise ValueError("serial_number must be set for a hardware calibration") - self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() - self.commands_enabled = bool(value("commands_enabled")) - self.calibration_speed = int(value("calibration_speed")) - self.command_topic = str(value("command_topic")) - self.state_topic = str(value("state_topic")) - self.info_topic = str(value("info_topic")) - self.camera_info_topic = str(value("camera_info_topic")) - self.image_topic = str(value("image_topic")) - self.detections_topic = str(value("detections_topic")) - self.tf_topic = str(value("tf_topic")) - self.angle_estimation_mode = str(value("angle_estimation_mode")) - if self.angle_estimation_mode not in { - "image_plane_2d", - "pnp_3d", - "trajectory_center_3d", - }: - raise ValueError( - "angle_estimation_mode must be image_plane_2d, pnp_3d, " - "or trajectory_center_3d" - ) - self.uses_pnp = self.angle_estimation_mode in { - "pnp_3d", - "trajectory_center_3d", - } - self.camera_serial_number = str(value("camera_serial_number")) - self.rosbag_path = str(value("rosbag_path")) - self.publish_debug_image = bool(value("publish_debug_image")) - self.debug_max_rate_hz = float(value("debug_max_rate_hz")) - self.debug_scale = float(value("debug_scale")) - if self.debug_max_rate_hz <= 0.0: - raise ValueError("debug_max_rate_hz must be positive") - if not 0.1 <= self.debug_scale <= 1.0: - raise ValueError("debug_scale must be in [0.1, 1.0]") - - roles = [str(item) for item in value("tag_roles")] - ids = [int(item) for item in value("tag_ids")] - frames = [str(item).lstrip("/") for item in value("tag_frames")] - sizes = [float(item) for item in value("tag_sizes_m")] - if len(roles) != 4 or set(roles) != {"t0", "t3", "t4", "t5"}: - raise ValueError("tag_roles must contain t0, t3, t4, t5") - if not (len(ids) == len(frames) == len(sizes) == 4): - raise ValueError("tag_ids, tag_frames and tag_sizes_m must have length 4") - if len(set(ids)) != 4 or len(set(frames)) != 4: - raise ValueError("tag IDs and frames must be unique") - if any(size <= 0.0 for size in sizes): - raise ValueError("all tag sizes must be measured positive values") - self.role_by_id = dict(zip(ids, roles)) - self.role_by_frame = dict(zip(frames, roles)) - self.tag_config = { - role: {"id": tag_id, "frame": frame, "size_m": size} - for role, tag_id, frame, size in zip(roles, ids, frames, sizes) - } - - self.repetitions = int(value("repetitions")) - self.command_step = int(value("command_step")) - self.scan_mode = str(value("scan_mode")) - self.continuous_motion_mode = str(value("continuous_motion_mode")) - self.auto_start_tip = bool(value("auto_start_tip")) - self.maximum_state_image_skew_ns = int( - float(value("maximum_state_image_skew_ms")) * 1_000_000.0 - ) - self.continuous_endpoint_tolerance_u8 = float( - value("continuous_endpoint_tolerance_u8") - ) - self.continuous_endpoint_hold_seconds = float( - value("continuous_endpoint_hold_seconds") - ) - self.continuous_timeout_seconds = float( - value("continuous_timeout_seconds") - ) - self.continuous_invalid_timeout_seconds = float( - value("continuous_invalid_timeout_seconds") - ) - self.continuous_minimum_valid_frames = int( - value("continuous_minimum_valid_frames") - ) - self.continuous_minimum_state_span_u8 = float( - value("continuous_minimum_state_span_u8") - ) - self.continuous_minimum_bins = int(value("continuous_minimum_bins")) - self.continuous_maximum_bin_gap = int( - value("continuous_maximum_bin_gap") - ) - self.continuous_segment_minimum_seconds = float( - value("continuous_segment_minimum_seconds") - ) - self.continuous_segment_timeout_seconds = float( - value("continuous_segment_timeout_seconds") - ) - self.continuous_prepare_timeout_seconds = float( - value("continuous_prepare_timeout_seconds") - ) - self.preflight_frames = int(value("preflight_frames")) - self.minimum_detection_rate = float(value("minimum_detection_rate")) - self.minimum_detection_hz = float(value("minimum_detection_hz")) - self.maximum_hamming = int(value("maximum_hamming")) - self.minimum_decision_margin = float(value("minimum_decision_margin")) - self.minimum_edge_pixels = float(value("minimum_edge_pixels")) - self.maximum_static_std_rad = math.radians( - float(value("maximum_static_std_deg")) - ) - self.pose_outlier_threshold_rad = math.radians( - float(value("pose_outlier_threshold_deg")) - ) - self.minimum_pose_inlier_rate = float( - value("minimum_pose_inlier_rate") - ) - self.pnp_minimum_valid_rate = float(value("pnp_minimum_valid_rate")) - self.pnp_maximum_reprojection_error_px = float( - value("pnp_maximum_reprojection_error_px") - ) - self.pnp_reprojection_tie_px = float( - value("pnp_reprojection_tie_px") - ) - self.pnp_maximum_pose_jump_rad = math.radians( - float(value("pnp_maximum_pose_jump_deg")) - ) - self.pnp_maximum_translation_jump_m = float( - value("pnp_maximum_translation_jump_m") - ) - self.pnp_maximum_tag_tilt_rad = math.radians( - float(value("pnp_maximum_tag_tilt_deg")) - ) - self.pnp_tracker_reset_seconds = float( - value("pnp_tracker_reset_seconds") - ) - self.pnp_group_relative_rotation_scale_rad = math.radians( - float(value("pnp_group_relative_rotation_scale_deg")) - ) - self.pnp_group_relative_translation_scale_m = float( - value("pnp_group_relative_translation_scale_m") - ) - self.pnp_group_reprojection_weight = float( - value("pnp_group_reprojection_weight") - ) - self.pnp_trajectory_reprojection_scale_px = float( - value("pnp_trajectory_reprojection_scale_px") - ) - self.pnp_rigid_rotation_scale_rad = math.radians( - float(value("pnp_rigid_rotation_scale_deg")) - ) - self.pnp_rigid_translation_scale_m = float( - value("pnp_rigid_translation_scale_m") - ) - self.pnp_rigid_p95_accepted_drift_rad = math.radians( - float(value("pnp_rigid_p95_accepted_drift_deg")) - ) - self.pnp_rigid_maximum_accepted_drift_rad = math.radians( - float(value("pnp_rigid_maximum_accepted_drift_deg")) - ) - self.pnp_rigid_p95_accepted_distance_drift_m = float( - value("pnp_rigid_p95_accepted_distance_drift_m") - ) - self.pnp_rigid_maximum_accepted_distance_drift_m = float( - value("pnp_rigid_maximum_accepted_distance_drift_m") - ) - self.trajectory_maximum_plane_rms_m = float( - value("trajectory_maximum_plane_rms_m") - ) - self.trajectory_maximum_radial_rms_m = float( - value("trajectory_maximum_radial_rms_m") - ) - self.trajectory_minimum_radius_m = float( - value("trajectory_minimum_radius_m") - ) - self.trajectory_minimum_arc_rad = math.radians( - float(value("trajectory_minimum_arc_deg")) - ) - self.trajectory_maximum_root_role_disagreement_rad = math.radians( - float( - value( - "trajectory_maximum_root_role_disagreement_deg" - ) - ) - ) - self.trajectory_maximum_anchor_drift_m = float( - value("trajectory_maximum_anchor_drift_m") - ) - self.passive_ip_multiplier = float( - value("passive_ip_multiplier") - ) - self.trajectory_static_translation_outlier_m = float( - value("trajectory_static_translation_outlier_m") - ) - self.trajectory_maximum_static_translation_rms_m = float( - value("trajectory_maximum_static_translation_rms_m") - ) - self.stable_frames = int(value("stable_frames")) - self.capture_frames = int(value("capture_frames")) - self.minimum_settle_seconds = float(value("minimum_settle_seconds")) - self.maximum_stable_spread_rad = math.radians( - float(value("maximum_stable_spread_deg")) - ) - self.maximum_stable_translation_spread_m = float( - value("maximum_stable_translation_spread_m") - ) - self.settle_timeout_seconds = float(value("settle_timeout_seconds")) - self.capture_timeout_seconds = float(value("capture_timeout_seconds")) - self.validation_command_count = int(value("validation_command_count")) - self.validation_approach_minimum_seconds = float( - value("validation_approach_minimum_seconds") - ) - self.validation_approach_timeout_seconds = float( - value("validation_approach_timeout_seconds") - ) - self.validation_position_tolerance_u8 = float( - value("validation_position_tolerance_u8") - ) - self.validation_seed = int(value("validation_seed")) - self.maximum_validation_mae_rad = math.radians( - float(value("maximum_validation_mae_deg")) - ) - self.maximum_validation_p95_rad = math.radians( - float(value("maximum_validation_p95_deg")) - ) - self.maximum_coupling_drift_rad = math.radians( - float(value("maximum_coupling_drift_deg")) - ) - self.minimum_ip_coupling_r_squared = float( - value("minimum_ip_coupling_r_squared") - ) - self.maximum_monotonic_correction_rad = math.radians( - float(value("maximum_monotonic_correction_deg")) - ) - self.maximum_hysteresis_rad = math.radians( - float(value("maximum_hysteresis_deg")) - ) - if self.repetitions < 1: - raise ValueError("repetitions must be positive") - if not 0 <= self.calibration_speed <= 255: - raise ValueError("calibration_speed must be in [0, 255]") - if self.scan_mode not in {"continuous", "point"}: - raise ValueError("scan_mode must be continuous or point") - if self.continuous_motion_mode not in {"endpoint", "paced"}: - raise ValueError( - "continuous_motion_mode must be endpoint or paced" - ) - if not 1 <= self.command_step <= 255: - raise ValueError("command_step must be in [1, 255]") - if ( - not math.isfinite(self.passive_ip_multiplier) - or not 0.5 <= self.passive_ip_multiplier <= 1.5 - ): - raise ValueError( - "passive_ip_multiplier must be finite and in [0.5, 1.5]" - ) - if self.maximum_state_image_skew_ns < 0: - raise ValueError("maximum_state_image_skew_ms must be non-negative") - if self.continuous_minimum_bins < 3: - raise ValueError("continuous_minimum_bins must be at least 3") - if self.continuous_maximum_bin_gap < 1: - raise ValueError("continuous_maximum_bin_gap must be positive") - if self.continuous_segment_minimum_seconds < 0.0: - raise ValueError( - "continuous_segment_minimum_seconds must be non-negative" - ) - if self.continuous_segment_timeout_seconds <= 0.0: - raise ValueError( - "continuous_segment_timeout_seconds must be positive" - ) - if self.continuous_prepare_timeout_seconds <= 0.0: - raise ValueError( - "continuous_prepare_timeout_seconds must be positive" - ) - if self.preflight_frames < 3: - raise ValueError("preflight_frames must be at least 3") - if not 0.0 <= self.minimum_detection_rate <= 1.0: - raise ValueError("minimum_detection_rate must be in [0, 1]") - if self.minimum_detection_hz <= 0.0: - raise ValueError("minimum_detection_hz must be positive") - if self.pose_outlier_threshold_rad <= 0.0: - raise ValueError("pose_outlier_threshold_deg must be positive") - if not 0.0 <= self.minimum_pose_inlier_rate <= 1.0: - raise ValueError("minimum_pose_inlier_rate must be in [0, 1]") - if not 0.0 <= self.pnp_minimum_valid_rate <= 1.0: - raise ValueError("pnp_minimum_valid_rate must be in [0, 1]") - if self.pnp_maximum_reprojection_error_px <= 0.0: - raise ValueError( - "pnp_maximum_reprojection_error_px must be positive" - ) - if self.pnp_reprojection_tie_px < 0.0: - raise ValueError("pnp_reprojection_tie_px must be non-negative") - if self.pnp_maximum_pose_jump_rad <= 0.0: - raise ValueError("pnp_maximum_pose_jump_deg must be positive") - if self.pnp_maximum_translation_jump_m <= 0.0: - raise ValueError( - "pnp_maximum_translation_jump_m must be positive" - ) - if not 0.0 < self.pnp_maximum_tag_tilt_rad < math.pi / 2.0: - raise ValueError("pnp_maximum_tag_tilt_deg must be in (0, 90)") - if self.pnp_tracker_reset_seconds <= 0.0: - raise ValueError("pnp_tracker_reset_seconds must be positive") - if self.pnp_group_relative_rotation_scale_rad <= 0.0: - raise ValueError( - "pnp_group_relative_rotation_scale_deg must be positive" - ) - if self.pnp_group_relative_translation_scale_m <= 0.0: - raise ValueError( - "pnp_group_relative_translation_scale_m must be positive" - ) - if self.pnp_group_reprojection_weight < 0.0: - raise ValueError( - "pnp_group_reprojection_weight must be non-negative" - ) - if self.pnp_trajectory_reprojection_scale_px <= 0.0: - raise ValueError( - "pnp_trajectory_reprojection_scale_px must be positive" - ) - if self.pnp_rigid_rotation_scale_rad <= 0.0: - raise ValueError( - "pnp_rigid_rotation_scale_deg must be positive" - ) - if self.pnp_rigid_translation_scale_m <= 0.0: - raise ValueError( - "pnp_rigid_translation_scale_m must be positive" - ) - if self.pnp_rigid_p95_accepted_drift_rad <= 0.0: - raise ValueError( - "pnp_rigid_p95_accepted_drift_deg must be positive" - ) - if self.pnp_rigid_maximum_accepted_drift_rad <= 0.0: - raise ValueError( - "pnp_rigid_maximum_accepted_drift_deg must be positive" - ) - if ( - self.pnp_rigid_p95_accepted_drift_rad - > self.pnp_rigid_maximum_accepted_drift_rad - ): - raise ValueError( - "pnp rigid p95 drift limit must not exceed maximum limit" - ) - if min( - self.pnp_rigid_p95_accepted_distance_drift_m, - self.pnp_rigid_maximum_accepted_distance_drift_m, - ) <= 0.0: - raise ValueError( - "pnp rigid distance drift limits must be positive" - ) - if ( - self.pnp_rigid_p95_accepted_distance_drift_m - > self.pnp_rigid_maximum_accepted_distance_drift_m - ): - raise ValueError( - "pnp rigid p95 distance drift limit must not exceed " - "maximum limit" - ) - if min( - self.trajectory_maximum_plane_rms_m, - self.trajectory_maximum_radial_rms_m, - self.trajectory_minimum_radius_m, - self.trajectory_minimum_arc_rad, - self.trajectory_maximum_root_role_disagreement_rad, - self.trajectory_maximum_anchor_drift_m, - self.trajectory_static_translation_outlier_m, - self.trajectory_maximum_static_translation_rms_m, - self.maximum_stable_translation_spread_m, - ) <= 0.0: - raise ValueError("trajectory centre-fit thresholds must be positive") - if not 1 <= self.validation_command_count <= 254: - raise ValueError("validation_command_count must be in [1, 254]") - if self.validation_approach_minimum_seconds < 0.0: - raise ValueError( - "validation_approach_minimum_seconds must be non-negative" - ) - if self.validation_approach_timeout_seconds <= 0.0: - raise ValueError( - "validation_approach_timeout_seconds must be positive" - ) - if self.validation_position_tolerance_u8 < 0.0: - raise ValueError( - "validation_position_tolerance_u8 must be non-negative" - ) - - def _camera_info_callback(self, message: CameraInfo) -> None: - camera_info_valid = ( - message.width > 0 - and message.height > 0 - and len(message.p) == 12 - and float(message.p[0]) > 0.0 - and float(message.p[5]) > 0.0 - ) - new_camera_matrix: np.ndarray | None = None - if camera_info_valid: - projection = np.asarray(message.p, dtype=float).reshape(3, 4) - new_camera_matrix = projection[:, :3].copy() - camera_info_valid = bool( - np.all(np.isfinite(new_camera_matrix)) - and new_camera_matrix[0, 0] > 0.0 - and new_camera_matrix[1, 1] > 0.0 - ) - if ( - new_camera_matrix is not None - and self.pnp_camera_matrix is not None - and not np.allclose( - new_camera_matrix, - self.pnp_camera_matrix, - rtol=1.0e-9, - atol=1.0e-9, - ) - ): - self.pnp_tracker.reset() - self.pnp_group_tracker.reset() - self.camera_info_valid = camera_info_valid - self.pnp_camera_matrix = ( - new_camera_matrix if camera_info_valid else None - ) - self.camera_frame = message.header.frame_id - self.camera_intrinsics = { - "frame_id": message.header.frame_id, - "width": int(message.width), - "height": int(message.height), - "distortion_model": message.distortion_model, - "d": [float(value) for value in message.d], - "k": [float(value) for value in message.k], - "p": [float(value) for value in message.p], - "rectified_camera_matrix": ( - [] - if self.pnp_camera_matrix is None - else [ - float(value) - for value in self.pnp_camera_matrix.reshape(-1) - ] - ), - "binning_x": int(message.binning_x), - "binning_y": int(message.binning_y), - "roi": { - "x_offset": int(message.roi.x_offset), - "y_offset": int(message.roi.y_offset), - "height": int(message.roi.height), - "width": int(message.roi.width), - "do_rectify": bool(message.roi.do_rectify), - }, - } - - def _state_callback(self, message: JointState) -> None: - if len(message.position) != 20: - return - if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): - lookup = dict(zip(message.name, message.position)) - self.latest_state_u8 = tuple( - float(lookup[name]) for name in COMMAND_NAMES - ) - else: - self.latest_state_u8 = tuple(float(value) for value in message.position) - stamp_ns = _stamp_ns(message.header.stamp) - if stamp_ns <= 0: - stamp_ns = int(self.get_clock().now().nanoseconds) - if not self.state_history or stamp_ns > self.state_history[-1].stamp_ns: - self.state_history.append( - StateSample( - stamp_ns=stamp_ns, - position_u8=self.latest_state_u8, - ) - ) - - def _info_callback(self, message: String) -> None: - try: - value = json.loads(message.data) - except json.JSONDecodeError: - return - if isinstance(value, dict): - self.latest_hand_info = value - - def _detections_callback(self, message: AprilTagDetectionArray) -> None: - stamp = _stamp_ns(message.header.stamp) - qualities: dict[str, TagQuality] = {} - latest_corners: dict[str, np.ndarray] = {} - for detection in message.detections: - role = self.role_by_id.get(int(detection.id)) - if role is None: - continue - corners = np.asarray( - [[float(point.x), float(point.y)] for point in detection.corners], - dtype=float, - ) - if corners.shape != (4, 2): - continue - edges = np.linalg.norm(corners - np.roll(corners, -1, axis=0), axis=1) - qualities[role] = TagQuality( - hamming=int(detection.hamming), - decision_margin=float(detection.decision_margin), - edge_pixels=float(np.mean(edges)), - ) - latest_corners[role] = corners - - transforms: dict[str, tuple[float, ...]] = {} - translations: dict[str, tuple[float, float, float]] = {} - pose_candidates: dict[str, tuple[SquareTagPose, ...]] = {} - pnp_rejections: dict[str, str] = {} - pnp_errors: dict[str, float] = {} - if self.uses_pnp: - if self.pnp_camera_matrix is None: - pnp_rejections = { - role: "camera_info_not_ready" - for role in self.tag_config - } - else: - for role in self.tag_config: - corners = latest_corners.get(role) - if corners is None: - pnp_rejections[role] = "tag_not_detected" - continue - quality = qualities.get(role) - if quality is None or not self._quality_is_valid( - quality, - include_pnp=False, - ): - pnp_rejections[role] = "tag_quality_invalid" - continue - pose, reason = self.pnp_tracker.estimate( - role, - corners, - tag_size_m=float( - self.tag_config[role]["size_m"] - ), - camera_matrix=self.pnp_camera_matrix, - stamp_ns=stamp, - reprojection_tie_px=( - self._pnp_reprojection_tie_for_role(role) - ), - ) - pose_candidates[role] = ( - self.pnp_tracker.last_candidates_by_role.get( - role, - (), - ) - ) - if pose is None and not pose_candidates[role]: - pnp_rejections[role] = reason - if ( - set(pose_candidates) == set(self.tag_config) - and all(pose_candidates.values()) - ): - selected_group, group_reason = ( - self.pnp_group_tracker.select( - pose_candidates, - stamp_ns=stamp, - ) - ) - if selected_group is None: - pnp_rejections["_group"] = group_reason - else: - transforms.clear() - pnp_errors.clear() - pnp_rejections.clear() - for role, pose in selected_group.items(): - transforms[role] = pose.quaternion_xyzw - translations[role] = pose.translation_xyz_m - pnp_errors[role] = pose.reprojection_error_px - quality = qualities[role] - qualities[role] = replace( - quality, - reprojection_error_px=( - pose.reprojection_error_px - ), - ) - self.transform_cache[stamp] = transforms - self.translation_cache[stamp] = translations - self.pnp_candidate_cache[stamp] = pose_candidates - self.preflight_pnp_flags.append( - set(transforms) == set(self.tag_config) - ) - self.latest_pnp_rejections = pnp_rejections - self.latest_pnp_errors_px = pnp_errors - - self.latest_corners = latest_corners - self.latest_tag_qualities = dict(qualities) - self.quality_cache[stamp] = qualities - self.corner_cache[stamp] = latest_corners - detection_good = set(qualities) == set(self.tag_config) and all( - self._quality_is_valid(quality, include_pnp=False) - for quality in qualities.values() - ) - all_good = detection_good and all( - self._quality_is_valid(quality, include_pnp=True) - for quality in qualities.values() - ) - self.preflight_detection_flags.append(detection_good) - self.preflight_detection_times.append(time.monotonic()) - if not all_good and self.collector.active: - self.collector.mark_invalid_frame() - self._try_emit_observation(stamp) - self._trim_caches() - - def _pnp_reprojection_tie_for_role(self, role: str) -> float: - """Lock every visible tag to its temporally continuous IPPE branch.""" - del role - return self.pnp_reprojection_tie_px - - def _quality_is_valid( - self, - quality: TagQuality, - *, - include_pnp: bool = True, - ) -> bool: - return tag_quality_is_valid( - quality, - maximum_hamming=self.maximum_hamming, - minimum_decision_margin=self.minimum_decision_margin, - minimum_edge_pixels=self.minimum_edge_pixels, - maximum_reprojection_error_px=( - self.pnp_maximum_reprojection_error_px - if include_pnp and self.uses_pnp - else None - ), - ) - - def _try_emit_observation(self, stamp: int) -> None: - if stamp in self.emitted_stamps: - return - quality = self.quality_cache.get(stamp) - required = set(self.tag_config) - if quality is None: - return - if set(quality) != required: - return - if not all(self._quality_is_valid(value) for value in quality.values()): - return - if self.angle_estimation_mode == "image_plane_2d": - corners = self.corner_cache.get(stamp) - if corners is None or set(corners) != required: - return - transforms = { - role: image_plane_tag_quaternion_xyzw(points) - for role, points in corners.items() - } - else: - transforms = self.transform_cache.get(stamp) - if transforms is None or set(transforms) != required: - return - translations = self.translation_cache.get(stamp, {}) - if ( - self.angle_estimation_mode == "trajectory_center_3d" - and set(translations) != required - ): - return - relative = { - PAIR_ROOT: relative_quaternion_xyzw( - transforms["t0"], transforms["t3"] - ), - PAIR_MCP: relative_quaternion_xyzw( - transforms["t3"], transforms["t4"] - ), - PAIR_IP: relative_quaternion_xyzw( - transforms["t4"], transforms["t5"] - ), - } - matched_state = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) - if matched_state is None: - state_u8 = self.latest_state_u8 - state_sync_error_ns = None - state_stamp_ns = None - else: - state_u8, state_sync_error_ns = matched_state - state_stamp_ns = stamp - observation = Observation( - stamp_ns=stamp, - received_at=time.monotonic(), - relative_quaternion_xyzw=relative, - tag_quality=quality, - state_u8=state_u8, - state_stamp_ns=state_stamp_ns, - state_sync_error_ns=state_sync_error_ns, - tag_quaternion_xyzw=transforms, - tag_translation_xyz_m=translations, - tag_pose_candidates=self.pnp_candidate_cache.get(stamp, {}), - ) - self.emitted_stamps.add(stamp) - self.preflight_observations.append(observation) - if self.collector.active: - result = self.collector.add(observation, time.monotonic()) - if result is not None: - self.pending_aggregate = result - if self.sweep_collector.active: - result = self.sweep_collector.add(observation, time.monotonic()) - if result is not None: - self.pending_sweep_observations = result - - def _trim_caches(self) -> None: - all_stamps = sorted( - set(self.quality_cache) - | set(self.corner_cache) - | set(self.transform_cache) - | set(self.translation_cache) - | set(self.pnp_candidate_cache) - ) - for stamp in all_stamps[:-300]: - self.quality_cache.pop(stamp, None) - self.corner_cache.pop(stamp, None) - self.transform_cache.pop(stamp, None) - self.translation_cache.pop(stamp, None) - self.pnp_candidate_cache.pop(stamp, None) - self.emitted_stamps.discard(stamp) - - def _image_callback(self, message: Image) -> None: - if ( - self.debug_publisher is None - or self.debug_publisher.get_subscription_count() < 1 - ): - return - now = time.monotonic() - if now - self.last_debug_publish < 1.0 / self.debug_max_rate_hz: - return - self.last_debug_publish = now - try: - image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8") - except Exception as error: - self.get_logger().warning(f"debug image conversion failed: {error}") - return - if self.debug_scale != 1.0: - image = cv2.resize( - image, - None, - fx=self.debug_scale, - fy=self.debug_scale, - interpolation=cv2.INTER_AREA, - ) - colors = { - "t0": (0, 200, 0), - "t3": (0, 128, 255), - "t4": (200, 0, 200), - "t5": (0, 220, 255), - } - for role, corners in self.latest_corners.items(): - points = np.rint(corners * self.debug_scale).astype(np.int32) - cv2.polylines(image, [points], True, colors[role], 2) - origin = tuple(points[0]) - cv2.putText( - image, - role.upper(), - origin, - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - colors[role], - 2, - ) - cv2.putText( - image, - f"{self.state}: {self.state_reason}", - (20, 32), - cv2.FONT_HERSHEY_SIMPLEX, - 0.65, - (0, 0, 255) if self.state in {STATE_PAUSED, STATE_ABORTED} else (0, 255, 0), - 2, - ) - output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") - output.header = message.header - self.debug_publisher.publish(output) - - def _timer_callback(self) -> None: - now = time.monotonic() - self.collector.poll(now) - self.sweep_collector.poll(now) - if self.collector.state == "failed" and self.state not in { - STATE_PAUSED, - STATE_ABORTED, - }: - self._pause(f"point_capture_failed:{self.collector.reason}") - if self.sweep_collector.state == "failed" and self.state not in { - STATE_PAUSED, - STATE_ABORTED, - }: - self._pause( - f"continuous_sweep_failed:{self.sweep_collector.reason}" - ) - - if self.pending_aggregate is not None: - aggregate = self.pending_aggregate - self.pending_aggregate = None - self._complete_active_task(aggregate) - if self.pending_sweep_observations is not None: - observations = self.pending_sweep_observations - self.pending_sweep_observations = None - self._complete_continuous_sweep(observations) - - if ( - self.active_task is not None - and self.active_task.get("kind") == "continuous_sweep" - and self.active_task.get("stage") == "prepare_move" - ): - self._poll_continuous_prepare(now) - - if ( - self.active_task is not None - and self.active_task.get("kind") == "continuous_sweep" - and self.active_task.get("stage") == "sweep" - and self.active_task.get("motion_mode") == "paced" - and self.sweep_collector.active - ): - self._poll_continuous_segment(now) - - if ( - self.state == STATE_VALIDATING - and self.active_task is not None - and self.active_task.get("kind") == "validation" - and self.active_task.get("stage") == "approach" - ): - self._poll_validation_approach(now) - - if self.state == STATE_PREFLIGHT: - passed, reason = self._preflight_status() - self.state_reason = reason - if passed: - self._select_post_preflight_state() - elif self.state in {STATE_SCAN_ROOT, STATE_SCAN_TIP, STATE_VALIDATING}: - if ( - self.active_task is None - and not self.collector.active - and not self.sweep_collector.active - ): - self._start_next_task() - - if now - self.last_status_publish >= 0.5: - self._publish_status() - self.last_status_publish = now - - def _preflight_status(self) -> tuple[bool, str]: - if not self.camera_info_valid: - return False, "camera_info_not_ready" - if len(self.latest_state_u8) != 20: - return False, "g20_state_not_ready" - if self.command_publisher.get_subscription_count() < 1: - return False, "g20_sdk_not_subscribed" - if self._has_other_command_publishers(): - return False, "another_command_publisher_exists" - if len(self.preflight_detection_flags) < self.preflight_frames: - return ( - False, - "collecting_detection_preflight:" - f"{len(self.preflight_detection_flags)}/{self.preflight_frames}", - ) - detection_rate = float(np.mean(self.preflight_detection_flags)) - if detection_rate < self.minimum_detection_rate: - return False, f"detection_rate_too_low:{detection_rate:.3f}" - detection_times = list(self.preflight_detection_times) - duration = detection_times[-1] - detection_times[0] - detection_hz = ( - (len(detection_times) - 1) / duration - if duration > 0.0 - else 0.0 - ) - if detection_hz < self.minimum_detection_hz: - return False, f"detection_hz_too_low:{detection_hz:.2f}" - if self.uses_pnp: - if len(self.preflight_pnp_flags) < self.preflight_frames: - return ( - False, - "collecting_pnp_preflight:" - f"{len(self.preflight_pnp_flags)}/{self.preflight_frames}", - ) - pnp_valid_rate = float(np.mean(self.preflight_pnp_flags)) - if pnp_valid_rate < self.pnp_minimum_valid_rate: - return False, f"pnp_valid_rate_too_low:{pnp_valid_rate:.3f}" - if len(self.preflight_observations) < self.preflight_frames: - return ( - False, - "collecting_static_preflight:" - f"{len(self.preflight_observations)}/{self.preflight_frames}", - ) - observations = list(self.preflight_observations) - if self.angle_estimation_mode == "trajectory_center_3d": - for pair, (parent, child) in TAG_PAIR_ROLES.items(): - vectors = np.asarray( - [ - np.asarray( - observation.tag_translation_xyz_m[child], - dtype=float, - ) - - np.asarray( - observation.tag_translation_xyz_m[parent], - dtype=float, - ) - for observation in observations - ], - dtype=float, - ) - reference = np.median(vectors, axis=0) - residuals = np.linalg.norm(vectors - reference, axis=1) - inliers = ( - residuals - <= self.trajectory_static_translation_outlier_m - ) - inlier_rate = float(np.mean(inliers)) - if inlier_rate < self.minimum_pose_inlier_rate: - return False, ( - f"{pair}_position_inlier_rate_too_low:" - f"{inlier_rate:.3f}" - ) - rms = float( - np.sqrt(np.mean(np.square(residuals[inliers]))) - ) - if ( - rms - > self.trajectory_maximum_static_translation_rms_m - ): - return False, ( - f"{pair}_static_position_rms_too_large:" - f"{1000.0 * rms:.3f}mm" - ) - else: - for pair in PAIR_NAMES: - quaternions = [ - observation.relative_quaternion_xyzw[pair] - for observation in observations - ] - inlier_rate = rotation_inlier_fraction( - quaternions, - outlier_threshold_rad=self.pose_outlier_threshold_rad, - ) - if inlier_rate < self.minimum_pose_inlier_rate: - return False, ( - f"{pair}_pose_inlier_rate_too_low:{inlier_rate:.3f}" - ) - rms = rotation_rms_rad( - quaternions, - outlier_threshold_rad=self.pose_outlier_threshold_rad, - ) - if rms > self.maximum_static_std_rad: - return False, ( - f"{pair}_static_rms_too_large:{rms:.6f}" - ) - return True, "preflight_passed" - - def _has_other_command_publishers(self) -> bool: - try: - publishers = self.get_publishers_info_by_topic(self.command_topic) - except Exception: - return True - return any( - endpoint.node_name != self.get_name() - or endpoint.node_namespace != self.get_namespace() - for endpoint in publishers - ) - - def _select_post_preflight_state(self) -> None: - observations = list(self.preflight_observations) - static_position_rms_m: dict[str, float] = {} - position_inlier_rate: dict[str, float] = {} - if self.angle_estimation_mode == "trajectory_center_3d": - for pair, (parent, child) in TAG_PAIR_ROLES.items(): - vectors = np.asarray( - [ - np.asarray( - observation.tag_translation_xyz_m[child], - dtype=float, - ) - - np.asarray( - observation.tag_translation_xyz_m[parent], - dtype=float, - ) - for observation in observations - ], - dtype=float, - ) - reference = np.median(vectors, axis=0) - residuals = np.linalg.norm(vectors - reference, axis=1) - inliers = ( - residuals - <= self.trajectory_static_translation_outlier_m - ) - position_inlier_rate[pair] = float(np.mean(inliers)) - static_position_rms_m[pair] = float( - np.sqrt(np.mean(np.square(residuals[inliers]))) - ) - self._write_manifest( - { - "preflight": { - "passed": True, - "detection_rate": float( - np.mean(self.preflight_detection_flags) - ), - "detection_hz": ( - (len(self.preflight_detection_times) - 1) - / ( - self.preflight_detection_times[-1] - - self.preflight_detection_times[0] - ) - ), - "pnp_valid_rate": ( - float(np.mean(self.preflight_pnp_flags)) - if self.uses_pnp - and self.preflight_pnp_flags - else None - ), - "maximum_reprojection_error_px": { - role: float( - max( - observation.tag_quality[ - role - ].reprojection_error_px - or 0.0 - for observation in observations - ) - ) - for role in self.tag_config - } - if self.uses_pnp - else {}, - "static_rms_rad": { - pair: rotation_rms_rad( - [ - observation.relative_quaternion_xyzw[pair] - for observation in observations - ], - outlier_threshold_rad=self.pose_outlier_threshold_rad, - ) - for pair in PAIR_NAMES - }, - "pose_inlier_rate": { - pair: rotation_inlier_fraction( - [ - observation.relative_quaternion_xyzw[pair] - for observation in observations - ], - outlier_threshold_rad=self.pose_outlier_threshold_rad, - ) - for pair in PAIR_NAMES - }, - "static_position_rms_m": static_position_rms_m, - "position_inlier_rate": position_inlier_rate, - } - } - ) - if self.final_path.exists(): - self.state = STATE_COMPLETE - self.state_reason = "existing_final_result" - elif not self._phase_complete(PHASE_ROOT): - self.state = STATE_WAIT_ROOT - self.state_reason = "call_start" - elif not self._phase_complete(PHASE_TIP): - self.state = STATE_WAIT_TIP - self.state_reason = "call_start_to_resume_tip" - else: - self._begin_validation() - - def _phase_complete(self, phase: str) -> bool: - if self.scan_mode == "continuous": - return all( - self._continuous_direction_complete(phase, direction) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - completed = completed_scan_keys(self.records) - return all( - (phase, cycle, direction, command) in completed - for cycle, direction, command in scan_targets( - self.repetitions, self.command_step - ) - ) - - def _continuous_direction_complete( - self, phase: str, direction: str - ) -> bool: - commands = sorted( - { - int(record["command_u8"]) - for record in self.records - if record.get("kind", "sample") == "sample" - and record.get("scan_mode") == "continuous" - and record.get("phase") == phase - and record.get("direction") == direction - } - ) - if ( - len(commands) < self.continuous_minimum_bins - or not commands - or commands[0] != 0 - or commands[-1] != 255 - ): - return False - return max(np.diff(commands), default=0) <= self.continuous_maximum_bin_gap - - def _start_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state not in {STATE_WAIT_ROOT, STATE_WAIT_TIP}: - response.success = False - response.message = f"current state is {self.state}" - return response - if not self.commands_enabled: - response.success = False - response.message = "commands_enabled=false; restart for hardware scan" - return response - phase = PHASE_ROOT if self.state == STATE_WAIT_ROOT else PHASE_TIP - self._begin_scan(phase) - response.success = True - response.message = ( - f"{self.scan_mode} calibration armed; root, tip and validation " - "run automatically" - if phase == PHASE_ROOT and self.auto_start_tip - else f"{phase} scan armed" - ) - return response - - def _confirm_root_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_WAIT_ROOT: - response.success = False - response.message = f"current state is {self.state}" - return response - if not self.commands_enabled: - response.success = False - response.message = "commands_enabled=false; restart for hardware scan" - return response - self._begin_scan(PHASE_ROOT) - response.success = True - response.message = "root full-range scan armed" - return response - - def _confirm_tip_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_WAIT_TIP: - response.success = False - response.message = f"current state is {self.state}" - return response - if not self.commands_enabled: - response.success = False - response.message = "commands_enabled=false; restart for hardware scan" - return response - self._begin_scan(PHASE_TIP) - response.success = True - response.message = "tip full-range scan armed" - return response - - def _pause_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state not in {STATE_SCAN_ROOT, STATE_SCAN_TIP, STATE_VALIDATING}: - response.success = False - response.message = f"cannot pause from {self.state}" - return response - self._pause("operator_pause") - response.success = True - response.message = "calibration paused; current command is held" - return response - - def _resume_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_PAUSED or self.paused_from is None: - response.success = False - response.message = "calibration is not resumably paused" - return response - self.state = self.paused_from - self.state_reason = "resuming_current_task" - self.paused_from = None - if ( - self.active_task is not None - and self.active_task.get("kind") == "continuous_sweep" - ): - # An end-to-end command cannot resume halfway with correct - # direction/history. Return to its start endpoint and repeat only - # the interrupted sweep. - self.active_task["stage"] = "prepare" - self.active_task["command_u8"] = int( - self.active_task["start_u8"] - ) - self.active_task.pop("start_aggregate", None) - self._restart_active_task() - response.success = True - response.message = "calibration resumed" - return response - - def _abort_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - self.collector.state = "idle" - self.sweep_collector.state = "idle" - self.pending_aggregate = None - self.pending_sweep_observations = None - self.state = STATE_ABORTED - self.state_reason = "operator_abort_current_command_held" - self._write_checkpoint() - response.success = True - response.message = "aborted without sending a return command" - return response - - def _pause(self, reason: str) -> None: - if self.state == STATE_PAUSED: - return - self.paused_from = self.state - self.collector.state = "idle" - self.sweep_collector.state = "idle" - self.pending_aggregate = None - self.pending_sweep_observations = None - self.state = STATE_PAUSED - self.state_reason = reason - self._write_checkpoint() - - def _begin_scan(self, phase: str) -> None: - if self.scan_mode == "continuous": - tasks: list[dict[str, Any]] = [] - for direction, start, target in ( - (DIRECTION_DECREASING, 255, 0), - (DIRECTION_INCREASING, 0, 255), - ): - if self._continuous_direction_complete(phase, direction): - continue - tasks.append( - { - "kind": "continuous_sweep", - "scan_mode": "continuous", - "motion_mode": self.continuous_motion_mode, - "phase": phase, - "cycle": 0, - "direction": direction, - "start_u8": start, - "target_u8": target, - "command_u8": start, - "stage": "prepare", - } - ) - self.task_queue = deque(tasks) - else: - completed = completed_scan_keys(self.records) - self.task_queue = deque( - { - "kind": "sample", - "scan_mode": "point", - "phase": phase, - "cycle": cycle, - "direction": direction, - "command_u8": command, - } - for cycle, direction, command in scan_targets( - self.repetitions, self.command_step - ) - if (phase, cycle, direction, command) not in completed - ) - self.current_phase = phase - self.active_task = None - self.state = STATE_SCAN_ROOT if phase == PHASE_ROOT else STATE_SCAN_TIP - self.state_reason = "scan_armed" - self._write_checkpoint() - - def _start_next_task(self) -> None: - if not self.task_queue: - if self.state == STATE_SCAN_ROOT: - if self.auto_start_tip: - self._begin_scan(PHASE_TIP) - else: - self.state = STATE_WAIT_TIP - self.state_reason = "root_complete_call_start_for_tip" - self.current_phase = None - self._write_checkpoint() - elif self.state == STATE_SCAN_TIP: - self.current_phase = None - self._begin_validation() - elif self.state == STATE_VALIDATING: - self._finalize() - return - self.active_task = self.task_queue.popleft() - self.active_task.setdefault("stage", "target") - self._restart_active_task() - - def _restart_active_task(self) -> None: - if self.active_task is None: - return - task = self.active_task - if task.get("kind") == "continuous_sweep": - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - if task.get("stage") == "prepare": - command = int(task["start_u8"]) - task["command_u8"] = command - self._publish_hand_command( - build_command(motor_index, command) - ) - task["stage"] = "prepare_move" - task["prepare_started_at"] = time.monotonic() - self.collector.state = "idle" - self.state_reason = ( - f"{task['phase']}:continuous:prepare_move:u={command}" - ) - return - if task.get("stage") != "sweep": - raise RuntimeError( - f"unknown continuous stage {task.get('stage')}" - ) - now = time.monotonic() - self.sweep_collector.start( - now, - motor_index=motor_index, - start_u8=int(task["start_u8"]), - target_u8=int(task["target_u8"]), - ) - if task.get("motion_mode") == "endpoint": - command = int(task["target_u8"]) - task["command_u8"] = command - task.pop("segment_targets_u8", None) - task.pop("segment_index", None) - task.pop("segment_started_at", None) - self._publish_hand_command( - build_command(motor_index, command) - ) - self.state_reason = ( - f"{task['phase']}:continuous_endpoint:" - f"{task['direction']}:" - f"{task['start_u8']}->{task['target_u8']}" - ) - return - direction_targets = [ - command - for _, direction, command in scan_targets( - 1, self.command_step - ) - if direction == task["direction"] - ] - if not direction_targets or int(direction_targets[0]) != int( - task["start_u8"] - ): - raise RuntimeError("continuous segment grid has wrong start") - task["segment_targets_u8"] = [ - int(command) for command in direction_targets[1:] - ] - task["segment_index"] = 0 - command = int(task["segment_targets_u8"][0]) - task["command_u8"] = command - task["segment_started_at"] = now - self._publish_hand_command(build_command(motor_index, command)) - self.state_reason = ( - f"{task['phase']}:continuous_paced:{task['direction']}:" - f"{task['start_u8']}->{task['target_u8']}" - ) - return - - command = int(task["command_u8"]) - if task.get("kind") == "validation" and task.get("stage") == "approach": - command = int(task["approach_u8"]) - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - self._publish_hand_command(build_command(motor_index, command)) - if task.get("kind") == "validation" and task.get("stage") == "approach": - task["approach_started_at"] = time.monotonic() - self.collector.state = "idle" - self.state_reason = ( - f"{task['phase']}:validation:approach_wait:u={command}" - ) - return - point_tolerance = ( - self.validation_position_tolerance_u8 - if task.get("kind") == "validation" - else self.continuous_endpoint_tolerance_u8 - ) - self.collector.start( - time.monotonic(), - required_state_index=motor_index, - required_state_u8=command, - maximum_state_error_u8=point_tolerance, - ) - self.state_reason = ( - f"{task['phase']}:{task.get('kind')}:{task.get('stage')}:u={command}" - ) - - def _publish_hand_command(self, values: list[int]) -> None: - if not self.commands_enabled: - self.get_logger().warning( - f"dry-run: suppressed hardware command {values}" - ) - return - message = JointState() - message.header.stamp = self.get_clock().now().to_msg() - message.name = list(COMMAND_NAMES) - message.position = [float(value) for value in values] - self.command_publisher.publish(message) - - def _complete_active_task(self, aggregate: dict[str, Any]) -> None: - if self.active_task is None: - return - task = self.active_task - if task.get("kind") == "continuous_sweep": - if task.get("stage") != "prepare_capture": - raise RuntimeError( - "continuous point capture is only for prepare_capture" - ) - task["start_aggregate"] = aggregate - task["stage"] = "sweep" - task.pop("prepare_started_at", None) - self.collector.state = "idle" - self._restart_active_task() - return - if task.get("kind") == "sample": - record = { - "kind": "sample", - "scan_mode": task.get("scan_mode", "point"), - "phase": task["phase"], - "cycle": int(task["cycle"]), - "direction": task["direction"], - "command_u8": int(task["command_u8"]), - **aggregate, - } - append_jsonl(self.raw_path, record) - self.records.append(record) - self.active_task = None - self.collector.state = "idle" - self._write_checkpoint() - return - - if task.get("kind") != "validation": - raise RuntimeError(f"unknown task kind {task.get('kind')}") - if task.get("stage") == "approach": - raise RuntimeError("validation approach must not capture images") - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - state_median = aggregate.get("state_u8_median", []) - if len(state_median) != 20: - self._pause("validation_capture_missing_synchronised_state") - return - actual = float(state_median[motor_index]) - target = float(task["command_u8"]) - if ( - not math.isfinite(actual) - or abs(actual - target) - > self.validation_position_tolerance_u8 - ): - self._pause( - "validation_capture_position_mismatch:" - f"target={target:.1f},actual={actual:.1f}" - ) - return - if self.angle_estimation_mode == "trajectory_center_3d": - maximum_capture_spread = max( - ( - float(value) - for value in aggregate.get( - "maximum_translation_spread_m", {} - ).values() - ), - default=float("inf"), - ) - if ( - maximum_capture_spread - > self.maximum_stable_translation_spread_m - ): - self._pause( - "validation_capture_position_spread_too_large:" - f"{1000.0 * maximum_capture_spread:.2f}mm" - ) - return - else: - maximum_capture_spread = max( - ( - float(value) - for value in aggregate.get( - "maximum_spread_rad", {} - ).values() - ), - default=float("inf"), - ) - if maximum_capture_spread > self.maximum_stable_spread_rad: - self._pause( - "validation_capture_pose_spread_too_large:" - f"{math.degrees(maximum_capture_spread):.2f}deg" - ) - return - if task.get("zero_reference"): - self.validation_zero[task["phase"]] = aggregate - else: - self._record_validation(task, aggregate) - self.active_task = None - self.collector.state = "idle" - self._write_checkpoint() - - def _poll_validation_approach(self, now: float) -> None: - task = self.active_task - if task is None: - return - started = float(task.get("approach_started_at", now)) - elapsed = float(now) - started - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - at_target = ( - len(self.latest_state_u8) == 20 - and abs( - float(self.latest_state_u8[motor_index]) - - float(task["approach_u8"]) - ) - <= self.validation_position_tolerance_u8 - ) - if ( - elapsed >= self.validation_approach_minimum_seconds - and at_target - ): - task["stage"] = "target" - task.pop("approach_started_at", None) - self._restart_active_task() - elif elapsed > self.validation_approach_timeout_seconds: - self._pause("validation_approach_timeout") - - def _poll_continuous_prepare(self, now: float) -> None: - task = self.active_task - if task is None: - return - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - command = int(task["start_u8"]) - elapsed = float(now) - float(task.get("prepare_started_at", now)) - at_target = ( - len(self.latest_state_u8) == 20 - and abs(float(self.latest_state_u8[motor_index]) - command) - <= self.continuous_endpoint_tolerance_u8 - ) - if at_target: - task["stage"] = "prepare_capture" - self.collector.start( - float(now), - required_state_index=motor_index, - required_state_u8=command, - maximum_state_error_u8=( - self.continuous_endpoint_tolerance_u8 - ), - ) - self.state_reason = ( - f"{task['phase']}:continuous:prepare_capture:u={command}" - ) - elif elapsed > self.continuous_prepare_timeout_seconds: - actual = ( - float(self.latest_state_u8[motor_index]) - if len(self.latest_state_u8) == 20 - else float("nan") - ) - self._pause( - "continuous_prepare_timeout:" - f"target={command},actual={actual:.1f}" - ) - - def _poll_continuous_segment(self, now: float) -> None: - task = self.active_task - if task is None or not self.sweep_collector.observations: - return - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - command = int(task["command_u8"]) - observation = self.sweep_collector.observations[-1] - actual = float(observation.state_u8[motor_index]) - elapsed = float(now) - float(task.get("segment_started_at", now)) - at_target = ( - abs(actual - command) - <= self.continuous_endpoint_tolerance_u8 - ) - if ( - at_target - and elapsed >= self.continuous_segment_minimum_seconds - and command != int(task["target_u8"]) - ): - next_index = int(task["segment_index"]) + 1 - targets = task["segment_targets_u8"] - if next_index >= len(targets): - self._pause("continuous_segment_grid_exhausted") - return - next_command = int(targets[next_index]) - task["segment_index"] = next_index - task["command_u8"] = next_command - task["segment_started_at"] = float(now) - self._publish_hand_command( - build_command(motor_index, next_command) - ) - elif ( - not at_target - and elapsed > self.continuous_segment_timeout_seconds - ): - self._pause( - "continuous_segment_timeout:" - f"target={command},actual={actual:.1f}" - ) - - def _resolve_pnp_trajectory_branches( - self, - observations: list[Observation], - *, - phase: str, - ) -> tuple[list[Observation], dict[str, Any]]: - if not self.uses_pnp: - return observations, {} - if phase == PHASE_ROOT: - rigid_roles = ("t3", "t4", "t5") - fixed_pairs = (("t3", "t4"), ("t4", "t5")) - # Motor 0 moves T3/T4/T5 as one rigid chain. Do not freeze T3 to - # its online IPPE branch: if T3 is the ambiguous tag, fixing it - # forces the optimiser to flip T4/T5 merely to compensate and - # leaves a large rigid drift. The two rigid pairs provide enough - # information to resolve all three branches jointly. - fixed_online_roles = () - elif phase == PHASE_TIP: - rigid_roles = ("t0", "t3") - fixed_pairs = (("t0", "t3"),) - # T3 is the parent reference for the active MCP trajectory. Do - # not let a non-target root constraint rewrite that active curve. - fixed_online_roles = ("t3",) - else: - raise ValueError(f"unknown calibration phase {phase}") - - frames = [] - for observation in observations: - frame = dict(observation.tag_pose_candidates) - for role in fixed_online_roles: - online_quaternion = observation.tag_quaternion_xyzw.get( - role - ) - candidates = tuple(frame.get(role, ())) - if online_quaternion is None or not candidates: - raise ValueError( - f"trajectory frame is missing fixed role {role}" - ) - frame[role] = ( - min( - candidates, - key=lambda candidate: rotation_distance_rad( - online_quaternion, - candidate.quaternion_xyzw, - ), - ), - ) - frames.append(frame) - if any( - any(not frame.get(role) for role in rigid_roles) - for frame in frames - ): - raise ValueError("trajectory frame is missing PnP candidates") - selected_path, quality = select_rigid_group_trajectory( - frames, - roles=rigid_roles, - fixed_pairs=fixed_pairs, - reprojection_scale_px=( - self.pnp_trajectory_reprojection_scale_px - ), - rotation_scale_rad=self.pnp_rigid_rotation_scale_rad, - translation_scale_m=self.pnp_rigid_translation_scale_m, - pair_geometry=( - "distance" - if self.angle_estimation_mode == "trajectory_center_3d" - else "pose" - ), - ) - - corrected: list[Observation] = [] - corrected_role_poses = 0 - corrected_frames = 0 - for observation, selected in zip(observations, selected_path): - transforms = dict(observation.tag_quaternion_xyzw) - translations = dict(observation.tag_translation_xyz_m) - tag_quality = dict(observation.tag_quality) - frame_corrected = False - for role, pose in selected.items(): - original = transforms.get(role) - if ( - original is not None - and rotation_distance_rad( - original, - pose.quaternion_xyzw, - ) - > 1.0e-6 - ): - corrected_role_poses += 1 - frame_corrected = True - transforms[role] = pose.quaternion_xyzw - translations[role] = pose.translation_xyz_m - if role in tag_quality: - tag_quality[role] = replace( - tag_quality[role], - reprojection_error_px=( - pose.reprojection_error_px - ), - ) - if frame_corrected: - corrected_frames += 1 - relative = { - PAIR_ROOT: relative_quaternion_xyzw( - transforms["t0"], transforms["t3"] - ), - PAIR_MCP: relative_quaternion_xyzw( - transforms["t3"], transforms["t4"] - ), - PAIR_IP: relative_quaternion_xyzw( - transforms["t4"], transforms["t5"] - ), - } - corrected.append( - replace( - observation, - relative_quaternion_xyzw=relative, - tag_quality=tag_quality, - tag_quaternion_xyzw=transforms, - tag_translation_xyz_m=translations, - ) - ) - report: dict[str, Any] = { - **quality, - "phase": phase, - "frames": len(observations), - "corrected_frames": corrected_frames, - "corrected_role_poses": corrected_role_poses, - "fixed_online_roles": list(fixed_online_roles), - } - return corrected, report - - def _complete_continuous_sweep( - self, observations: list[Observation] - ) -> None: - task = self.active_task - if ( - task is None - or task.get("kind") != "continuous_sweep" - or task.get("stage") != "sweep" - ): - return - try: - observations, trajectory_report = ( - self._resolve_pnp_trajectory_branches( - observations, - phase=str(task["phase"]), - ) - ) - except Exception as error: - self.sweep_collector.state = "idle" - self._pause(f"pnp_trajectory_selection_failed:{error}") - return - if trajectory_report: - trajectory_report["direction"] = str(task["direction"]) - self.latest_trajectory_branch_quality = trajectory_report - maximum_rigid_drift = float( - trajectory_report.get( - "maximum_pair_rotation_drift_rad", - float("inf"), - ) - ) - p95_rigid_drift = float( - trajectory_report.get( - "p95_pair_rotation_drift_rad", - float("inf"), - ) - ) - if self.angle_estimation_mode == "trajectory_center_3d": - maximum_translation_drift = float( - trajectory_report.get( - "maximum_pair_distance_drift_m", - float("inf"), - ) - ) - p95_translation_drift = float( - trajectory_report.get( - "p95_pair_distance_drift_m", - float("inf"), - ) - ) - if ( - p95_translation_drift - > self.pnp_rigid_p95_accepted_distance_drift_m - or maximum_translation_drift - > self.pnp_rigid_maximum_accepted_distance_drift_m - ): - self.sweep_collector.state = "idle" - self._pause( - "pnp_trajectory_rigid_distance_drift_too_large:" - f"p95={1000.0 * p95_translation_drift:.2f}mm," - "max=" - f"{1000.0 * maximum_translation_drift:.2f}mm" - ) - return - elif ( - p95_rigid_drift - > self.pnp_rigid_p95_accepted_drift_rad - or maximum_rigid_drift - > self.pnp_rigid_maximum_accepted_drift_rad - ): - self.sweep_collector.state = "idle" - self._pause( - "pnp_trajectory_rigid_drift_too_large:" - f"p95={math.degrees(p95_rigid_drift):.2f}deg," - "max=" - f"{math.degrees(maximum_rigid_drift):.2f}deg" - ) - return - self.trajectory_branch_reports.append(trajectory_report) - motor_index = 0 if task["phase"] == PHASE_ROOT else 15 - aggregates = aggregate_sweep_observations( - observations, - motor_index=motor_index, - start_u8=int(task["start_u8"]), - target_u8=int(task["target_u8"]), - endpoint_tolerance_u8=self.continuous_endpoint_tolerance_u8, - ) - start_command = int(task["start_u8"]) - start_aggregate = task.get("start_aggregate") - if ( - isinstance(start_aggregate, dict) - and start_command not in aggregates - ): - aggregates[start_command] = start_aggregate - - commands = sorted(aggregates) - maximum_gap = max(np.diff(commands), default=0) - if ( - len(commands) < self.continuous_minimum_bins - or not commands - or commands[0] != 0 - or commands[-1] != 255 - or maximum_gap > self.continuous_maximum_bin_gap - ): - self.sweep_collector.state = "idle" - self._pause( - "continuous_sweep_insufficient_coverage:" - f"bins={len(commands)},max_gap={maximum_gap}" - ) - return - - for command in commands: - record = { - "kind": "sample", - "scan_mode": "continuous", - "phase": task["phase"], - "cycle": int(task["cycle"]), - "direction": task["direction"], - "command_u8": int(command), - "sweep_start_u8": int(task["start_u8"]), - "sweep_target_u8": int(task["target_u8"]), - "pnp_trajectory_quality": trajectory_report, - **aggregates[command], - } - append_jsonl(self.raw_path, record) - self.records.append(record) - self.active_task = None - self.sweep_collector.state = "idle" - self._write_checkpoint() - if trajectory_report: - self._write_manifest( - { - "pnp_trajectory_branch_reports": ( - self.trajectory_branch_reports - ) - } - ) - - def _fit_calibration_records(self) -> FitResult: - if self.angle_estimation_mode != "trajectory_center_3d": - return fit_calibration_curves(self.records) - return fit_center_trajectory_curves( - self.records, - passive_ip_multiplier=self.passive_ip_multiplier, - maximum_plane_rms_m=self.trajectory_maximum_plane_rms_m, - maximum_radial_rms_m=self.trajectory_maximum_radial_rms_m, - minimum_radius_m=self.trajectory_minimum_radius_m, - minimum_arc_rad=self.trajectory_minimum_arc_rad, - maximum_root_role_disagreement_rad=( - self.trajectory_maximum_root_role_disagreement_rad - ), - maximum_anchor_drift_m=( - self.trajectory_maximum_anchor_drift_m - ), - ) - - def _begin_validation(self) -> None: - try: - self.fit = self._fit_calibration_records() - except Exception as error: - self._pause(f"curve_fit_failed:{error}") - return - if self.fit.trajectory_quality: - self._write_manifest( - { - "trajectory_center_fit": { - "zero_command_u8": 255, - "models": self.fit.trajectory_models, - "quality": self.fit.trajectory_quality, - } - } - ) - generator = random.Random(self.validation_seed) - commands = sorted( - generator.sample(range(1, 255), self.validation_command_count) - ) - tasks: list[dict[str, Any]] = [] - for phase in (PHASE_ROOT, PHASE_TIP): - tasks.append( - { - "kind": "validation", - "phase": phase, - "direction": DIRECTION_DECREASING, - "command_u8": 255, - "zero_reference": True, - "stage": "target", - } - ) - for index, command in enumerate(commands): - direction = ( - DIRECTION_DECREASING - if index % 2 == 0 - else DIRECTION_INCREASING - ) - approach = ( - min(255, command + self.command_step) - if direction == DIRECTION_DECREASING - else max(0, command - self.command_step) - ) - tasks.append( - { - "kind": "validation", - "phase": phase, - "direction": direction, - "command_u8": command, - "approach_u8": approach, - "zero_reference": False, - "stage": "approach", - } - ) - tasks.append( - { - "kind": "validation", - "phase": phase, - "direction": DIRECTION_INCREASING, - "command_u8": 255, - "zero_reference": True, - "stage": "target", - } - ) - self.task_queue = deque(tasks) - self.active_task = None - self.validation_records = [] - self.validation_errors = [] - self.validation_zero = {} - self.state = STATE_VALIDATING - self.state_reason = "random_validation_started" - self._write_checkpoint() - - def _record_validation( - self, task: dict[str, Any], aggregate: dict[str, Any] - ) -> None: - if self.fit is None: - raise RuntimeError("validation requires fitted curves") - phase = str(task["phase"]) - direction = str(task["direction"]) - command = int(task["command_u8"]) - zero = self.validation_zero.get(phase) - if zero is None: - raise RuntimeError(f"missing validation zero for {phase}") - centre_measurements: dict[str, float] = {} - centre_zero: dict[str, float] = {} - if self.fit.measurement_mode == "trajectory_center_3d": - centre_measurements = measure_center_trajectory_phase_angles( - self.fit.trajectory_models, - aggregate["tag_translation_xyz_m"], - phase=phase, - ) - centre_zero = measure_center_trajectory_phase_angles( - self.fit.trajectory_models, - zero["tag_translation_xyz_m"], - phase=phase, - ) - measurements: dict[str, float] = {} - predictions: dict[str, float] = {} - errors: dict[str, float] = {} - joint_names = ( - ("thumb_cmc_pitch",) - if phase == PHASE_ROOT - else ("thumb_mcp", "thumb_ip") - ) - for joint_name in joint_names: - pair = { - "thumb_cmc_pitch": PAIR_ROOT, - "thumb_mcp": PAIR_MCP, - "thumb_ip": PAIR_IP, - }[joint_name] - if self.fit.measurement_mode == "trajectory_center_3d": - measurement = ( - centre_measurements[joint_name] - - centre_zero[joint_name] - ) - else: - measurement = self.fit.measure_from_reference( - joint_name, - aggregate["relative_quaternion_xyzw"][pair], - zero["relative_quaternion_xyzw"][pair], - ) - prediction = float( - self.fit.joints[joint_name]["angle_rad"][command] - ) - error = measurement - prediction - measurements[joint_name] = measurement - predictions[joint_name] = prediction - errors[joint_name] = error - self.validation_errors.append(error) - record = { - "kind": "validation", - "phase": phase, - "direction": direction, - "command_u8": command, - "measurements_rad": measurements, - "predictions_rad": predictions, - "errors_rad": errors, - **aggregate, - } - self.validation_records.append(record) - append_jsonl(self.raw_path, record) - - def _finalize(self) -> None: - if self.fit is None: - self.fit = self._fit_calibration_records() - errors = np.abs(np.asarray(self.validation_errors, dtype=float)) - mae = float(np.mean(errors)) if errors.size else float("inf") - p95 = float(np.percentile(errors, 95)) if errors.size else float("inf") - coupling_drift = ( - maximum_center_non_target_drift_rad( - self.records, - self.fit.trajectory_models, - ) - if self.fit.measurement_mode == "trajectory_center_3d" - else maximum_non_target_drift_rad(self.records, self.fit) - ) - passed = ( - errors.size > 0 - and mae <= self.maximum_validation_mae_rad - and p95 <= self.maximum_validation_p95_rad - and coupling_drift <= self.maximum_coupling_drift_rad - and self.fit.ip_coupling["r_squared"] - >= self.minimum_ip_coupling_r_squared - and self.fit.max_monotonic_correction_rad - <= self.maximum_monotonic_correction_rad - and self.fit.max_hysteresis_rad <= self.maximum_hysteresis_rad - ) - final_payload = create_final_payload( - serial_number=self.serial_number, - fit=self.fit, - validation_errors_rad=self.validation_errors, - passed=passed, - ) - validation_payload = { - "records": self.validation_records, - "summary": { - "passed": passed, - "mae_rad": mae, - "p95_rad": p95, - "maximum_non_target_drift_rad": coupling_drift, - "coupling_anomaly": ( - coupling_drift > self.maximum_coupling_drift_rad - ), - "ip_coupling_r_squared": self.fit.ip_coupling["r_squared"], - "maximum_monotonic_correction_rad": ( - self.fit.max_monotonic_correction_rad - ), - "maximum_hysteresis_rad": self.fit.max_hysteresis_rad, - }, - } - atomic_write_json(self.validation_path, validation_payload) - atomic_write_json(self.final_path, final_payload) - self.state = STATE_COMPLETE - self.state_reason = ( - "calibration_passed" if passed else "calibration_completed_quality_failed" - ) - self._write_checkpoint() - self._write_manifest( - { - "completed_at_utc": datetime.now(timezone.utc).isoformat(), - "quality": validation_payload["summary"], - "final_result": str(self.final_path), - } - ) - self.get_logger().info( - f"Calibration complete: passed={passed}; result={self.final_path}" - ) - - def _write_checkpoint(self) -> None: - payload = { - "state": self.state, - "reason": self.state_reason, - "updated_at_utc": datetime.now(timezone.utc).isoformat(), - "scan_records": len( - [ - record - for record in self.records - if record.get("kind", "sample") == "sample" - ] - ), - "active_task": self.active_task, - "remaining_tasks": len(self.task_queue), - } - atomic_write_json(self.checkpoint_path, payload) - - def _validate_existing_session(self) -> None: - if not self.manifest_path.exists(): - return - try: - existing = json.loads( - self.manifest_path.read_text(encoding="utf-8") - ) - except (OSError, json.JSONDecodeError) as error: - raise ValueError("existing session manifest is invalid") from error - if not isinstance(existing, dict): - raise ValueError("existing session manifest must be an object") - expected = { - "serial_number": self.serial_number, - "tag_config": self.tag_config, - "code_sha256": self._code_sha256(), - } - for key, value in expected.items(): - if existing.get(key) != value: - raise ValueError( - f"existing session {key} does not match; start a new session" - ) - sdk = existing.get("sdk", {}) - if sdk.get("baseline_command_u8") != list(BASELINE_COMMAND): - raise ValueError( - "existing session baseline does not match; start a new session" - ) - if int(sdk.get("calibration_speed_u8", -1)) != self.calibration_speed: - raise ValueError( - "existing session calibration speed does not match; " - "start a new session" - ) - existing_mode = existing.get("camera", {}).get( - "angle_estimation_mode", "pnp_3d" - ) - if existing_mode != self.angle_estimation_mode: - raise ValueError( - "existing session angle estimation mode does not match; " - "start a new session" - ) - capture = existing.get("capture", {}) - if capture.get("scan_mode", "point") != self.scan_mode: - raise ValueError( - "existing session scan_mode does not match; start a new session" - ) - if ( - capture.get("continuous_motion_mode", "paced") - != self.continuous_motion_mode - ): - raise ValueError( - "existing session continuous_motion_mode does not match; " - "start a new session" - ) - if int(capture.get("repetitions", -1)) != self.repetitions: - raise ValueError( - "existing session repetitions do not match; start a new session" - ) - if int(capture.get("command_step", -1)) != self.command_step: - raise ValueError( - "existing session command_step does not match; start a new session" - ) - - def _write_manifest(self, updates: dict[str, Any] | None = None) -> None: - existing: dict[str, Any] = {} - if self.manifest_path.exists(): - try: - value = json.loads(self.manifest_path.read_text(encoding="utf-8")) - if isinstance(value, dict): - existing = value - except (OSError, json.JSONDecodeError): - pass - payload = { - "schema_version": 1, - "session_kind": "g20_thumb_front_apriltag", - "created_at_utc": existing.get( - "created_at_utc", datetime.now(timezone.utc).isoformat() - ), - "serial_number": self.serial_number, - "code_sha256": self._code_sha256(), - "tag_config": self.tag_config, - "camera": { - "serial_number": self.camera_serial_number, - "camera_info_topic": self.camera_info_topic, - "image_topic": self.image_topic, - "depth_used_for_angle": False, - "angle_estimation_mode": self.angle_estimation_mode, - "pnp": { - "solver": "opencv_solvePnPGeneric_IPPE_SQUARE", - "input_is_rectified": True, - "uses_detection_corners_directly": True, - "maximum_reprojection_error_px": ( - self.pnp_maximum_reprojection_error_px - ), - "reprojection_tie_px": self.pnp_reprojection_tie_px, - "maximum_pose_jump_rad": ( - self.pnp_maximum_pose_jump_rad - ), - "maximum_translation_jump_m": ( - self.pnp_maximum_translation_jump_m - ), - "maximum_tag_tilt_rad": ( - self.pnp_maximum_tag_tilt_rad - ), - "tracker_reset_seconds": ( - self.pnp_tracker_reset_seconds - ), - "group_relative_rotation_scale_rad": ( - self.pnp_group_relative_rotation_scale_rad - ), - "group_relative_translation_scale_m": ( - self.pnp_group_relative_translation_scale_m - ), - "group_reprojection_weight": ( - self.pnp_group_reprojection_weight - ), - "trajectory_reprojection_scale_px": ( - self.pnp_trajectory_reprojection_scale_px - ), - "rigid_rotation_scale_rad": ( - self.pnp_rigid_rotation_scale_rad - ), - "rigid_translation_scale_m": ( - self.pnp_rigid_translation_scale_m - ), - "rigid_p95_accepted_drift_rad": ( - self.pnp_rigid_p95_accepted_drift_rad - ), - "rigid_maximum_accepted_drift_rad": ( - self.pnp_rigid_maximum_accepted_drift_rad - ), - "rigid_p95_accepted_distance_drift_m": ( - self.pnp_rigid_p95_accepted_distance_drift_m - ), - "rigid_maximum_accepted_distance_drift_m": ( - self.pnp_rigid_maximum_accepted_distance_drift_m - ), - }, - "trajectory_center_3d": { - "uses_tag_centres": True, - "palm_tag_is_translation_anchor": True, - "zero_command_u8": 255, - "passive_ip_estimation": "mcp_mimic_constraint", - "passive_ip_multiplier": self.passive_ip_multiplier, - "maximum_plane_rms_m": ( - self.trajectory_maximum_plane_rms_m - ), - "maximum_radial_rms_m": ( - self.trajectory_maximum_radial_rms_m - ), - "minimum_radius_m": self.trajectory_minimum_radius_m, - "minimum_arc_rad": self.trajectory_minimum_arc_rad, - "maximum_root_role_disagreement_rad": ( - self.trajectory_maximum_root_role_disagreement_rad - ), - "maximum_anchor_drift_m": ( - self.trajectory_maximum_anchor_drift_m - ), - "static_translation_outlier_m": ( - self.trajectory_static_translation_outlier_m - ), - "maximum_static_translation_rms_m": ( - self.trajectory_maximum_static_translation_rms_m - ), - }, - "debug_image_enabled": self.publish_debug_image, - "intrinsics": self.camera_intrinsics, - }, - "rosbag_path": self.rosbag_path, - "sdk": { - "command_topic": self.command_topic, - "state_topic": self.state_topic, - "baseline_command_u8": list(BASELINE_COMMAND), - "calibration_speed_u8": self.calibration_speed, - "commands_enabled": self.commands_enabled, - "latest_info": self.latest_hand_info, - }, - "capture": { - "scan_mode": self.scan_mode, - "continuous_motion_mode": self.continuous_motion_mode, - "repetitions": self.repetitions, - "command_step": self.command_step, - "auto_start_tip": self.auto_start_tip, - "stable_frames": self.stable_frames, - "capture_frames": self.capture_frames, - "minimum_settle_seconds": self.minimum_settle_seconds, - "maximum_stable_spread_rad": self.maximum_stable_spread_rad, - "maximum_stable_translation_spread_m": ( - self.maximum_stable_translation_spread_m - ), - "pose_outlier_threshold_rad": self.pose_outlier_threshold_rad, - "minimum_pose_inlier_rate": self.minimum_pose_inlier_rate, - "pnp_minimum_valid_rate": self.pnp_minimum_valid_rate, - "maximum_state_image_skew_ms": ( - self.maximum_state_image_skew_ns / 1_000_000.0 - ), - "continuous_endpoint_tolerance_u8": ( - self.continuous_endpoint_tolerance_u8 - ), - "continuous_endpoint_hold_seconds": ( - self.continuous_endpoint_hold_seconds - ), - "continuous_minimum_valid_frames": ( - self.continuous_minimum_valid_frames - ), - "continuous_minimum_bins": self.continuous_minimum_bins, - "continuous_maximum_bin_gap": ( - self.continuous_maximum_bin_gap - ), - "continuous_segment_minimum_seconds": ( - self.continuous_segment_minimum_seconds - ), - "continuous_segment_timeout_seconds": ( - self.continuous_segment_timeout_seconds - ), - "continuous_prepare_timeout_seconds": ( - self.continuous_prepare_timeout_seconds - ), - }, - } - existing.update(payload) - payload = existing - if updates: - payload.update(updates) - atomic_write_json(self.manifest_path, payload) - - @staticmethod - def _code_sha256() -> str: - digest = hashlib.sha256() - source_dir = Path(__file__).resolve().parent - for name in ( - "acquisition.py", - "core.py", - "diagnostics.py", - "node.py", - "pnp.py", - "storage.py", - "trajectory.py", - ): - path = source_dir / name - digest.update(name.encode("utf-8")) - digest.update(path.read_bytes()) - return digest.hexdigest() - - def _publish_status(self) -> None: - active = self.active_task or {} - completed_scan = len( - [ - record - for record in self.records - if record.get("kind", "sample") == "sample" - ] - ) - if self.scan_mode == "continuous": - completed_directions = sum( - self._continuous_direction_complete(phase, direction) - for phase in (PHASE_ROOT, PHASE_TIP) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - scan_progress = completed_directions / 4.0 - else: - total_scan = len( - scan_targets(self.repetitions, self.command_step) - ) * 2 - scan_progress = completed_scan / total_scan - tag_quality = build_tag_quality_diagnostics( - self.tag_config, - self.latest_tag_qualities, - self.latest_pnp_rejections, - self.latest_pnp_errors_px, - maximum_hamming=self.maximum_hamming, - minimum_decision_margin=self.minimum_decision_margin, - minimum_edge_pixels=self.minimum_edge_pixels, - maximum_reprojection_error_px=( - self.pnp_maximum_reprojection_error_px - ), - ) - reason_zh, action_zh = status_guidance_zh( - self.state, - self.state_reason, - tag_quality, - ) - payload = { - "state": self.state, - "state_zh": STATE_NAMES_ZH.get(self.state, self.state), - "reason": self.state_reason, - "reason_zh": reason_zh, - "action_zh": action_zh, - "commands_enabled": self.commands_enabled, - "calibration_speed_u8": self.calibration_speed, - "angle_estimation_mode": self.angle_estimation_mode, - "passive_ip_estimation": ( - "mcp_mimic_constraint" - if self.angle_estimation_mode == "trajectory_center_3d" - else None - ), - "passive_ip_multiplier": ( - self.passive_ip_multiplier - if self.angle_estimation_mode == "trajectory_center_3d" - else None - ), - "pnp_valid_rate": ( - float(np.mean(self.preflight_pnp_flags)) - if self.preflight_pnp_flags - else None - ), - "pnp_rejections": self.latest_pnp_rejections, - "pnp_rejections_zh": { - role: pnp_rejection_zh(reason) - for role, reason in self.latest_pnp_rejections.items() - }, - "pnp_reprojection_error_px": self.latest_pnp_errors_px, - "tag_quality": tag_quality, - "pnp_branch_corrections": ( - self.pnp_group_tracker.branch_correction_counts - ), - "pnp_trajectory_quality": ( - self.latest_trajectory_branch_quality - ), - "trajectory_center_quality": ( - {} if self.fit is None else self.fit.trajectory_quality - ), - "scan_mode": self.scan_mode, - "continuous_motion_mode": self.continuous_motion_mode, - "command_step": self.command_step, - "auto_start_tip": self.auto_start_tip, - "active_phase": active.get("phase"), - "active_command_u8": active.get("command_u8"), - "active_direction": active.get("direction"), - "active_cycle": active.get("cycle"), - "collector_state": self.collector.state, - "collector_reason": self.collector.reason, - "stable_frames_seen": self.collector.stable_frames_seen, - "stable_spread_deg": { - pair: math.degrees(spread) - for pair, spread in self.collector.stable_spread_rad.items() - }, - "stable_spread_mm": { - pair: 1000.0 * spread - for pair, spread in self.collector.stable_spread_m.items() - }, - "capture_frames_seen": self.collector.capture_frames_seen, - "sweep_collector_state": self.sweep_collector.state, - "sweep_collector_reason": self.sweep_collector.reason, - "sweep_valid_frames_seen": ( - self.sweep_collector.valid_frames_seen - ), - "sweep_state_span_u8": self.sweep_collector.state_span_u8, - "sweep_segment_index": active.get("segment_index"), - "sweep_segment_count": len( - active.get("segment_targets_u8", []) - ), - "scan_progress": scan_progress, - "scan_records": completed_scan, - "session_dir": str(self.session_dir), - } - message = String() - message.data = json.dumps(payload, ensure_ascii=False) - self.status_publisher.publish(message) - text_message = String() - text_message.data = render_status_text_zh( - payload["state_zh"], - reason_zh, - action_zh, - tag_quality, - ) - self.status_text_publisher.publish(text_message) - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - node: G20ThumbCalibrationNode | None = None - try: - node = G20ThumbCalibrationNode() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - rclpy.shutdown() diff --git a/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py b/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py index 36d6da7..f2938ba 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py @@ -2,6 +2,6 @@ import sys -from .models.g20 import offline_replay as _implementation +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20 import offline_replay as _implementation sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/product.py b/src/linkerhand_calibration/linkerhand_calibration/product.py index 3865cb9..836a750 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/product.py +++ b/src/linkerhand_calibration/linkerhand_calibration/product.py @@ -10,6 +10,7 @@ from __future__ import annotations from dataclasses import dataclass import hashlib +import math from pathlib import Path import re from typing import Any, Mapping @@ -24,38 +25,50 @@ from .compat import ( ) from .core import CalibrationProfile, ProfileKey from .core.geometry import camera_info_fingerprint, load_camera_extrinsics -from .models import RegisteredProfile, get_default_registry +from linkerhand_calibration.compat.legacy_diagnostic_tools.models import RegisteredProfile, get_default_registry +from .profiles import load_hand_profile @dataclass(frozen=True) class ProductCalibrationContract: """Compatibility facade over one typed, locally registered profile.""" - registered: RegisteredProfile + registered: RegisteredProfile | None = None + declarative: CalibrationProfile | None = None @property def model(self) -> str: - return self.registered.profile.key.model + return self.typed_profile.key.model @property def side(self) -> str: - return self.registered.profile.key.side + return self.typed_profile.key.side @property def layout_id(self) -> str: - return self.registered.profile.key.layout + return self.typed_profile.key.layout @property def typed_profile(self) -> CalibrationProfile: + if self.declarative is not None: + return self.declarative + if self.registered is None: + raise ValueError("product has no calibration profile") return self.registered.profile + def _legacy_binding(self): + # Transitional offline/API compatibility. Product validation itself + # never resolves a Python model or constructs its online state machine. + registered = self.registered or get_default_registry().get(self.typed_profile.key) + return registered.engine + @property def profile(self): - return self.registered.engine.hand_profile + return self._legacy_binding().hand_profile @property def zero_profile(self): - return self.registered.engine.zero_profile + return self._legacy_binding().zero_profile @property def required_tag_ids(self) -> frozenset[int]: @@ -218,6 +231,10 @@ class ProductConfig: sdk_setup: Path | None = None sdk_config: Path | None = None sdk_config_sha256: str = "" + sdk_python_package: Path | None = None + sdk_package_sha256: str = "" + profile_config: Path | None = None + profile_config_sha256: str = "" @property def session_root(self) -> Path: @@ -230,7 +247,7 @@ def load_product_config( workspace: str | Path | None = None, check_can: bool = True, ) -> ProductConfig: - """Load and verify a product against its registered calibration contract.""" + """Load a protected YAML contract without requiring a Python model plugin.""" source = Path(path).expanduser().resolve() if not source.is_file(): raise ValueError(f"product config does not exist: {source}") @@ -244,9 +261,31 @@ def load_product_config( model = profile_key.model side = profile_key.side layout = profile_key.layout - contract = get_product_calibration_contract( - model, side, layout, profile_key.revision - ) + profile_config: Path | None = None + profile_config_hash = "" + if raw.get("profile_config"): + profile_config = _resolve_path( + raw.get("profile_config"), workspace=root, name="profile_config" + ) + if not profile_config.is_file(): + raise ValueError(f"profile config does not exist: {profile_config}") + profile_config_hash = str(raw.get("profile_config_sha256", "")).lower() + if re.fullmatch(r"[0-9a-f]{64}", profile_config_hash) is None: + raise ValueError("profile config expected SHA-256 is invalid") + if sha256_file(profile_config) != profile_config_hash: + raise ValueError("profile config SHA-256 mismatch") + declarative = load_hand_profile(profile_config) + if declarative.key != profile_key: + raise ValueError("declarative profile identity differs from the product") + # Check again after decoding so an edited file cannot be paired with + # the hash of a previous version during a concurrent configuration edit. + if sha256_file(profile_config) != profile_config_hash: + raise ValueError("profile config changed while loading") + contract = ProductCalibrationContract(declarative=declarative) + else: + # Archived configurations without a profile path remain readable by + # offline diagnostics until their legacy format is retired. + contract = get_product_calibration_contract(model, side, layout, profile_key.revision) namespace = str( raw.get("namespace", contract.typed_profile.namespace) ).strip() @@ -267,6 +306,12 @@ def load_product_config( sdk_setup: Path | None = None sdk_config: Path | None = None sdk_config_hash = "" + sdk_python_package, sdk_package_hash = None, "" + if sdk_raw.get("python_package"): + sdk_python_package = _resolve_path(sdk_raw["python_package"], workspace=root, name="sdk.python_package") + sdk_package_hash = str(sdk_raw.get("package_sha256", "")).lower() + if re.fullmatch(r"[0-9a-f]{64}", sdk_package_hash) is None or not sdk_python_package.is_file() or sha256_file(sdk_python_package) != sdk_package_hash: + raise ValueError("vendor Python SDK package SHA-256 mismatch") if sdk_transport == "hcan": sdk_setup = _resolve_path( sdk_raw.get("setup"), workspace=root, name="sdk.setup" @@ -333,9 +378,15 @@ def load_product_config( if actual != expected: raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}") _validate_profile_urdf(contract.typed_profile, source_urdf) + if profile_config is not None: + from .profiles.validator import validate_executable_profile + validate_executable_profile(contract.typed_profile, source_urdf) expected_tag_ids = set(contract.required_tag_ids) tag_sizes = _tag_sizes_m_by_id(tag_config) + if profile_config is not None and any(not math.isclose(tag.size_m, tag_sizes.get(tag.tag_id, -1), abs_tol=1e-12) + for view in contract.typed_profile.vision.views for tag in view.tags): + raise ValueError("Profile Tag side lengths differ from the protected detector configuration") if set(tag_sizes) != expected_tag_ids: raise ValueError( f"{model}/{side}/{layout} Tag IDs differ from the registered " @@ -427,4 +478,8 @@ def load_product_config( sdk_setup=sdk_setup, sdk_config=sdk_config, sdk_config_sha256=sdk_config_hash, + sdk_python_package=sdk_python_package, + sdk_package_sha256=sdk_package_hash, + profile_config=profile_config, + profile_config_sha256=profile_config_hash, ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/__init__.py new file mode 100644 index 0000000..0f4e3c6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/__init__.py @@ -0,0 +1,12 @@ +"""Declarative hand-profile loading and registration.""" + +from .export import dump_hand_profile, hand_profile_payload, write_hand_profile +from .loader import load_hand_profile, load_bundled_hand_profile + +__all__ = [ + "dump_hand_profile", + "hand_profile_payload", + "load_hand_profile", + "load_bundled_hand_profile", + "write_hand_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py new file mode 100644 index 0000000..cbdabe9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/export.py @@ -0,0 +1,74 @@ +"""Canonical serialization for reviewed hand profiles. + +The runtime loader is deliberately independent of model Python modules. This +writer exists to migrate the four established profiles without maintaining a +second, hand-written representation of the same contract. +""" + +from __future__ import annotations + +from dataclasses import asdict +from pathlib import Path +from typing import Any, Mapping + +import yaml + +from ..core import CalibrationProfile + + +def _plain(value: Any) -> Any: + if isinstance(value, Mapping): + return {str(key): _plain(item) for key, item in value.items()} + if isinstance(value, (set, frozenset)): + return sorted(_plain(item) for item in value) + if isinstance(value, tuple): + return [_plain(item) for item in value] + if isinstance(value, list): + return [_plain(item) for item in value] + return value + + +def hand_profile_payload(profile: CalibrationProfile) -> dict[str, Any]: + """Return the complete YAML contract accepted by ``load_hand_profile``.""" + command = _plain(asdict(profile.command)) + vision = _plain(asdict(profile.vision)) + for view in vision["views"]: + for tag in view["tags"]: + tag["id"] = tag.pop("tag_id") + payload = { + "schema_version": 1, + "profile_id": profile.key.profile_id, + "namespace": profile.namespace, + "sdk_adapter": profile.sdk_adapter, + "command": command, + "vision": vision, + "motion": _plain(asdict(profile.motion)), + "measurement": _plain(asdict(profile.measurement)), + "zero": _plain(asdict(profile.zero)), + "quality": _plain(asdict(profile.quality)), + "scope": _plain(asdict(profile.scope)), + "artifacts": _plain(asdict(profile.artifacts)), + "acquisition": _plain(asdict(profile.acquisition)), + "urdf": { + "authorized_fields": _plain(profile.urdf_authorized_fields), + }, + "joint_coverage": _plain(profile.joint_coverage), + } + return payload + + +def dump_hand_profile(profile: CalibrationProfile) -> str: + return yaml.safe_dump( + hand_profile_payload(profile), + allow_unicode=True, + sort_keys=False, + default_flow_style=False, + ) + + +def write_hand_profile(profile: CalibrationProfile, path: str | Path) -> None: + """Explicit migration utility; online calibration never rewrites profiles.""" + Path(path).write_text(dump_hand_profile(profile), encoding="utf-8") + + +__all__ = ["dump_hand_profile", "hand_profile_payload", "write_hand_profile"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py new file mode 100644 index 0000000..80d13e5 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/loader.py @@ -0,0 +1,329 @@ +"""Strict YAML loader for a model that reuses the unified engine.""" + +from __future__ import annotations + +from dataclasses import fields +from pathlib import Path +import re +from typing import Any, Mapping + +import yaml +from ..core.domain.profile import ReferenceWaypoint + +from ..core import ( + AcquisitionPolicy, + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + QualityPolicy, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, + validate_profile, +) + + +def _map(value: Any, name: str) -> Mapping[str, Any]: + if not isinstance(value, Mapping): + raise ValueError(f"{name} must be a mapping") + return value + + +def _set(value: Any) -> frozenset[str]: + return frozenset(str(item) for item in (value or ())) + + +def _keys(value, allowed, name): + unknown = set(_map(value, name)) - set(allowed) + if unknown: + raise ValueError(f"unknown {name} fields: {sorted(unknown)}") + + +def _validate_keys(payload): + # Misspelled safety/geometry fields must not silently select a default. + _keys(payload, {"schema_version", "profile_id", "namespace", "sdk_adapter", "command", + "vision", "motion", "measurement", "zero", "quality", "scope", "artifacts", + "acquisition", "urdf", "joint_coverage"}, "profile") + for name, contract in (("command", CommandLayout), ("vision", VisionRigSpec), + ("motion", MotionPolicy), ("measurement", MeasurementPolicy), ("zero", ZeroSolvePolicy), + ("quality", QualityPolicy), ("scope", ScopePolicy), ("artifacts", ArtifactPolicy), + ("acquisition", AcquisitionPolicy)): + _keys(payload.get(name, {}), {f.name for f in fields(contract)}, name) + _keys(payload.get("urdf", {}), {"authorized_fields"}, "urdf") + for view in payload.get("vision", {}).get("views", ()): + _keys(view, {"name", "tags"}, "view") + for tag in view.get("tags", ()): + _keys(tag, {"id", "role", "fixed_reference", "link", "size_m"}, "Tag") + for task in payload.get("motion", {}).get("tasks", ()): + _keys(task, {f.name for f in fields(TaskSpec)}, "task") + for waypoint in payload.get("motion", {}).get("resume_verification_waypoints", ()): + _keys(waypoint, {f.name for f in fields(ReferenceWaypoint)}, "reference waypoint") + for name, measurement in payload.get("measurement", {}).get("measurements", {}).items(): + _keys(measurement, {f.name for f in fields(MeasurementSpec)}, "measurement item") + if measurement.get("joint", name) != name: + raise ValueError("measurement key and joint field differ") + + +def load_bundled_hand_profile(layout: str) -> CalibrationProfile: + """Resolve installed or source-tree YAML without constructing a model. + + No cache: a product hash check and the subsequently loaded contract must + see the same on-disk inputs, not a previous in-process Python singleton. + """ + if re.fullmatch(r"[a-z0-9_]+", str(layout)) is None: + raise ValueError("invalid bundled profile layout name") + source = Path(__file__).resolve().parents[2] / "config" / "profiles" / f"{layout}.yaml" + if source.is_file(): + return load_hand_profile(source) + from ament_index_python.packages import get_package_share_directory + return load_hand_profile(Path(get_package_share_directory("linkerhand_calibration")) / "config" / "profiles" / f"{layout}.yaml") + + +def load_hand_profile(path: str | Path) -> CalibrationProfile: + """Load the complete extension contract; no Python model hook is used.""" + source = Path(path).expanduser().resolve() + payload = _map(yaml.safe_load(source.read_text(encoding="utf-8")), str(source)) + if int(payload.get("schema_version", 0)) != 1: + raise ValueError("hand profile schema_version must be 1") + _validate_keys(payload) + command = _map(payload.get("command"), "command") + vision = _map(payload.get("vision"), "vision") + motion = _map(payload.get("motion"), "motion") + measurement = _map(payload.get("measurement"), "measurement") + zero = _map(payload.get("zero"), "zero") + quality = _map(payload.get("quality"), "quality") + scope = _map(payload.get("scope"), "scope") + artifacts = _map(payload.get("artifacts"), "artifacts") + acquisition = _map(payload.get("acquisition", {}), "acquisition") + urdf = _map(payload.get("urdf", {}), "urdf") + + command_layout = CommandLayout( + names=tuple(str(value) for value in command.get("names", ())), + baseline_u8=tuple(int(value) for value in command.get("baseline_u8", ())), + command_index_by_joint={ + str(name): int(index) + for name, index in _map( + command.get("command_index_by_joint", {}), + "command.command_index_by_joint", + ).items() + }, + disabled_indices=frozenset( + int(index) for index in command.get("disabled_indices", ()) + ), + urdf_joint_by_joint={ + str(name): str(joint) + for name, joint in _map( + command.get("urdf_joint_by_joint", {}), + "command.urdf_joint_by_joint", + ).items() + }, + feedback_name_aliases={ + str(name): str(canonical) + for name, canonical in _map( + command.get("feedback_name_aliases", {}), + "command.feedback_name_aliases", + ).items() + }, + speed_slot_by_command_index={ + int(index): int(slot) + for index, slot in _map( + command.get("speed_slot_by_command_index", {}), + "command.speed_slot_by_command_index", + ).items() + }, + unit=str(command.get("unit", "u8")), + baseline=tuple(float(value) for value in command.get("baseline", ())), + lower_bounds=tuple(float(value) for value in command.get("lower_bounds", ())), + upper_bounds=tuple(float(value) for value in command.get("upper_bounds", ())), + feedback_lower_bounds=tuple( + float(value) for value in command.get("feedback_lower_bounds", ()) + ), + feedback_upper_bounds=tuple( + float(value) for value in command.get("feedback_upper_bounds", ()) + ), + feedback_by_index=bool(command.get("feedback_by_index", False)), + sdk_to_joint_direction=tuple(command.get("sdk_to_joint_direction", ())), + maximum_velocity=tuple(float(value) for value in command.get("maximum_velocity", ())), + ) + views = tuple( + ViewSpec( + str(view["name"]), + tuple( + TagSpec( + str(tag["role"]), + int(tag["id"]), + bool(tag.get("fixed_reference", False)), + None if tag.get("link") is None else str(tag["link"]), + float(tag.get("size_m", 0.016)), + ) + for tag in view.get("tags", ()) + ), + ) + for view in vision.get("views", ()) + ) + tasks = tuple( + TaskSpec( + key=str(task["key"]), + view=str(task["view"]), + command_index=int(task["command_index"]), + joints=tuple(str(name) for name in task.get("joints", ())), + auxiliary_commands=tuple( + (int(pair[0]), float(pair[1])) + for pair in task.get("auxiliary_commands", ()) + ), + validation_only=bool(task.get("validation_only", False)), + start_u8=int(task.get("start_u8", 255)), + end_u8=int(task.get("end_u8", 0)), + preflight_speed_u8=( + None if task.get("preflight_speed_u8") is None + else int(task["preflight_speed_u8"]) + ), + formal_speed_u8=( + None if task.get("formal_speed_u8") is None + else int(task["formal_speed_u8"]) + ), + start=None if task.get("start") is None else float(task["start"]), + end=None if task.get("end") is None else float(task["end"]), + preflight_speed=( + None if task.get("preflight_speed") is None + else float(task["preflight_speed"]) + ), + formal_speed=( + None if task.get("formal_speed") is None + else float(task["formal_speed"]) + ), + preparation_groups=tuple(tuple(int(index) for index in group) for group in task.get("preparation_groups", ())), + entry_waypoints=tuple(tuple((int(index), float(value)) for index, value in waypoint) + for waypoint in task.get("entry_waypoints", ())), + ) + for task in motion.get("tasks", ()) + ) + measurements = { + str(name): MeasurementSpec( + joint=str(name), + kind=str(value["kind"]), + view=None if value.get("view") is None else str(value["view"]), + parent_role=( + None if value.get("parent_role") is None + else str(value["parent_role"]) + ), + child_role=( + None if value.get("child_role") is None + else str(value["child_role"]) + ), + validation_source=( + None if value.get("validation_source") is None + else str(value["validation_source"]) + ), + pose_axis_line_required=bool( + value.get("pose_axis_line_required", True) + ), + ) + for name, value in _map( + measurement.get("measurements", {}), + "measurement.measurements", + ).items() + } + profile = CalibrationProfile( + key=ProfileKey.parse(str(payload["profile_id"])), + namespace=str(payload["namespace"]), + command=command_layout, + vision=VisionRigSpec( + views, + str(vision["common_frame"]), + str(vision["extrinsic_reference_view"]), + dict(vision.get("extrinsics_quality_limits", {})), + dict(vision.get("minimum_capture_counts", {})), + ), + motion=MotionPolicy( + tasks, + tuple(tuple(int(item) for item in row) for row in motion.get("preparation_waypoints_u8", ())), + tuple(tuple(int(item) for item in row) for row in motion.get("safe_return_waypoints_u8", ())), + dict(motion.get("speed_parameters", {})), + bool(motion.get("precheck_sweeps", False)), + bool(motion.get("steady_command_checkpoints", False)), + tuple(tuple(int(index) for index in group) for group in motion.get("return_groups", ())), + tuple(ReferenceWaypoint(str(row["key"]), tuple(map(float, row["command"])), + {str(view): tuple(map(int, ids)) for view, ids in row["tag_ids_by_view"].items()}) + for row in motion.get("resume_verification_waypoints", ())), + ), + measurement=MeasurementPolicy( + measurements, + dict(measurement.get("cross_view_sources", {})), + _set(measurement.get("image_curve_joints")), + bool(measurement.get("directional_zero", False)), + bool(measurement.get("cross_view_roll_curve", False)), + bool(measurement.get("stable_cross_view_cone_bias", False)), + _set(measurement.get("candidate_selection_tasks", ())), + str(measurement.get("input_domain", "")), + ), + zero=ZeroSolvePolicy( + _set(zero.get("active_joints")), + _set(zero.get("passive_joints")), + tuple(str(value) for value in zero.get("direct_zero_joints", ())), + tuple(str(value) for value in zero.get("axis_joints", ())), + _set(zero.get("mechanical_endpoint_joints")), + _set(zero.get("post_solve_endpoint_joints")), + dict(zero.get("mimic_source_by_joint", {})), + _set(zero.get("cad_frozen_joints")), + dict(zero.get("endpoint_anchor_by_joint", {})), + _set(zero.get("fitted_mimic_joints")), + dict(zero.get("coupling_model_by_joint", {})), + dict(zero.get("transferred_zero_sources", {})), + dict(zero.get("transferred_mimic_sources", {})), + dict(zero.get("spatial", {})), + ), + quality=QualityPolicy( + tuple(int(value) for value in quality.get("training_cycles", ())), + None if quality.get("holdout_cycle") is None else int(quality["holdout_cycle"]), + _set(quality.get("hard_threshold_keys")), + dict(quality.get("retry_metric_scope", {})), + bool(quality.get("isolated_holdout", False)), + ), + scope=ScopePolicy( + { + str(name): _set(values) + for name, values in _map(scope.get("calibrate_joints", {}), "scope.calibrate_joints").items() + }, + { + str(name): _set(values) + for name, values in _map(scope.get("frozen_joints", {}), "scope.frozen_joints").items() + }, + str(scope.get("default_scope", "full")), + ), + artifacts=ArtifactPolicy( + int(artifacts["output_schema_version"]), + str(artifacts["calibration_filename"]), + str(artifacts["corrected_urdf_filename"]), + _set(artifacts.get("protected_input_fields")), + str(artifacts.get("publication_pointer", "latest_passed")), + _set(artifacts.get("session_compatibility_tokens")), + bool(artifacts.get("publish_corrected_urdf", False)), + ), + acquisition=AcquisitionPolicy(**dict(acquisition)), + sdk_adapter=str(payload["sdk_adapter"]), + urdf_authorized_fields={ + str(name): _set(fields) + for name, fields in _map( + urdf.get("authorized_fields", {}), "urdf.authorized_fields" + ).items() + }, + joint_coverage={ + str(name): str(value) + for name, value in _map( + payload.get("joint_coverage", {}), "joint_coverage" + ).items() + }, + ) + validate_profile(profile) + return profile + + +__all__ = ["load_hand_profile", "load_bundled_hand_profile"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py new file mode 100644 index 0000000..0670d71 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/observations.py @@ -0,0 +1,40 @@ +"""Compile one physical Tag-to-link binding from Profile and CAD topology.""" + +from __future__ import annotations + +from ..core.domain.profile import CalibrationProfile +from ..core.urdf.kinematics import UrdfKinematicModel + + +def compile_tag_links(profile: CalibrationProfile, model: UrdfKinematicModel) -> dict[str, str]: + """A shared Tag belongs to the deepest observed link on a single chain. + + Example: the same Tag can observe a root axis with distal axes held and a + distal axis with the root held. It still has only ONE physical mounting. + Ambiguous branches require an explicit ``TagSpec.link``; no name guessing. + """ + result = {} + for view in profile.vision.views: + for tag in view.tags: + if tag.link is not None: + if tag.link not in model.links: + raise ValueError(f"Tag references missing URDF link:{tag.role}:{tag.link}") + result[tag.role] = tag.link + continue + if tag.fixed_reference: + result[tag.role] = model.root_link + continue + candidates = { + profile.command.urdf_joint_by_joint.get(name, name) + for name, spec in profile.measurement.measurements.items() + if spec.view == view.name and spec.child_role == tag.role + and spec.kind != "axis_cross_view_validation" + } + if not candidates or not candidates <= model.joints.keys(): + raise ValueError(f"Tag link cannot be derived; declare link explicitly:{tag.role}") + descendants = [name for name in candidates + if candidates <= {joint.name for joint in model._chain(name)}] + if len(descendants) != 1: + raise ValueError(f"Tag is assigned to different kinematic branches:{tag.role}") + result[tag.role] = model.joints[descendants[0]].child + return result diff --git a/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py new file mode 100644 index 0000000..5e02ef6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/profiles/validator.py @@ -0,0 +1,55 @@ +"""Validate executable mathematics and CAD bindings before opening hardware.""" + +from ..core.domain.profile import validate_profile +from ..core.fitting.session import compile_spatial_profile +from ..core.urdf.kinematics import UrdfKinematicModel +from .observations import compile_tag_links + + +def validate_executable_profile(profile, source_urdf): + validate_profile(profile) + if not profile.command.sdk_to_joint_direction: + raise ValueError("executable Profile must declare fixed SDK-to-joint directions") + if any(sum(tag.fixed_reference for tag in view.tags) != 1 for view in profile.vision.views): + raise ValueError("each capture view must declare exactly one fixed reference Tag") + model = UrdfKinematicModel(source_urdf) + spatial = compile_spatial_profile(profile) + links = compile_tag_links(profile, model) + for joint, observer in spatial.offset_observer_joint.items(): + if joint == observer: + raise ValueError(f"absolute_zero_unobservable:{joint}:a free Tag on its own hinge is not a CAD datum") + measured = {joint for task in profile.motion.tasks for joint in task.joints} + if not measured <= model.joints.keys() or not profile.zero.active_joints <= model.joints.keys(): + raise ValueError("Profile joint keys must name URDF joints; SDK names belong in command.names") + for name, spec in profile.measurement.measurements.items(): + if spec.kind not in {"relative_rotation", "urdf_axis_chain", "curve", "axis_cross_view_validation"}: + raise ValueError(f"unsupported measurement primitive:{name}:{spec.kind}") + if not set(profile.zero.axis_joints) <= measured: + raise ValueError("spatial observer axes require declared measured tasks") + if profile.artifacts.output_schema_version != 2: + raise ValueError("production calibration requires unified output_schema_version: 2; legacy formats are read-only") + for target, source in profile.zero.mimic_source_by_joint.items(): + if target not in model.joints or model.joints[target].mimic_joint != source: + raise ValueError(f"Profile mimic topology differs from CAD:{target}") + if not set(profile.urdf_authorized_fields) <= model.joints.keys(): + raise ValueError("URDF authorization references a missing joint") + conflicts = [] + for name, joint in model.joints.items(): + if joint.mimic_joint is None or joint.lower is None: + continue + parent = model.joints[joint.mimic_joint] + if parent.lower is None: + continue + lo, hi = sorted((parent.lower*joint.mimic_multiplier+joint.mimic_offset, + parent.upper*joint.mimic_multiplier+joint.mimic_offset)) + if lo < joint.lower-1e-9 or hi > joint.upper+1e-9: + conflicts.append({"joint": name, "source_joint": joint.mimic_joint, + "reachable_rad": [lo, hi], "cad_limits_rad": [joint.lower, joint.upper], + "measured_mimic_authorized": name in profile.zero.fitted_mimic_joints}) + return {"profile_id": profile.key.profile_id, "tag_links": links, + "zero_observers": dict(spatial.offset_observer_joint), + "cad_retained": sorted(profile.zero.cad_frozen_joints), + "transferred": dict(profile.zero.transferred_zero_sources), + "cad_constraint_conflicts": conflicts, + "observability": "structural_checks_only; numerical_rank_and_holdout_required", + "arbitrary_multiaxis_validated": False} diff --git a/src/linkerhand_calibration/linkerhand_calibration/publication.py b/src/linkerhand_calibration/linkerhand_calibration/publication.py index 4f836d4..0a2e6d7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/publication.py +++ b/src/linkerhand_calibration/linkerhand_calibration/publication.py @@ -2,6 +2,6 @@ import sys -from .models.g20 import publication as _implementation +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20 import publication as _implementation sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py index d8ad4cf..eaf337e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py @@ -1,22 +1,39 @@ -"""Common calibration engine; model registry imports remain lazy.""" +"""Common calibration runtime, independent of model registries.""" -from .controller import ControllerSnapshot, SessionController, SessionState 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) +from .reference_lock import ReferenceLock, ReferenceMovement +from .resume import ( + ResumeDecision, + ResumeFingerprint, + ResumeVerifier, + TagPoseFingerprint, +) +from .safety import SafetyDecision, SafetyPolicy, SafetySample +from .session import CalibrationPhase, CalibrationSession, SessionAction +from .trajectory import ( + AvoidanceArrival, + avoidance_arrival, + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, + cosine_position_trajectory_u8, + smoothstep_position, +) __all__ = [ - "ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult", - "ControllerSnapshot", "ScanUnit", "SessionController", "SessionState", - "SweepQuality", "build_session_controller", + "ACQUISITION_POLICY_VERSION", "CalibrationEngine", + "CalibrationPhase", "CalibrationSession", + "ReferenceLock", "ReferenceMovement", "ResumeDecision", + "ResumeFingerprint", "ResumeVerifier", "SafetyDecision", "SafetyPolicy", + "SafetySample", "ScanUnit", "SessionAction", + "SweepQuality", "TagPoseFingerprint", + "AvoidanceArrival", "avoidance_arrival", "cosine_position_trajectory_u8", + "build_calibration_motion_command", "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", + "smoothstep_position", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py new file mode 100644 index 0000000..0b7ad99 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/acquisition.py @@ -0,0 +1,123 @@ +"""Retained image selection shared by online finalizers and offline replay.""" + +from __future__ import annotations + +import math +import json +from pathlib import Path +from typing import Any, Mapping, Sequence + +from ..core.domain.profile import CalibrationProfile +from ..core.fitting.observed_motion import image_identity + + +def load_capture(path): + rows = [] + with Path(path).open(encoding="utf-8") as stream: + for number, line in enumerate(stream, 1): + if not line.strip(): + continue + try: + row = json.loads(line) + except json.JSONDecodeError as error: + raise ValueError(f"invalid calibration JSONL at line {number}") from error + if not isinstance(row, dict): + raise ValueError(f"calibration JSONL record is not an object at line {number}") + rows.append(row) + return rows + + +def accepted_joint_records(profile: CalibrationProfile, records: Sequence[Mapping[str, Any]], + *, directions_are_task_relative: bool = False, sample_phase="sweep"): + """Select newest attempt by scan unit, preserve complete native evidence. + + Never synthesize identities from row number, SDK value or a fitted curve. + The same camera frame can observe several joints, but only once per joint. + """ + expected = {name for name, spec in profile.measurement.measurements.items() + if spec.view is not None and spec.kind != "axis_cross_view_validation"} + samples = [row for row in records if row.get("joint") in expected and "relative_quaternion_xyzw" in row + and row.get("sample_phase", "sweep") == sample_phase] + def unit(row): + return str(row["task_name"]), int(row["cycle"]), str(row["direction"]) + latest = {} + for row in records: + if not all(key in row for key in ("task_name", "cycle", "direction")): + continue + key = unit(row) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + selected = {name: [] for name in expected} + seen = set() + tasks = {task.key: task for task in profile.motion.tasks} + for raw in samples: + if int(raw.get("attempt", 1)) != latest[unit(raw)]: + continue + row = dict(raw) + task = tasks.get(str(row["task_name"])) + if task is None or row["joint"] not in task.joints: + raise ValueError("retained observation is not authorized by its task") + if directions_are_task_relative and task.end_value > task.start_value: + row["direction"] = {"increasing": "decreasing", "decreasing": "increasing"}[row["direction"]] + row["sample_id"] = image_identity(row) + identity = (row["joint"], row["sample_id"]) + if identity in seen: + raise ValueError(f"duplicate retained image:{identity}") + seen.add(identity) + if profile.command.unit == "u8": + value = float(row[profile.curve_input_domain]) + if not math.isfinite(value) or not 0 <= value <= 255: + raise ValueError("retained feedback outside byte domain") + # Raw command and feedback are immutable, distinct observations. + # Quantization belongs to the numerical primitive, not capture. + else: + value = float(row["feedback_rad"]) + index = task.command_index + if not math.isfinite(value) or not profile.command.minimum_feedback_values[index] <= value <= profile.command.maximum_feedback_values[index]: + raise ValueError("retained feedback outside physical SDK domain") + if profile.curve_input_domain.startswith("command_"): + command = float(row["command_rad"]) + if not math.isfinite(command) or not profile.command.minimum_values[index] <= command <= profile.command.maximum_values[index]: + raise ValueError("retained command outside SDK domain") + row["state_rad"] = list(row["command_vector_rad"]) + selected[str(row["joint"])].append(row) + return selected + + +def accepted_secondary_records(profile, records, *, directions_are_task_relative=False, sample_phase="sweep"): + """Select secondary observations without dropping their image/pose evidence. + + Secondary rows use ``observation_joint`` to identify the scanned joint; + they must never be mixed into its primary-view curve. + """ + tasks = {task.key: task for task in profile.motion.tasks} + samples = [dict(row) for row in records if row.get("observation_joint") in profile.measurement.cross_view_sources + and row.get("sample_phase", "sweep") == sample_phase] + latest = {} + for row in records: + if not all(key in row for key in ("task_name", "cycle", "direction")): + continue + key = (row["task_name"], int(row["cycle"]), row["direction"]) + latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1))) + output, seen = {}, set() + for row in samples: + key = (row["task_name"], int(row["cycle"]), row["direction"]) + if int(row.get("attempt", 1)) != latest[key]: + continue + name = row["observation_joint"] + task = tasks.get(str(row["task_name"])) + if task is None or name not in task.joints: + raise ValueError("secondary observation is not authorized by its task") + spec = profile.measurement.measurements[profile.measurement.cross_view_sources[name]] + if row.get("view") != spec.view: + raise ValueError("secondary observation has a different camera view") + row["sample_id"] = image_identity(row) + identity = (name, row["sample_id"]) + if identity in seen: + raise ValueError("duplicate secondary observation image") + seen.add(identity) + if directions_are_task_relative and task.end_value > task.start_value: + row["direction"] = {"increasing": "decreasing", "decreasing": "increasing"}[row["direction"]] + if profile.curve_input_domain.startswith("command_"): + row[f"state_{profile.command.unit}"] = list(row[f"command_vector_{profile.command.unit}"]) + output.setdefault(name, []).append(row) + return output diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py index f1ec941..cdff85b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py @@ -1,5 +1,13 @@ """Camera, detector, and hand-SDK runtime adapters.""" from .base import HardwareHealth, ProfileSdkAdapter, SdkAdapter +from .legacy_byte_sdk import LegacyByteSdkAdapter +from .o12_hcan_sdk import O12HcanSdkAdapter -__all__ = ["HardwareHealth", "ProfileSdkAdapter", "SdkAdapter"] +__all__ = [ + "HardwareHealth", + "LegacyByteSdkAdapter", + "O12HcanSdkAdapter", + "ProfileSdkAdapter", + "SdkAdapter", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py index 5ed9b35..107aecc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/base.py @@ -19,10 +19,17 @@ class HardwareHealth: connected: bool position_mode: bool active_faults: tuple[str, ...] = () + feedback_fresh: bool = True + diagnostic: str = "" @property def safe(self) -> bool: - return self.connected and self.position_mode and not self.active_faults + return ( + self.connected + and self.position_mode + and self.feedback_fresh + and not self.active_faults + ) @runtime_checkable @@ -49,6 +56,8 @@ class SdkAdapter(Protocol): def set_speed(self, command_index: int, speed: float) -> None: ... + def initial_command(self, feedback: Sequence[float]) -> tuple[float, ...]: ... + class ProfileSdkAdapter: """Shared profile-driven parsing and physical-domain validation. @@ -61,12 +70,20 @@ class ProfileSdkAdapter: def __init__(self, command_layout: CommandLayout) -> None: self.command_layout = command_layout + def initial_command(self, feedback: Sequence[float]) -> tuple[float, ...]: + """Protocols must explicitly declare how to start from observed pose.""" + raise NotImplementedError("SDK adapter has no safe initial-command conversion") + 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 and ( + len(names) != len(values) or len(set(names)) != len(names) + ): + 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(): @@ -74,9 +91,15 @@ class ProfileSdkAdapter: 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) + try: + parsed = tuple(float(by_name[name]) for name in layout.names) + except (ValueError, TypeError, OverflowError): + return None else: - parsed = tuple(float(value) for value in values) + try: + parsed = tuple(float(value) for value in values) + except (ValueError, TypeError, OverflowError): + return None return parsed if all(math.isfinite(value) for value in parsed) else None def validate_command(self, values: Sequence[float]) -> tuple[float, ...]: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/legacy_byte_sdk.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/legacy_byte_sdk.py new file mode 100644 index 0000000..7ae71ff --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/legacy_byte_sdk.py @@ -0,0 +1,51 @@ +"""Common adapter for existing byte-command SDK transports.""" + +from __future__ import annotations + +from typing import Callable, Sequence +import math + +from ...core import CommandLayout +from .base import HardwareHealth, ProfileSdkAdapter + + +class LegacyByteSdkAdapter(ProfileSdkAdapter): + """Bind profile validation to arbitrary ROS/vendor transport callbacks.""" + + def __init__( + self, + command_layout: CommandLayout, + *, + publish: Callable[[tuple[float, ...]], None], + set_speed_callback: Callable[[int, float], None], + health_callback: Callable[[], HardwareHealth], + ) -> None: + if command_layout.unit != "u8": + raise ValueError("legacy byte adapter requires a u8 command layout") + super().__init__(command_layout) + self._publish = publish + self._set_speed = set_speed_callback + self._health = health_callback + + def publish_position(self, values: Sequence[float]) -> None: + validated = self.validate_command(values) + self._publish(tuple(float(round(value)) for value in validated)) + + def set_speed(self, command_index: int, speed: float) -> None: + index = int(command_index) + if not 0 <= index < self.command_layout.command_count: + raise ValueError("speed command index is out of range") + numeric = float(speed) + if not math.isfinite(numeric) or not 0 < numeric <= 255: + raise ValueError("byte SDK speed must be in (0, 255]") + slot = self.command_layout.speed_slot_by_command_index.get(index, index) + self._set_speed(int(slot), float(round(numeric))) + + def health(self) -> HardwareHealth: + return self._health() + + def initial_command(self, feedback): + return self.validate_command(tuple(float(round(v)) for v in feedback)) + + +__all__ = ["LegacyByteSdkAdapter"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o12_bridge.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o12_bridge.py new file mode 100644 index 0000000..0296257 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o12_bridge.py @@ -0,0 +1,118 @@ +"""Thin HCAN transport: independent readback, no calibration business logic. + +Only this process opens HCAN. Before the first explicit position message it +calls read methods only; it never moves to a presumed zero to obtain feedback. +""" + +import hashlib +import json +import math +from pathlib import Path +import time + + +ERROR_FIELDS = ("stalled", "overheat", "over_current", "motor_except", "commu_except") + + +def read_health(hand): + modes = tuple(int(v) for v in hand.get_all_control_modes()) + errors = tuple(hand.get_all_error_reports()) + if len(modes) != 12 or len(errors) != 12: + raise ValueError("SDK returned an incomplete health report") + codes = tuple(sum(1 << i for i, field in enumerate(ERROR_FIELDS) if bool(getattr(report, field))) + for report in errors) + return {"position_mode": all(mode == 0 for mode in modes), "modes": modes, + "error_codes": codes, "active_faults": [f"channel={i}:{ERROR_FIELDS[bit]}" + for i, code in enumerate(codes) for bit in range(4) if code & (1 << bit)], + "communication_latches": [i for i, code in enumerate(codes) if code & 16], + "temperature_policy": "vendor_error_bit1_overheat"} + + +def main(args=None): + import rclpy + from rclpy.node import Node + from sensor_msgs.msg import JointState + from std_msgs.msg import String + import yaml + from .vendor_package import load_vendor_sdk + + class Bridge(Node): + def __init__(self): + super().__init__("o12_sdk_bridge") + for name, default in {"vendor_config": "", "vendor_config_sha256": "", + "vendor_python_package": "", "vendor_package_sha256": "", "hand_type": "right", + "topic_prefix": "/o12/right", "readback_hz": 50.0}.items(): + self.declare_parameter(name, default) + value = lambda name: self.get_parameter(name).value + config = Path(value("vendor_config")) + if hashlib.sha256(config.read_bytes()).hexdigest() != value("vendor_config_sha256"): + raise ValueError("vendor connection configuration hash mismatch") + sdk = load_vendor_sdk(value("vendor_python_package"), value("vendor_package_sha256")) + side = str(value("hand_type")) + if side not in {"left", "right"}: + raise ValueError("invalid SDK hand side") + connection = yaml.safe_load(config.read_text())["/**"]["ros__parameters"][side+"_hand"] + if connection["connection_type"] != "hcan": + raise ValueError("HCAN adapter received a different transport") + self.hand = sdk.OmniHandPro2025.create_hand_by_hcan( + hand_type=sdk.HandType.LEFT if side == "left" else sdk.HandType.RIGHT, + hand_device_id=int(connection["hand_device_id"]), + canfd_device_id=int(connection["canfd_device_id"]), + canfd_channel_id=int(connection["canfd_channel_id"])) + self.hand.set_request_interval(int(connection.get("request_interval_ms", 0))) + self.hand.set_frame_recv_timeout(int(connection.get("frame_recv_timeout_ms", 100))) + if not self.hand.init(): + raise RuntimeError("HCAN initialization failed") + prefix = str(value("topic_prefix")).rstrip("/") + self.state_pub = self.create_publisher(JointState, prefix+"/joint_states", 10) + self.health_pub = self.create_publisher(String, prefix+"/calibration_health", 10) + self.create_subscription(JointState, prefix+"/joint_cmd", self.command, 1) + self.pending, self.pending_at, self.health_at = None, 0.0, 0.0 + self.health = None + self.invalid_health_since = None + self.create_timer(1.0/float(value("readback_hz")), self.tick) + + def command(self, message): + values = tuple(float(v) for v in message.position) + if len(values) != 12 or not all(math.isfinite(v) for v in values): + self.get_logger().error("rejected malformed SDK position vector") + return + self.pending, self.pending_at = values, time.monotonic() + + def tick(self): + now = time.monotonic() + if now-self.health_at >= .5: + try: + self.health = read_health(self.hand) + self.invalid_health_since = None + except (ValueError, TypeError, AttributeError) as error: + if self.invalid_health_since is None: + self.invalid_health_since = now + if now-self.invalid_health_since >= 1.0: + self.health = {"position_mode": False, "active_faults": ["health_report_unparseable"], "diagnostic": str(error)} + self.health_at = now + if self.health is not None: + report = String() + report.data = json.dumps(self.health) + self.health_pub.publish(report) + if (self.pending is not None and now-self.pending_at <= 1.0 and self.health is not None + and self.health["position_mode"] and not self.health["active_faults"]): + self.hand.set_all_active_joint_angles(list(self.pending)) + self.pending = None + values = tuple(float(v) for v in self.hand.get_all_active_joint_angles()) + if len(values) == 12 and all(math.isfinite(v) for v in values): + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + # Product-fixed order, intentionally no SDK-generated names. + message.position = list(values) + self.state_pub.publish(message) + + rclpy.init(args=args) + node = None + try: + node = Bridge() + rclpy.spin(node) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o12_hcan_sdk.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o12_hcan_sdk.py new file mode 100644 index 0000000..0b904f4 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/o12_hcan_sdk.py @@ -0,0 +1,90 @@ +"""Physical-angle HCAN adapter used by O12 without leaking into the engine.""" + +from __future__ import annotations + +from typing import Callable, Sequence +import math +import time + +from ...core import CommandLayout +from .base import HardwareHealth, ProfileSdkAdapter + + +class O12HcanSdkAdapter(ProfileSdkAdapter): + def __init__( + self, + command_layout: CommandLayout, + *, + publish: Callable[[tuple[float, ...]], None], + set_speed_callback: Callable[[int, float], None], + health_callback: Callable[[], HardwareHealth], + clock: Callable[[], float] = time.monotonic, + health_timeout_seconds: float = 1.0, + ) -> None: + if command_layout.unit != "rad": + raise ValueError("O12 HCAN adapter requires a radian command layout") + if not command_layout.feedback_by_index: + raise ValueError("O12 feedback must use the fixed SDK array order") + super().__init__(command_layout) + self._publish = publish + self._set_speed = set_speed_callback + self._health = health_callback + if not math.isfinite(health_timeout_seconds) or health_timeout_seconds <= 0: + raise ValueError("health timeout must be finite and positive") + self._clock = clock + self._health_timeout_seconds = health_timeout_seconds + self._invalid_health_since: float | None = None + self._last_health = HardwareHealth(False, False, diagnostic="waiting") + + def publish_position(self, values: Sequence[float]) -> None: + self._publish(self.validate_command(values)) + + def initial_command(self, feedback): + # Vendor get/set_all_active_joint_angles use the same nominal radian + # coordinates. This initializes a smooth trajectory AFTER Start; + # it is not a baseline command sent during preview, nor a calibration + # correction inferred from command/feedback error. + layout = self.command_layout + if len(feedback) != layout.command_count or any(not math.isfinite(v) or not lo <= v <= hi + for v, lo, hi in zip(feedback, layout.minimum_feedback_values, layout.maximum_feedback_values)): + raise ValueError("cannot initialize motion from invalid physical feedback") + return self.validate_command(tuple(min(hi, max(lo, value)) + for value, lo, hi in zip(feedback, layout.minimum_values, layout.maximum_values))) + + def set_speed(self, command_index: int, speed: float) -> None: + index = int(command_index) + if not 0 <= index < self.command_layout.command_count: + raise ValueError("speed command index is out of range") + if not math.isfinite(float(speed)) or float(speed) <= 0.0: + raise ValueError("physical speed must be positive") + self._set_speed(index, float(speed)) + + def health(self) -> HardwareHealth: + """Retain good health through transient parse errors, using elapsed time. + + Three malformed packets in a single callback burst are not evidence of + a persistent fault. A valid active fault is still returned immediately. + """ + try: + health = self._health() + if not isinstance(health, HardwareHealth): + raise TypeError("SDK health callback returned an invalid report") + except (TypeError, ValueError, IndexError) as error: + now = self._clock() + if self._invalid_health_since is None: + self._invalid_health_since = now + if now - self._invalid_health_since < self._health_timeout_seconds: + return self._last_health + return HardwareHealth( + connected=self._last_health.connected, + position_mode=self._last_health.position_mode, + active_faults=("health_report_unparseable",), + feedback_fresh=self._last_health.feedback_fresh, + diagnostic=str(error), + ) + self._invalid_health_since = None + self._last_health = health + return health + + +__all__ = ["O12HcanSdkAdapter"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py new file mode 100644 index 0000000..0c17292 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_binding.py @@ -0,0 +1,67 @@ +"""SDK binding with explicit feedback freshness, clock and health input ports.""" + +from dataclasses import dataclass +import json +from typing import Callable + +from .base import HardwareHealth +from .legacy_byte_sdk import LegacyByteSdkAdapter +from .o12_hcan_sdk import O12HcanSdkAdapter + + +@dataclass(frozen=True) +class SdkBindingPorts: + feedback_fresh: Callable[[], bool] + monotonic: Callable[[], float] + subscribe_health: Callable[[Callable[[str], None]], None] + + +def _byte(profile, ports, publish, set_speed): + return LegacyByteSdkAdapter(profile.command, publish=publish, set_speed_callback=set_speed, + health_callback=lambda: HardwareHealth(True, True, feedback_fresh=ports.feedback_fresh(), + diagnostic="position_protocol;no_separate_fault_telemetry")) + + +@dataclass +class HealthReport: + payload: dict | None = None + received_at: float = 0.0 + invalid: bool = False + + +def _hcan(profile, ports, publish, set_speed): + latest = HealthReport() + + def receive(payload: str): + try: + value = json.loads(payload) + if not isinstance(value.get("position_mode"), bool) or not isinstance(value.get("active_faults"), list): + raise ValueError("malformed SDK health") + latest.payload, latest.received_at, latest.invalid = value, ports.monotonic(), False + except (ValueError, TypeError, AttributeError): + latest.invalid = True + + ports.subscribe_health(receive) + + def health(): + value = latest.payload + if value is None: + return HardwareHealth(True, False, feedback_fresh=ports.feedback_fresh(), diagnostic="waiting_for_sdk_health") + if latest.invalid or ports.monotonic()-latest.received_at > 1.0: + raise ValueError("SDK health report is stale or unparseable") + return HardwareHealth(True, value["position_mode"], tuple(map(str, value["active_faults"])), + ports.feedback_fresh(), json.dumps(value, ensure_ascii=False)) + + return O12HcanSdkAdapter(profile.command, publish=publish, set_speed_callback=set_speed, + health_callback=health, clock=ports.monotonic) + + +FACTORIES = {"legacy_byte_sdk": _byte, "o12_hcan_sdk": _hcan} + + +def bind_ros_sdk(profile, ports: SdkBindingPorts, *, publish, set_speed): + try: + factory = FACTORIES[profile.sdk_adapter] + except KeyError as error: + raise ValueError(f"unregistered SDK protocol:{profile.sdk_adapter}") from error + return factory(profile, ports, publish, set_speed) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py new file mode 100644 index 0000000..42ca38d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/ros_topics.py @@ -0,0 +1,20 @@ +"""Topic spelling belongs to SDK protocols, not calibration scheduling.""" + +from dataclasses import dataclass + +from ...core.domain.profile import CalibrationProfile + + +@dataclass(frozen=True) +class SdkTopics: + command: str + feedback: str + + +def sdk_topics(profile: CalibrationProfile) -> SdkTopics: + prefix, side = f"/{profile.key.model.lower()}", profile.key.side + if profile.sdk_adapter == "legacy_byte_sdk": + return SdkTopics(f"{prefix}/cb_{side}_hand_control_cmd", f"{prefix}/cb_{side}_hand_state") + if profile.sdk_adapter == "o12_hcan_sdk": + return SdkTopics(f"{prefix}/{side}/joint_cmd", f"{prefix}/{side}/joint_states") + raise ValueError(f"SDK adapter has no ROS topic binding: {profile.sdk_adapter}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/vendor_package.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/vendor_package.py new file mode 100644 index 0000000..41fe9dd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/vendor_package.py @@ -0,0 +1,36 @@ +"""Load the pinned, bundled vendor wheel without installing globally.""" + +import hashlib +import importlib +from pathlib import Path +import sys +import tempfile +import zipfile + + +def load_vendor_sdk(wheel, expected_sha256): + path = Path(wheel).resolve() + if hashlib.sha256(path.read_bytes()).hexdigest() != expected_sha256: + raise ValueError("vendor SDK wheel changed before loading") + abi = f"cp{sys.version_info.major}{sys.version_info.minor}" + if f"-{abi}-{abi}-" not in path.name: + raise ValueError(f"vendor SDK wheel is not built for {abi}") + # Private per-process directory: never trust another run's writable cache + # of native libraries. The original wheel/SDK are never modified. + directory = tempfile.TemporaryDirectory(prefix="calibration-vendor-") + with zipfile.ZipFile(path) as archive: + for member in archive.infolist(): + target = (Path(directory.name)/member.filename).resolve() + if not target.is_relative_to(Path(directory.name)): + raise ValueError("unsafe path in vendor SDK wheel") + archive.extractall(directory.name) + if hashlib.sha256(path.read_bytes()).hexdigest() != expected_sha256: + directory.cleanup() + raise ValueError("vendor SDK wheel changed during loading") + sys.path.insert(0, directory.name) + module = importlib.import_module("omnihand") + if not Path(module.__file__).resolve().is_relative_to(Path(directory.name)): + raise ValueError("an unpinned vendor SDK was already imported") + # Keep libraries mapped from this directory until the process exits. + module._calibration_package_directory = directory + return module diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/__init__.py new file mode 100644 index 0000000..d6d6484 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/__init__.py @@ -0,0 +1,5 @@ +"""Common artifact serialization and atomic publication.""" + +from .publisher import ArtifactRelease, ArtifactPublisher + +__all__ = ["ArtifactPublisher", "ArtifactRelease"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py new file mode 100644 index 0000000..b5b33ad --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/completion.py @@ -0,0 +1,46 @@ +"""Read back the committed release; never re-fit or re-publish in the runner.""" + +from __future__ import annotations + +import json +import hashlib +from pathlib import Path +from typing import Any, Mapping + +from ...product import ProductConfig + + +def finish_online_artifacts( + config: ProductConfig, session: Path, status: Mapping[str, Any], +) -> dict[str, str]: + profile = config.calibration_contract.typed_profile + outputs = status.get("outputs", {}) + json_path = Path(outputs.get("json") or status.get("final_json") or "") + urdf_path = Path(outputs.get("urdf") or status.get("final_urdf") or "") + for path in (json_path, urdf_path): + if not path.is_file() or not path.resolve().is_relative_to(session.resolve()): + raise ValueError(f"node completion refers to a missing/external artifact: {path}") + payload = json.loads(json_path.read_text(encoding="utf-8")) + unified = profile.artifacts.output_schema_version == 2 + if unified and (payload.get("format") != "unified_calibration_v2" or payload.get("schema_version") != 2): + raise ValueError("completed JSON format differs from selected Profile") + if not unified and payload.get("schema_version") != profile.artifacts.output_schema_version: + raise ValueError("completed JSON schema differs from selected Profile") + if not unified and payload.get("quality", {}).get("passed") is not True: + raise ValueError("completed JSON has no passing quality evidence") + manifest = json.loads((session/"release_manifest.json").read_text(encoding="utf-8")) + if unified and (manifest.get("validation", {}).get("final_file_spatial_replay") != "passed" + or not manifest.get("validation", {}).get("steady_command_tag_holdout")): + raise ValueError("completed release lacks independent command and feedback spatial evidence") + for path, name in ((json_path, "calibration_json"), (urdf_path, "corrected_urdf")): + if manifest.get(name) != path.name or manifest.get(name+"_sha256") != hashlib.sha256(path.read_bytes()).hexdigest(): + raise ValueError("completed artifact differs from the validated release manifest") + if unified: + from .reader import load_unified_mapper + load_unified_mapper(json_path, expected_side=profile.key.side) + if manifest.get("validation", {}).get("profile_id") != profile.key.profile_id: + raise ValueError("release evidence belongs to a different Profile") + pointer = config.session_root / profile.artifacts.publication_pointer + if not pointer.is_symlink() or pointer.resolve() != session.resolve(): + raise ValueError("node completion has no verified publication pointer to this session") + return {"JSON": str(json_path), "URDF": str(urdf_path), "发布指针": str(pointer)} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py new file mode 100644 index 0000000..0d82938 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/controller.py @@ -0,0 +1,76 @@ +"""Background preparation and one-time publication, without session ownership. + +The coordinator holds its state lock before entering this component. The worker +never takes the state lock: it reports stage events through a queue. This keeps +feedback live during fitting and orders cancellation against publication. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from functools import partial +from pathlib import Path +from queue import Empty, SimpleQueue +from typing import Any, Callable, Mapping + +from .finalization import finalize_profile_session +from .publisher import ArtifactPublisher +from .worker import FinalizationWorker + + +@dataclass(frozen=True) +class FinalizationInputs: + session_dir: Path + serial_number: str + source_urdf: Path + protected_inputs: Mapping[str, str] + records: tuple[dict[str, Any], ...] + + +class FinalizationProtocolError(RuntimeError): + """The worker's event order does not match the session's acceptance stages.""" + + +class FinalizationController: + def __init__(self, profile, session_dir: Path, *, finalizer=finalize_profile_session): + self.worker = FinalizationWorker() + self._events: SimpleQueue[str] = SimpleQueue() + self._finalizer = partial(finalizer, profile=profile, phase_changed=self._events.put) + self._publisher = ArtifactPublisher(session_dir.parent, profile.artifacts.publication_pointer) + self.started = False + + def start(self, inputs: FinalizationInputs) -> None: + self.worker.start(self._finalizer, session_dir=inputs.session_dir, + serial_number=inputs.serial_number, source_urdf=inputs.source_urdf, + protected_inputs=dict(inputs.protected_inputs), records=inputs.records) + self.started = True + + def drain_events(self, on_event: Callable[[str], None]) -> None: + while True: + try: + event = self._events.get_nowait() + except Empty: + return + on_event(event) + + def poll(self, on_event: Callable[[str], None], authorize: Callable[[], None]): + with self.worker.lock: + try: + self.drain_events(on_event) + except (ValueError, RuntimeError) as error: + raise FinalizationProtocolError(str(error)) from error + + def commit(result): + # The final event may arrive between the first drain and + # Future.done(). Consume it before authorizing the pointer swap. + self.drain_events(on_event) + authorize() + self._publisher.commit(result[2].staged_release) + + return self.worker.finish_if_ready(commit) + + def cancel(self) -> None: + self.worker.cancel() + + def close(self) -> None: + self.worker.close() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py new file mode 100644 index 0000000..5f73dab --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/evidence.py @@ -0,0 +1,150 @@ +"""Compile retained capture records into immutable, model-neutral evidence.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Mapping, Sequence + + +from ...core.domain.profile import CalibrationProfile +from ...core.domain.result import JointMapping, evaluate_mappings +from ...core.fitting.tag_installation import ( + FrozenTagInstallation, TagTrainingPose, fit_tag_installations, matrix_tuple, register_base_translation, +) +from ...core.geometry.extrinsics import transform_matrix +from ...core.urdf.kinematics import UrdfKinematicModel +from ...core.urdf.plan import StandardUrdfPlan +from ...core.urdf.tag_acceptance import TagHoldout +from ...profiles.observations import compile_tag_links + + +@dataclass(frozen=True) +class TagReplayEvidence: + common_from_base: tuple[tuple[float, ...], ...] + installations: Mapping[str, FrozenTagInstallation] + holdout: tuple[TagHoldout, ...] + required_roles: tuple[str, ...] + + +def _pose(value): + if isinstance(value, Mapping): + return matrix_tuple(transform_matrix(value["translation_xyz_m"], value["quaternion_xyzw"])) + return matrix_tuple(value) + + +def prepare_command_holdout(profile, mappings, records): + """Raw steady fourth-cycle poses; no registration or mounting fit here.""" + specs = profile.measurement.measurements + tasks = {task.key: task for task in profile.motion.tasks} + seen, result = set(), [] + for row in records: + if row["cycle"] != 3: + continue + if row.get("sample_phase") != "steady": + raise ValueError("command holdout requires stable command observations") + spec = specs[row["joint"]] + task = tasks[row["task_name"]] + identity = (spec.child_role, row["sample_id"]) + if identity in seen: + continue + seen.add(identity) + directions = {joint: row["direction"] for joint, m in mappings.items() + if m.motor_index == task.command_index} + for joint, mapping in mappings.items(): + previous = row.get("command_direction_by_index", ()) + if mapping.motor_index != task.command_index and len(previous) > mapping.motor_index: + directions[joint] = previous[mapping.motor_index] + result.append(TagHoldout(row["sample_id"], spec.child_role, 3, + tuple(row[f"command_vector_{profile.command.unit}"]), directions, _pose(row["child_pose_common"]))) + return tuple(result) + + +def prepare_tag_replay( + *, profile: CalibrationProfile, source_model: UrdfKinematicModel, + plan: StandardUrdfPlan, output_mappings: Mapping[str, JointMapping], + common_from_base, records: Sequence[Mapping[str, Any]], + directions_are_task_relative: bool = False, +) -> TagReplayEvidence: + """Use fit results for training poses; final validation reads files anew. + + Capture records must already have passed direction selection/quality. + A legacy-label conversion is explicit at this ingestion boundary, never + guessed from model identity. No unknown/missing image becomes evidence. + """ + links = compile_tag_links(profile, source_model) + tasks = {task.key: task for task in profile.motion.tasks} + specs = profile.measurement.measurements + required = {spec.child_role for spec in specs.values() if spec.view is not None} + # Include fixed references when observed, but do not demand they remain + # visible after an avoidance pose occludes them. Their locked-frame + # fingerprint is independently protected by the session reference policy. + required.discard(None) + training, holdout = [], [] + unique = {} + for row in records: + name = str(row.get("joint", "")) + if name not in specs or specs[name].view is None: + continue + if int(row.get("cycle", -1)) not in {0, 1, 2, 3}: + continue + spec = specs[name] + if row.get("task_name") not in tasks: + raise ValueError("Tag replay record has no declared task") + task = tasks[str(row["task_name"])] + role = str(spec.child_role) + if row.get("view") != spec.view: + raise ValueError("Tag replay record has a different view") + identity = str(row.get("sample_id", "")) + if not identity and row.get("image_stamp_ns") is not None: + identity = f"{spec.view}:{int(row['image_stamp_ns'])}" + if not identity or "child_pose_common" not in row: + raise ValueError("Tag replay requires identified common-frame poses") + domain = {value.input_domain for value in output_mappings.values()} + if len(domain) != 1: + raise ValueError("Tag replay requires one declared SDK input domain") + input_domain = next(iter(domain)) + field = f"state_{profile.command.unit}" if input_domain.startswith("feedback_") else f"command_vector_{profile.command.unit}" + sdk = tuple(float(value) for value in row[field]) + direction = str(row["direction"]) + if directions_are_task_relative and task.end_value > task.start_value: + direction = {"increasing": "decreasing", "decreasing": "increasing"}[direction] + directions = {joint: direction for joint, mapping in output_mappings.items() + if mapping.motor_index == task.command_index} + pose = _pose(row["child_pose_common"]) + cycle = int(row["cycle"]) + key = (role, identity) + previous = unique.get(key) + signature = (cycle, sdk, tuple(sorted(directions.items())), pose) + if previous is not None: + if previous != signature: + raise ValueError(f"conflicting duplicate capture:{role}:{identity}") + continue + unique[key] = signature + if cycle == 3: + holdout.append(TagHoldout(identity, role, cycle, sdk, directions, pose)) + continue + angles = evaluate_mappings(output_mappings, sdk, directions) + # Resolve the FITTED linear relation in output coordinates, then + # transform every joint back to CAD exactly once. Using source-CAD + # mimic on output parent angles would miss a*delta_parent. + visiting = set() + def resolve(joint): + if joint in angles: + return angles[joint] + if joint in visiting: + raise ValueError("cyclic fitted mimic relation") + visiting.add(joint) + parent, multiplier, offset = plan.mimic_output[joint] + angles[joint] = multiplier * resolve(parent) + offset + visiting.remove(joint) + return angles[joint] + for joint in plan.mimic_output: + resolve(joint) + cad = {joint: angle + plan.zero_offsets_rad.get(joint, 0.0) for joint, angle in angles.items()} + training.append(TagTrainingPose(identity, role, cycle, pose, cad)) + selected_links = {role: links[role] for role in required} + registered_base = register_base_translation(source_model=source_model, common_from_base=common_from_base, + link_by_role=selected_links, observations=training) + mounts = fit_tag_installations(source_model=source_model, common_from_base=registered_base, + link_by_role=selected_links, observations=training) + return TagReplayEvidence(registered_base, mounts, tuple(holdout), tuple(sorted(required))) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py new file mode 100644 index 0000000..f24af80 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/finalization.py @@ -0,0 +1,125 @@ +"""Shared retained-data -> fit -> standard URDF -> read-back -> publication.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Callable + +from ...core.artifacts.storage import atomic_write_json +from ...core.fitting.session import fit_profile_calibration +from ...core.fitting.command_mapping import fit_command_mappings +from ...core.geometry.candidate_selection import resolve_chain_observations +from ...core.geometry.extrinsics import transform_matrix +from ...core.urdf.kinematics import UrdfKinematicModel +from ...core.urdf.result_plan import prepare_standard_result +from ..acquisition import accepted_joint_records, accepted_secondary_records +from .evidence import prepare_tag_replay +from .publisher import ArtifactPublisher, FrozenTagArtifactValidator, StagedRelease +from .serializers.compact_v2 import from_report, REPORT_FILENAME +from .serializers.unified_v1 import serialize as serialize_report +from .standard_loader import validate_with_robot_state_publisher + + +@dataclass(frozen=True) +class CorrectedArtifact: + path: Path + staged_release: StagedRelease + + +def finalize_profile_session(*, profile, session_dir, serial_number, source_urdf, + protected_inputs, records, publish=True, timestamp=None, + standard_loader: Callable = validate_with_robot_state_publisher, + cancelled: Callable[[], bool] = lambda: False, + directions_are_task_relative: bool = False, + phase_changed: Callable[[str], None] = lambda _phase: None): + """No worker may publish after an operator abort. + + The runtime owns cancellation synchronization. This function also checks + immediately before publication and never returns the hand to a pose. + """ + del timestamp + if profile.artifacts.output_schema_version != 2: + raise ValueError("production calibration requires unified output_schema_version: 2; legacy formats are read-only") + directory, source = Path(session_dir).resolve(), Path(source_urdf).resolve() + directory.mkdir(parents=True, exist_ok=True) + def check_cancelled(): + if cancelled(): + raise ValueError("operator_abort:no_artifact_publication") + check_cancelled() + if any(row.get("projection_reprocessing_scope") for row in records): + raise ValueError("partial external projection is diagnostic-only; cannot certify a whole session") + pose_reports = {} + for task in profile.motion.tasks: + if task.key not in profile.measurement.candidate_selection_tasks: + continue + records, report = resolve_chain_observations(records, task_key=task.key, + role_pairs={name: (profile.measurement.measurements[name].parent_role, + profile.measurement.measurements[name].child_role) for name in task.joints}) + pose_reports[task.key] = report + atomic_write_json(directory / "pose_selection_diagnostics.json", pose_reports) + if report["status"] == "legacy_projection_unverified": + raise ValueError("camera_projection_unverified:no_artifact_publication") + accepted = accepted_joint_records(profile, records, directions_are_task_relative=directions_are_task_relative) + secondary = accepted_secondary_records(profile, records, directions_are_task_relative=directions_are_task_relative) + try: + fit = fit_profile_calibration(profile, source, accepted, cross_view_records=secondary) + steady = accepted_joint_records(profile, records, sample_phase="steady", + directions_are_task_relative=directions_are_task_relative) + fit = fit_command_mappings(profile, fit, steady) + except ValueError as error: + atomic_write_json(directory / "fit_diagnostics.json", { + "passed": False, "reason": str(error), "publication_allowed": False}) + raise + check_cancelled() + phase_changed("fit_complete") + prepared = prepare_standard_result(profile, source, protected_inputs["source_urdf_sha256"], fit) + zero = fit.spatial_zero + replay_records = [row for rows in accepted.values() for row in rows] + replay_records.extend(dict(row, joint=profile.measurement.cross_view_sources[name]) + for name, rows in secondary.items() for row in rows) + replay = prepare_tag_replay(profile=profile, source_model=UrdfKinematicModel(source), + plan=prepared.plan, output_mappings=fit.output_mappings, + common_from_base=transform_matrix(zero.base_translation_xyz_m, zero.base_quaternion_xyzw), + records=replay_records) + from .evidence import prepare_command_holdout + steady_secondary = accepted_secondary_records(profile, records, sample_phase="steady", + directions_are_task_relative=directions_are_task_relative) + steady_rows = [r for rows in steady.values() for r in rows] + steady_rows.extend(dict(r, joint=profile.measurement.cross_view_sources[name]) + for name, rows in steady_secondary.items() for r in rows) + command_holdout = prepare_command_holdout(profile, fit.command_mappings, steady_rows) + validator = FrozenTagArtifactValidator(profile.key.profile_id, source, + protected_inputs["source_urdf_sha256"], profile.urdf_authorized_fields, + fit.zero_offsets_rad, replay.common_from_base, replay.installations, + replay.holdout, replay.required_roles, standard_loader, protected_inputs=dict(protected_inputs), + command_observations=command_holdout) + report = serialize_report(profile, fit, prepared, serial_number=serial_number, protected_inputs=protected_inputs) + payload = from_report(report) + naming = {"serial_number": serial_number, "side": profile.key.side, "model": profile.key.model.lower()} + json_path = directory / profile.artifacts.calibration_filename.format(**naming) + urdf_path = directory / profile.artifacts.corrected_urdf_filename.format(**naming) + check_cancelled() + phase_changed("holdout_complete") + prepared.plan.write(source, urdf_path) + atomic_write_json(json_path, payload) + atomic_write_json(directory / REPORT_FILENAME, report) + atomic_write_json(directory / "frozen_tag_installations.json", { + "common_from_base": replay.common_from_base, + "installations": {name: asdict(mount) for name, mount in replay.installations.items()}}) + phase_changed("artifacts_built") + publisher = ArtifactPublisher(directory.parent, profile.artifacts.publication_pointer) + staged = publisher.prepare(session_directory=directory, calibration_json=json_path, + corrected_urdf=urdf_path, validate=validator, cancelled=cancelled) + import json + acceptance = json.loads(staged.validation_json) + atomic_write_json(directory / "standard_urdf_acceptance.json", acceptance) + check_cancelled() + phase_changed("urdf_validated") + if publish: + publisher.commit(staged, cancelled=cancelled) + atomic_write_json(directory / "calibration_summary_zh.json", { + "profile_id": profile.key.profile_id, "result": "PASS", + "publication_evidence": "release_manifest.json and the product publication pointer", + "artifacts": {"json": json_path.name, "urdf": urdf_path.name}, "acceptance": acceptance}) + return payload, fit, CorrectedArtifact(urdf_path, staged) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py new file mode 100644 index 0000000..8622d47 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/publisher.py @@ -0,0 +1,307 @@ +"""Atomic JSON/URDF release shared by all output-schema serializers.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, field +import hashlib +import json +import os +from pathlib import Path +from typing import Any, Callable, Mapping, Sequence +import uuid +import xml.etree.ElementTree as ET + +from ...core.artifacts.storage import atomic_write_json +from ...core.urdf.acceptance import ( + JointHoldout, validate_artifact_structure, validate_standard_urdf_holdout, +) +from ...core.fitting.tag_installation import FrozenTagInstallation +from ...core.urdf.tag_acceptance import TagHoldout, validate_serialized_tag_holdout +from ...core.domain.profile import ProfileKey +from .serializers.compact_v2 import FORMAT as COMPACT_FORMAT, REPORT_FILENAME, from_report + + +def _validate_identity(payload, profile_id): + if payload.get("schema_version") == 4 and "profile_id" not in payload: + # The exact deployed v4 shape has no layout/profile/hash fields. + # The protected caller fixes layout, source geometry and authorization; + # source/JSON/URDF hashes and full identity are kept in the manifest. + key = ProfileKey.parse(profile_id) + if (payload.get("model"), payload.get("side")) == (key.model, key.side): + return + elif payload.get("profile_id") == profile_id: + return + raise ValueError("serialized artifact belongs to another profile") + + +@dataclass(frozen=True) +class StandardArtifactValidator: + """Release evidence independent of the serializer and its fitted payload. + + Construct this from the locked profile and training-frozen measurement + model. A serializer cannot choose its own authorized fields, truth angles, + required joints or protected source hash. + """ + + profile_id: str + source_urdf: Path + source_sha256: str + authorized_fields: Mapping[str, Sequence[str]] + zero_offsets_rad: Mapping[str, float] + observations: tuple[JointHoldout, ...] + required_joints: tuple[str, ...] + required_pose_joints: tuple[str, ...] + standard_loader: Callable[[Path], None] + minimum_samples: int = 40 + + def __call__(self, json_path: Path, urdf_path: Path) -> Mapping[str, Any]: + payload = json.loads(json_path.read_text(encoding="utf-8")) + _validate_identity(payload, self.profile_id) + if not self.required_pose_joints: + raise ValueError("final URDF acceptance requires independent spatial observations") + changes = validate_artifact_structure(source_urdf=self.source_urdf, + corrected_urdf=urdf_path, source_sha256=self.source_sha256, + authorized_fields=self.authorized_fields, payload=payload, + zero_offsets_rad=self.zero_offsets_rad) + metrics = validate_standard_urdf_holdout(corrected_urdf=urdf_path, + payload=payload, observations=self.observations, + required_joints=self.required_joints, required_pose_joints=self.required_pose_joints, + minimum_samples=self.minimum_samples) + # Production injects the standard parser/robot_state_publisher check; + # offline tests inject an independent parser, never a hardware node. + self.standard_loader(urdf_path) + evidence = json.dumps([asdict(row) for row in self.observations], + sort_keys=True, separators=(",", ":"), allow_nan=False).encode() + return { + "acceptance": "serialized_standard_urdf_v1", + "profile_id": self.profile_id, + "source_urdf_sha256": self.source_sha256, + "holdout_evidence_sha256": hashlib.sha256(evidence).hexdigest(), + "holdout_cycle": 3, + "holdout_by_joint": {name: asdict(value) for name, value in metrics.items()}, + "spatial_holdout_joints": list(self.required_pose_joints), + "structured_changes": list(changes), + } + + +@dataclass(frozen=True) +class FrozenTagArtifactValidator: + """Final-file gate for raw Tag observations with training-frozen mounts.""" + + profile_id: str + source_urdf: Path + source_sha256: str + authorized_fields: Mapping[str, Sequence[str]] + zero_offsets_rad: Mapping[str, float] + common_from_base: tuple[tuple[float, ...], ...] + installations: Mapping[str, FrozenTagInstallation] + observations: tuple[TagHoldout, ...] + required_roles: tuple[str, ...] + standard_loader: Callable[[Path], None] + minimum_samples: int = 40 + protected_inputs: Mapping[str, str] = field(default_factory=dict) + command_observations: tuple[TagHoldout, ...] = () + + def __call__(self, json_path: Path, urdf_path: Path) -> Mapping[str, Any]: + payload = json.loads(json_path.read_text(encoding="utf-8")) + _validate_identity(payload, self.profile_id) + feedback_payload = payload + if payload.get("format") == COMPACT_FORMAT: + feedback_payload = json.loads((json_path.parent / REPORT_FILENAME).read_text(encoding="utf-8")) + _validate_identity(feedback_payload, self.profile_id) + if from_report(feedback_payload) != payload: + raise ValueError("compact JSON differs from the frozen calibration report") + validate_artifact_structure(source_urdf=self.source_urdf, + corrected_urdf=urdf_path, source_sha256=self.source_sha256, + authorized_fields=self.authorized_fields, payload=feedback_payload, + zero_offsets_rad=self.zero_offsets_rad) + changes = validate_artifact_structure(source_urdf=self.source_urdf, + corrected_urdf=urdf_path, source_sha256=self.source_sha256, + authorized_fields=self.authorized_fields, payload=payload, + zero_offsets_rad=self.zero_offsets_rad) + metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path, + payload=feedback_payload, common_from_base=self.common_from_base, + installations=self.installations, observations=self.observations, + required_roles=self.required_roles, minimum_samples=self.minimum_samples) + command_metrics = {} + if payload.get("format") in {"unified_calibration_v1", COMPACT_FORMAT}: + try: + command_metrics = validate_serialized_tag_holdout(corrected_urdf=urdf_path, + payload=payload, common_from_base=self.common_from_base, + installations=self.installations, observations=self.command_observations, + required_roles=self.required_roles, minimum_samples=3, input_kind="command") + except ValueError as error: + if payload.get("format") == COMPACT_FORMAT: + raise ValueError(f"compact_command_table_holdout_failed:{error}") from error + raise + self.standard_loader(urdf_path) + evidence = {"base": self.common_from_base, + "installations": {k: asdict(v) for k, v in self.installations.items()}, + "observations": [asdict(row) for row in self.observations]} + evidence["steady_command_observations"] = [asdict(row) for row in self.command_observations] + return {"acceptance": "serialized_standard_urdf_tag_replay_v1", + "profile_id": self.profile_id, "source_urdf_sha256": self.source_sha256, + "protected_inputs": dict(self.protected_inputs), + "holdout_cycle": 3, "tag_holdout": metrics, + "steady_command_tag_holdout": command_metrics, + "final_file_spatial_replay": "passed", + "holdout_evidence_sha256": hashlib.sha256(json.dumps(evidence, + sort_keys=True, separators=(",", ":"), allow_nan=False).encode()).hexdigest(), + "structured_changes": list(changes)} + + +@dataclass(frozen=True) +class ArtifactRelease: + calibration_json: Path + corrected_urdf: Path + manifest: Path + pointer: Path + sha256: Mapping[str, str] + + +@dataclass(frozen=True) +class StagedRelease: + session: Path + calibration_json: Path + corrected_urdf: Path + calibration_json_sha256: str + corrected_urdf_sha256: str + # Canonical immutable evidence; no mutable callback object crosses the + # worker/control-thread boundary. + validation_json: str + calibration_report: Path | None = None + calibration_report_sha256: str | None = None + + +class ArtifactPublisher: + """Validate both staged products before changing the public pointer.""" + + def __init__(self, release_root: str | Path, pointer_name: str) -> None: + self.release_root = Path(release_root).expanduser().resolve() + if not pointer_name or Path(pointer_name).name != pointer_name: + raise ValueError("publication pointer must be one filename") + self.pointer_name = str(pointer_name) + + def prepare( + self, + *, + session_directory: str | Path, + calibration_json: str | Path, + corrected_urdf: str | Path, + validate: Callable[[Path, Path], Mapping[str, Any] | None], + cancelled: Callable[[], bool] = lambda: False, + ) -> StagedRelease: + session = Path(session_directory).expanduser().resolve() + if cancelled(): + raise ValueError("operator_abort:no_artifact_publication") + json_path = Path(calibration_json).expanduser().resolve() + urdf_path = Path(corrected_urdf).expanduser().resolve() + if session.parent != self.release_root: + raise ValueError("published session must be a direct child of the release root") + if json_path.parent != session or urdf_path.parent != session: + raise ValueError("artifacts must be staged inside the session directory") + if not json_path.is_file() or not urdf_path.is_file(): + raise ValueError("both JSON and URDF must exist before publication") + if json_path == urdf_path or "release_manifest.json" in {json_path.name, urdf_path.name}: + raise ValueError("artifact names conflict with the release manifest") + hashes = { + "calibration_json_sha256": _sha256(json_path), + "corrected_urdf_sha256": _sha256(urdf_path), + } + value = json.loads(json_path.read_text(encoding="utf-8")) + if not isinstance(value, dict): + raise ValueError("calibration JSON root must be an object") + report_path = session / REPORT_FILENAME if value.get("format") == COMPACT_FORMAT else None + report_hash = None + if report_path is not None: + if report_path in {json_path, urdf_path} or not report_path.is_file() or report_path.resolve().parent != session: + raise ValueError("compact release requires a separate calibration report") + report_hash = _sha256(report_path) + ET.parse(urdf_path) + validation = validate(json_path, urdf_path) + if cancelled(): + raise ValueError("operator_abort:no_artifact_publication") + if hashes["calibration_json_sha256"] != _sha256(json_path) or hashes["corrected_urdf_sha256"] != _sha256(urdf_path): + raise ValueError("artifacts changed during validation") + if report_path is not None and report_hash != _sha256(report_path): + raise ValueError("calibration report changed during validation") + return StagedRelease(session, json_path, urdf_path, + hashes["calibration_json_sha256"], hashes["corrected_urdf_sha256"], + json.dumps(dict(validation or {}), sort_keys=True, allow_nan=False), report_path, report_hash) + + def commit(self, staged: StagedRelease, *, cancelled: Callable[[], bool] = lambda: False) -> ArtifactRelease: + """Fast pointer commit; call under the runtime's abort/commit lock.""" + session, json_path, urdf_path = staged.session, staged.calibration_json, staged.corrected_urdf + if session.parent != self.release_root or json_path.parent != session or urdf_path.parent != session: + raise ValueError("staged artifacts belong to another release root") + if cancelled(): + raise ValueError("operator_abort:no_artifact_publication") + hashes = {"calibration_json_sha256": staged.calibration_json_sha256, + "corrected_urdf_sha256": staged.corrected_urdf_sha256} + if hashes["calibration_json_sha256"] != _sha256(json_path) or hashes["corrected_urdf_sha256"] != _sha256(urdf_path): + raise ValueError("artifacts changed after validation") + if staged.calibration_report is not None: + if (staged.calibration_report != session / REPORT_FILENAME + or staged.calibration_report_sha256 != _sha256(staged.calibration_report)): + raise ValueError("calibration report changed after validation") + hashes["calibration_report_sha256"] = staged.calibration_report_sha256 + manifest = session / "release_manifest.json" + manifest_payload = { + "schema_version": 1, + "session": session.name, + "calibration_json": json_path.name, + "corrected_urdf": urdf_path.name, + **hashes, + "validation": json.loads(staged.validation_json), + } + if staged.calibration_report is not None: + manifest_payload["calibration_report"] = staged.calibration_report.name + # Refuse to mutate an already materialized release. A retry using the + # same frozen files/evidence is idempotent; a new fit needs a new session. + if manifest.exists(): + if json.loads(manifest.read_text(encoding="utf-8")) != manifest_payload: + raise ValueError("refusing to overwrite a different release manifest") + else: + json.dumps(manifest_payload, allow_nan=False) + atomic_write_json(manifest, manifest_payload) + # Verify after durable re-read, then atomically replace only the + # pointer. A crash cannot expose one artifact without the other. + persisted = json.loads(manifest.read_text(encoding="utf-8")) + if ( + persisted.get("calibration_json_sha256") != _sha256(json_path) + or persisted.get("corrected_urdf_sha256") != _sha256(urdf_path) + ): + raise ValueError("artifact hashes changed after staging") + if staged.calibration_report is not None and persisted.get("calibration_report_sha256") != _sha256(staged.calibration_report): + raise ValueError("calibration report changed after staging") + self.release_root.mkdir(parents=True, exist_ok=True) + pointer = self.release_root / self.pointer_name + temporary = self.release_root / f".{self.pointer_name}.{uuid.uuid4().hex}.tmp" + relative = os.path.relpath(session, self.release_root) + try: + os.symlink(relative, temporary, target_is_directory=True) + if cancelled(): + raise ValueError("operator_abort:no_artifact_publication") + os.replace(temporary, pointer) + descriptor = os.open(self.release_root, os.O_RDONLY | os.O_DIRECTORY) + try: + os.fsync(descriptor) + finally: + os.close(descriptor) + finally: + temporary.unlink(missing_ok=True) + return ArtifactRelease(json_path, urdf_path, manifest, pointer, hashes) + + def publish(self, *, cancelled: Callable[[], bool] = lambda: False, **kwargs) -> ArtifactRelease: + return self.commit(self.prepare(cancelled=cancelled, **kwargs), cancelled=cancelled) + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +__all__ = ["ArtifactPublisher", "ArtifactRelease", "StandardArtifactValidator", "FrozenTagArtifactValidator", "StagedRelease"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py new file mode 100644 index 0000000..298e756 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/reader.py @@ -0,0 +1,89 @@ +"""Model-neutral reader for a certified JSON/URDF pair; no ROS dependency.""" + +import hashlib +import json +import math +from pathlib import Path + +from ...core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables +from ...core.urdf.kinematics import UrdfKinematicModel +from .serializers.compact_v2 import FORMAT as COMPACT_FORMAT, from_report + + +class UnifiedCommandMapper: + def __init__(self, payload, corrected_urdf, *, input_kind="command", expected_side=None, report=None): + compact = payload.get("format") == COMPACT_FORMAT + if payload.get("format") not in {"unified_calibration_v1", COMPACT_FORMAT}: + raise ValueError("requires a unified calibration artifact") + if expected_side and payload["side"] != expected_side: + raise ValueError("calibration side does not match requested side") + if compact and (report is None or from_report(report) != payload): + raise ValueError("compact JSON requires its matching calibration report") + metadata = report if compact else payload + self.mapping = SerializedJointMapping(metadata if compact and input_kind == "feedback" else payload, input_kind=input_kind) + self.urdf = UrdfKinematicModel(corrected_urdf) + self.active = tuple(name for name, j in self.urdf.joints.items() if j.kind != "fixed" and j.mimic_joint is None) + if compact: + validate_compact_urdf_tables(payload, self.urdf) + if not self.mapping.compact and set(self.mapping.joints) != set(self.active): + raise ValueError("JSON active joints differ from the exported URDF") + self.urdf_joint_names = tuple(sorted(name for name, j in self.urdf.joints.items() if j.kind != "fixed")) + self.command_names = tuple(metadata["command_names"]) + if any(row["motor_index"] >= len(self.command_names) for row in self.mapping.joints.values()): + raise ValueError("JSON SDK channel exceeds the recorded SDK layout") + self.input_domain = self.mapping.domain + self.model, self.side = payload["model"], payload["side"] + self.profile_id, self.serial_number = payload["profile_id"], payload["serial_number"] + self.feedback_by_index = bool(metadata.get("feedback_by_index", False)) if input_kind == "feedback" else True + self.feedback_name_aliases = dict(metadata.get("feedback_name_aliases", {})) + self.previous = None + self.directions = [""]*len(self.command_names) + + def map_positions(self, positions, names=()): + values = tuple(float(v) for v in positions) + if len(values) != len(self.command_names) or not all(math.isfinite(v) for v in values): + raise ValueError("SDK input requires a complete finite channel vector") + if names and not self.feedback_by_index and self.input_domain.startswith("feedback_"): + names = tuple(self.feedback_name_aliases.get(n, n) for n in names) + if len(names) != len(values) or set(names) != set(self.command_names): + raise ValueError("feedback names differ from the serialized SDK layout") + named = dict(zip(names, values)) + values = tuple(named[n] for n in self.command_names) + directions = self.directions.copy() + if self.previous is not None: + for i, (old, new) in enumerate(zip(self.previous, values)): + if abs(new-old) > (0.5 if self.input_domain.endswith("u8") else 1e-9): + directions[i] = "increasing" if new > old else "decreasing" + active = self.mapping.evaluate(values, "", active_joints=self.active, + directions_by_joint={name: directions[row["motor_index"]] for name, row in self.mapping.joints.items()}) + result = self.urdf.resolve_angles(active) + for name, angle in result.items(): + joint = self.urdf.joints[name] + if joint.lower is not None and not joint.lower-1e-9 <= angle <= joint.upper+1e-9: + raise ValueError(f"mapped value exceeds exported URDF range:{name}") + self.previous, self.directions = values, directions + return tuple(result[name] for name in self.urdf_joint_names) + + +def load_unified_mapper(calibration_file, *, expected_side=None, input_kind="command"): + path = Path(calibration_file).expanduser().resolve() + manifest_path = path if path.name == "release_manifest.json" else path.parent/"release_manifest.json" + manifest = json.loads(manifest_path.read_text()) + def artifact(field): + name = manifest[field] + if Path(name).name != name: + raise ValueError("release artifact must be a sibling filename") + result = manifest_path.parent/name + if hashlib.sha256(result.read_bytes()).hexdigest() != manifest[field+"_sha256"]: + raise ValueError(f"released artifact SHA256 changed:{field}") + return result + json_path, urdf_path = artifact("calibration_json"), artifact("corrected_urdf") + if path not in {manifest_path, json_path.resolve()}: + raise ValueError("JSON is not the artifact certified by this manifest") + validation = manifest["validation"] + if validation.get("final_file_spatial_replay") != "passed" or not validation.get("steady_command_tag_holdout"): + raise ValueError("release lacks independent command and feedback URDF evidence") + payload = json.loads(json_path.read_text()) + report = json.loads(artifact("calibration_report").read_text()) if payload.get("format") == COMPACT_FORMAT else None + return UnifiedCommandMapper(payload, urdf_path, + input_kind=input_kind, expected_side=expected_side, report=report) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py new file mode 100644 index 0000000..c039cd2 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/replay.py @@ -0,0 +1,83 @@ +"""Offline use of exactly the online finalizer, for any declared Profile.""" + +from pathlib import Path +import hashlib +import json + +from ..acquisition import load_capture +from ..engine import CalibrationEngine +from ..resume import ResumeVerifier, fingerprint_from_mapping +from ..scan_quality import evaluate_capture_unit +from .finalization import finalize_profile_session + + +def validate_capture_provenance(profile, serial_number, hashes, records): + """An offline journal cannot substitute today's hashes for old evidence.""" + headers = [row for row in records if row.get("kind") == "session_start"] + references = [row for row in records if row.get("kind") == "fixed_base_reference_locked"] + if len(headers) != 1 or len(references) != 1: + raise ValueError("capture_provenance:requires_unique_session_and_locked_reference") + header, reference = headers[0], references[0] + engine = CalibrationEngine(profile) + if (not engine.resume_compatible(header) or header.get("serial_number") != serial_number + or header.get("curve_input_domain") != profile.curve_input_domain): + raise ValueError("capture_provenance:obsolete_policy_or_coordinate_identity; fresh capture required") + for key, value in hashes.items(): + if header.get(key) != value or reference.get("protected_hashes", {}).get(key) != value: + raise ValueError(f"capture_provenance:protected_input_changed:{key}") + fingerprint = fingerprint_from_mapping(reference) + checker = ResumeVerifier(required_fixed_views=profile.vision.view_names, + required_fixed_poses=profile.vision.view_names, required_hashes=(*hashes, "intrinsics_sha256")) + evidence = checker.compare(fingerprint, fingerprint) + if not evidence.reuse or fingerprint.profile_id != profile.key.profile_id: + raise ValueError(f"capture_provenance:invalid_locked_reference:{evidence.incompatible_fields}") + models = {} + # Only preview models preceding the formal reference are relevant. Later + # camera changes are rejected online and cannot authorize a mixed replay. + for row in records: + if row is reference: + break + if row.get("kind") == "rectified_camera_model": + if row.get("matrix_source") != "CameraInfo.P[:3,:3]" or row.get("input_is_rectified") is not True: + raise ValueError("capture_provenance:unverified_rectified_projection") + models[row["view"]] = row["camera_matrix"] + intrinsic_hash = hashlib.sha256(json.dumps(dict(sorted(models.items())), sort_keys=True).encode()).hexdigest() + if set(models) != set(profile.vision.view_names) or intrinsic_hash != fingerprint.protected_hashes["intrinsics_sha256"]: + raise ValueError("capture_provenance:intrinsics_evidence_changed_or_missing") + + +def validate_capture_units(profile, records): + """Offline and resumed data must satisfy the actual post-sweep policy.""" + first_spans = {} + for unit in CalibrationEngine(profile).scan_units(): + key = (unit.task_key, unit.cycle, unit.direction) + rows = [row for row in records + if (row.get("task_name"), row.get("cycle"), row.get("direction")) == key] + attempt = max((int(row.get("attempt", 1)) for row in rows), default=1) + complete = any(row.get("kind") == "scan_unit_complete" and row.get("passed") is True + and int(row.get("attempt", 1)) == attempt for row in rows) + quality = evaluate_capture_unit(profile, unit, attempt, rows, first_cycle_spans=first_spans) + if not complete or not quality.passed: + raise ValueError(f"capture_incomplete:{key}:{quality.failures}") + + +def replay_capture(config, raw_path: Path, *, output: Path | None, publish: bool): + from ..runner_support import create_session_directory, protected_inputs + records = load_capture(raw_path) + profile = config.calibration_contract.typed_profile + hashes = protected_inputs(config) + validate_capture_provenance(profile, config.serial_number, hashes, records) + validate_capture_units(profile, records) + reference = next(row for row in records if row.get("kind") == "fixed_base_reference_locked") + hashes["intrinsics_sha256"] = reference["protected_hashes"]["intrinsics_sha256"] + hashes["raw_samples_sha256"] = hashlib.sha256(Path(raw_path).read_bytes()).hexdigest() + if output is None: + output = create_session_directory(config.session_root) + else: + output = output.expanduser().resolve() + output.mkdir(parents=True, exist_ok=False) + payload, _fit, correction = finalize_profile_session( + profile=profile, + session_dir=output, serial_number=config.serial_number, source_urdf=config.source_urdf, + protected_inputs=hashes, records=records, publish=publish) + print(f"离线回放完成：schema {payload['schema_version']}，URDF {correction.path}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/__init__.py new file mode 100644 index 0000000..871b95b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/__init__.py @@ -0,0 +1 @@ +"""Format compatibility only. Mathematical choices precede serialization.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py new file mode 100644 index 0000000..750610b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/compact_v2.py @@ -0,0 +1,61 @@ +"""Small command lookup; the complete training result stays in the report. + +No extrapolation, new fit, zero shift or independent passive curve is allowed. +The exported single curve must subsequently pass final-file visual holdout. +""" + +import numpy as np + +from ....core.urdf.acceptance import SerializedJointMapping +from .unified_v1 import serialize as serialize_report + +FORMAT = "unified_calibration_v2" +REPORT_FILENAME = "calibration_report.json" + + +def from_report(report): + mapping = SerializedJointMapping(report, input_kind="command") + unit = report["command_unit"] + rows, visiting = {}, set() + + def resolve(name): + if name in rows: + return rows[name] + if name in visiting or name not in report["joints"]: + raise ValueError("invalid report mimic graph") + visiting.add(name) + source = report["joints"][name] + if source.get("passive"): + mimic = source["mimic"] + parent = resolve(mimic["joint"]) + row = {**parent, "angle_rad": [mimic["multiplier"]*v + mimic["offset_rad"] + for v in parent["angle_rad"]]} + else: + data = mapping.joints[name] + knots = np.asarray(data["curve_input_knots_rad"], dtype=float) + # Averaging is a declared training-only reduction. Never choose a + # better branch using holdout, or retain hidden direction fallback. + angles = (np.asarray(data["increasing_rad"]) + np.asarray(data["decreasing_rad"]))/2 + row = {"sdk_channel": data["motor_index"]} + if unit == "u8": + if knots[0] != 0 or knots[-1] != 255: + raise ValueError(f"compact_byte_requires_measured_full_command_domain:{name}") + angles = np.interp(np.arange(256), knots, angles) + else: + row["input_values"] = knots.tolist() + row["angle_rad"] = angles.tolist() + if not np.all(np.isfinite(row["angle_rad"])): + raise ValueError(f"nonfinite compact curve:{name}") + visiting.remove(name) + rows[name] = row + return row + + for name in sorted(report["joints"]): + resolve(name) + return {"format": FORMAT, "schema_version": 2, + **{key: report[key] for key in ("profile_id", "model", "side", "serial_number")}, + "input_unit": unit, "joints": {name: rows[name] for name in sorted(rows)}} + + +def serialize(profile, fit, prepared, **metadata): + return from_report(serialize_report(profile, fit, prepared, **metadata)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py new file mode 100644 index 0000000..73add70 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/serializers/unified_v1.py @@ -0,0 +1,47 @@ +"""Lossless dual-input format. No fitting, rebasing or passive curve bypass.""" + +FORMAT = "unified_calibration_v1" + + +def _mapping(mapping): + return {"input_domain": mapping.input_domain, "channel_index": mapping.motor_index, + "input_unit": mapping.input_domain.rsplit("_", 1)[1], "output_unit": "rad", + "knots": list(mapping.knots), "valid_input_range": [mapping.knots[0], mapping.knots[-1]], + "interpolation": "piecewise_linear", "extrapolation": "reject", + **{field: list(getattr(mapping, field)) for field in ("angle_rad", "increasing_rad", "decreasing_rad")}} + + +def serialize(profile, fit, prepared, *, serial_number, protected_inputs): + if set(fit.command_mappings) != set(fit.output_mappings): + raise ValueError("unified artifacts require independently measured command AND feedback mappings") + if any(not m.input_domain.startswith("feedback_") for m in fit.output_mappings.values()): + raise ValueError("unified geometry must use feedback, not a relabelled command curve") + rows = {} + for name in sorted(prepared.mappings): + row = {"urdf_joint": name, "calibration_status": profile.joint_coverage[name], + "zero_offset_rad": fit.zero_offsets_rad.get(name, 0.0), + "zero_method": fit.zero_method_by_joint.get(name, "source_cad_passive_zero")} + if name in prepared.plan.mimic_output: + parent, multiplier, offset = prepared.plan.mimic_output[name] + row.update(passive=True, runtime_source="exported_standard_urdf_mimic", + mimic={"joint": parent, "multiplier": multiplier, "offset_rad": offset}) + else: + row.update(passive=False, command_to_rad=_mapping(fit.command_mappings[name]), + feedback_to_rad=_mapping(fit.output_mappings[name]), + applicability=fit.command_applicability[name], + command_holdout=fit.command_holdout_metrics[name]) + donor = profile.zero.transferred_zero_sources.get(name) + if donor: + row.update(transferred_from_joint=donor, independently_measured=False) + rows[name] = row + return {"format": FORMAT, "schema_version": 1, "profile_id": profile.key.profile_id, + "serial_number": serial_number, "model": profile.key.model, "side": profile.key.side, + "angle_unit": "rad", "command_unit": profile.command.unit, + "command_names": list(profile.command.names), "baseline_command": list(profile.command.baseline_values), + "feedback_by_index": profile.command.feedback_by_index, + "feedback_name_aliases": dict(profile.command.feedback_name_aliases), + "protected_inputs": dict(protected_inputs), "joints": rows, + "coordinate_convention": "q_CAD=q_output+delta; JSON already emits q_output", + "quality": {"training_cycles": [0, 1, 2], "holdout_cycle": 3, + "command_and_feedback_fits_passed": True, + "final_file_acceptance": "release_manifest.json", "arbitrary_multiaxis_validated": False}} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/standard_loader.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/standard_loader.py new file mode 100644 index 0000000..72f3b44 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/standard_loader.py @@ -0,0 +1,62 @@ +"""Bounded, isolated standard ROS URDF loader check. Never starts an SDK.""" + +from __future__ import annotations + +import os +from pathlib import Path +import selectors +import subprocess +import tempfile +import time +import uuid + +import yaml + + +def validate_with_robot_state_publisher(path: Path, *, timeout_seconds: float = 8.0) -> None: + """Require urdfdom/KDL/RSP to load the generated file successfully. + + All subscriptions and TF outputs use a fresh private namespace. The + process has no motor-command publisher or SDK connection. It is terminated + after initialization, not left running as another hand model publisher. + """ + from ament_index_python.packages import get_package_prefix + executable = Path(get_package_prefix("robot_state_publisher")) / "lib/robot_state_publisher/robot_state_publisher" + if not executable.is_file(): + raise ValueError("standard_urdf_loader_unavailable:robot_state_publisher") + namespace = "/calibration_verify_" + uuid.uuid4().hex + with tempfile.TemporaryDirectory(prefix="calibration_urdf_verify_") as directory: + parameters = Path(directory) / "parameters.yaml" + parameters.write_text(yaml.safe_dump({"/**": {"ros__parameters": { + "robot_description": path.read_text(encoding="utf-8"), + "publish_frequency": 1.0}}}), encoding="utf-8") + environment = dict(os.environ, ROS_LOG_DIR=directory, ROS_LOCALHOST_ONLY="1") + process = subprocess.Popen([str(executable), "--ros-args", "--params-file", str(parameters), + "-r", f"__ns:={namespace}", "-r", f"/tf:={namespace}/tf", + "-r", f"/tf_static:={namespace}/tf_static", "-r", f"joint_states:={namespace}/joint_states", + "--log-level", "info", "--disable-rosout-logs"], + stdout=subprocess.PIPE, stderr=subprocess.STDOUT, env=environment) + output = bytearray() + try: + assert process.stdout is not None + with selectors.DefaultSelector() as selector: + selector.register(process.stdout, selectors.EVENT_READ) + deadline = time.monotonic() + timeout_seconds + while time.monotonic() < deadline: + for key, _ in selector.select(timeout=0.1): + output.extend(os.read(key.fileobj.fileno(), 65536)) + if b"Robot initialized" in output and process.poll() is None: + return + if process.poll() is not None: + break + raise ValueError("standard_urdf_loader_failed:" + output.decode(errors="replace")[-4000:]) + finally: + if process.poll() is None: + process.terminate() + try: + process.wait(timeout=2) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=2) + if process.stdout is not None: + process.stdout.close() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/worker.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/worker.py new file mode 100644 index 0000000..08a2b60 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/worker.py @@ -0,0 +1,49 @@ +"""CPU-bound finalization without blocking the SDK or status callbacks. + +The worker only prepares artifacts. The controller commits them, under the +same lock as abort/pause, after observing successful completion. +""" + +from concurrent.futures import ThreadPoolExecutor +from threading import Event, RLock + + +class FinalizationWorker: + def __init__(self): + self.lock = RLock() + self._cancelled = Event() + self._executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix="calibration-fit") + self._future = None + self._consumed = False + + def start(self, finalizer, **kwargs): + with self.lock: + if self._future is not None or self._cancelled.is_set(): + raise RuntimeError("finalization_already_started_or_cancelled") + self._future = self._executor.submit( + finalizer, **kwargs, publish=False, cancelled=self._cancelled.is_set) + + def finish_if_ready(self, commit): + """Invoke commit at most once; an abort cannot race the pointer swap. + + The controller holds ``lock`` while also updating its terminal state. + A failed job is consumed too, so a failed publication is never retried. + """ + with self.lock: + if (self._cancelled.is_set() or self._consumed or + self._future is None or not self._future.done()): + return None + self._consumed = True + result = self._future.result() + commit(result) + return result + + def cancel(self): + with self.lock: + self._cancelled.set() + if self._future is not None: + self._future.cancel() + + def close(self): + self.cancel() + self._executor.shutdown(wait=False, cancel_futures=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/cameras.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/cameras.py new file mode 100644 index 0000000..e6832e1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/cameras.py @@ -0,0 +1,71 @@ +"""Protected rectified camera identity and live pipeline readiness.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Callable + +import numpy as np + +from ..core.geometry.extrinsics import CameraExtrinsics, camera_info_fingerprint +from ..storage import append_jsonl +from .inputs import CameraModelInput + + +class CameraObservations: + def __init__(self, extrinsics: CameraExtrinsics, raw_path: Path, monotonic: Callable[[], float]): + self.extrinsics = extrinsics + self.raw_path = raw_path + self.monotonic = monotonic + self.matrices: dict[str, np.ndarray] = {} + self.image_sizes: dict[str, tuple[int, int]] = {} + self.info_received_at: dict[str, float] = {} + self.detections_received_at: dict[str, float] = {} + self.errors: dict[str, str] = {} + self.models: dict[str, dict] = {} + + def accept(self, view: str, message: CameraModelInput, *, started: bool, pause, trackers) -> None: + 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') + identity = self.extrinsics.cameras[view] + fingerprint = camera_info_fingerprint( + width=message.width, height=message.height, camera_matrix=message.k, + distortion=message.d, rectification=message.r, projection=message.p) + if (message.width != identity.width or message.height != identity.height + or fingerprint != identity.intrinsics_sha256): + raise ValueError('camera_info_does_not_match_extrinsics') + except ValueError as error: + self.errors[view] = str(error) + self.info_received_at.pop(view, None) + if started: + pause(f"protected_camera_model_changed:{view}:{error}") + self.matrices.pop(view, None) + self.image_sizes.pop(view, None) + return + previous = self.matrices.get(view) + dimensions = (int(message.width), int(message.height)) + if started and previous is not None and ( + not np.array_equal(previous, matrix) or self.image_sizes.get(view) != dimensions + ): + pause(f"protected_camera_model_changed:{view}:intrinsics_or_dimensions") + return + self.matrices[view] = matrix + self.image_sizes[view] = (int(message.width), int(message.height)) + self.info_received_at[view] = self.monotonic() + self.errors.pop(view, None) + 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 self.models.get(view) != model: + self.models[view] = model + append_jsonl(self.raw_path, model) + if previous is not None and not np.allclose(previous, matrix): + trackers[view].reset() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py new file mode 100644 index 0000000..f980ee2 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/capture.py @@ -0,0 +1,175 @@ +"""Profile observation capture shared by every SDK and ROS camera host.""" + +from collections import deque +from dataclasses import dataclass + +import numpy as np +from scipy.spatial.transform import Rotation + +from ..core.geometry.extrinsics import matrix_payload, transform_matrix +from ..core.geometry.pnp import SquareTagPose, SquareTagPoseTracker +from .scan_quality import observation_streams + + +@dataclass(frozen=True) +class CaptureFrame: + view: str + stamp_ns: int + camera_matrix: np.ndarray + corners: dict + feedback: tuple[float, ...] | None + command: tuple[float, ...] | None + skew_ns: int = 0 + command_directions: tuple[str, ...] = () + + +class ObservationCapture: + def __init__(self, profile, *, reference_lock, extrinsics, tag_size_m=0.016, trackers=None): + self.profile, self.reference_lock, self.extrinsics = profile, reference_lock, extrinsics + self.tag_size_m = float(tag_size_m) + self.trackers = trackers or {view.name: SquareTagPoseTracker() + for view in profile.vision.views} + self._seen = set() + self._poses = {} + self.locked_poses = {} + self.installations = {} + self.last_missing = {} + self.last_filtered = {} + + def reset(self): + self._seen.clear() + self._poses.clear() + self.locked_poses.clear() + self.installations.clear() + self.last_missing.clear() + self.last_filtered.clear() + for tracker in self.trackers.values(): + tracker.reset() + + @staticmethod + def _median_pose(poses): + return SquareTagPose( + quaternion_xyzw=tuple(Rotation.from_quat([p.quaternion_xyzw for p in poses]).mean().as_quat()), + translation_xyz_m=tuple(np.median([p.translation_xyz_m for p in poses], axis=0)), + reprojection_error_px=float(np.median([p.reprojection_error_px for p in poses]))) + + def consume(self, frame: CaptureFrame, motion=None, *, collect_installations=False): + identity = (frame.view, frame.stamp_ns) + if frame.stamp_ns <= 0 or identity in self._seen: + return (), None + self._seen.add(identity) + view = next(v for v in self.profile.vision.views if v.name == frame.view) + base = next(tag for tag in view.tags if tag.fixed_reference) + selected, filtered = {}, [] + for tag in view.tags: + points = frame.corners.get(tag.role) + if points is None: + continue + pose, reason = self.trackers[frame.view].estimate(tag.role, points, + tag_size_m=tag.size_m, camera_matrix=frame.camera_matrix, stamp_ns=frame.stamp_ns) + if pose is None: + filtered.append(tag.tag_id) + else: + selected[tag.role] = pose + self.last_missing[frame.view] = tuple(tag.tag_id for tag in view.tags if tag.role not in frame.corners) + self.last_filtered[frame.view] = tuple(filtered) + movement = None + if frame.view in self.reference_lock.references and base.role in frame.corners: + # Once locked, fixed-object drift is a CORNER measurement. A large + # real displacement may fail the temporal PnP jump filter and must + # not consequently evade the ten-frame movement protection. + movement = self.reference_lock.observe(frame.view, base.tag_id, frame.corners[base.role]) + if base.role in selected: + # Only independent frames with a valid pose may establish a lock. + if frame.view not in self.reference_lock.references: + movement = self.reference_lock.observe(frame.view, base.tag_id, frame.corners[base.role]) + if self.reference_lock.locking_enabled and frame.view not in self.locked_poses: + buffer = self._poses.setdefault(frame.view, deque(maxlen=30)) + buffer.append(selected[base.role]) + if frame.view in self.reference_lock.references and len(buffer) >= self.reference_lock.minimum_frames: + self.locked_poses[frame.view] = self._median_pose(buffer) + # Include the last locking frame exactly once; never update mounting + # fingerprints during task motion or while the worker is fitting. + if collect_installations and self.reference_lock.locking_enabled: + self._observe_installations(frame.view, base.role, selected, + prefix="" if collect_installations is True else str(collect_installations)+"/") + if movement is not None or motion is None or not motion.recording: + return (), movement + task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key) + streams = [item for item in observation_streams(self.profile, task) if item[2].view == frame.view] + if not streams or frame.feedback is None or frame.command is None: + return (), movement + locked = [] + if base.role not in selected and frame.view in self.locked_poses: + selected[base.role] = self.locked_poses[frame.view] + locked.append(base.role) + rows = [] + common_from_view = self.extrinsics.transform(frame.view) + unit = self.profile.command.unit + for field, joint, spec in streams: + if spec.parent_role not in selected or spec.child_role not in selected: + continue + parent, child = selected[spec.parent_role], selected[spec.child_role] + p = common_from_view @ transform_matrix(parent.translation_xyz_m, parent.quaternion_xyzw) + c = common_from_view @ transform_matrix(child.translation_xyz_m, child.quaternion_xyzw) + relative = np.linalg.inv(p) @ c + row = {"kind": "joint_sample" if field == "joint" else "secondary_joint_sample", + "profile_id": self.profile.key.profile_id, field: joint, "task_name": task.key, + "view": frame.view, "image_stamp_ns": frame.stamp_ns, + "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + "sample_phase": "steady" if motion.phase == "steady" else "sweep", + "command_direction_by_index": list(frame.command_directions), + "motor_index": task.command_index, + f"feedback_{unit}": frame.feedback[task.command_index], + f"command_{unit}": frame.command[task.command_index], + f"state_{unit}": list(frame.feedback), f"command_vector_{unit}": list(frame.command), + "state_image_sync_error_ms": abs(frame.skew_ns)/1e6, + "relative_quaternion_xyzw": Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(), + "relative_translation_xyz_m": relative[:3, 3].tolist(), + "parent_pose_common": matrix_payload(p), "child_pose_common": matrix_payload(c), + "view_normal_common_xyz": common_from_view[:3, 2].tolist(), + "camera_center_common_xyz_m": common_from_view[:3, 3].tolist(), + "locked_reference_roles": locked, + "pnp_reprojection_error_px": max(parent.reprojection_error_px, child.reprojection_error_px)} + if motion.phase == "steady": + row.update(steady_index=motion.steady_index, + steady_target=motion.target[task.command_index]) + if spec.parent_role in frame.corners and spec.child_role in frame.corners: + row["image_relative_xy_px"] = (np.mean(frame.corners[spec.child_role], axis=0) + - np.mean(frame.corners[spec.parent_role], axis=0)).tolist() + elif joint in self.profile.measurement.image_curve_joints: + continue # Image-circle observations cannot be synthesized from a cached pose. + rows.append(row) + roles = {role for _, _, spec in streams for role in (spec.parent_role, spec.child_role)} + if task.key in self.profile.measurement.candidate_selection_tasks: + rows.append({"kind": "pnp_candidate_frame", "task_name": task.key, + "cycle": motion.cycle, "direction": motion.direction, "attempt": motion.attempt, + "view": frame.view, "image_stamp_ns": frame.stamp_ns, "tag_size_m": self.tag_size_m, + "camera_matrix": frame.camera_matrix.tolist(), "camera_matrix_source": "CameraInfo.P[:3,:3]", + "input_is_rectified": True, + "roles": {role: {"corners_xy": np.asarray(frame.corners[role]).tolist(), + "tag_size_m": next(tag.size_m for tag in view.tags if tag.role == role), + "maximum_reprojection_error_px": self.trackers[frame.view].maximum_reprojection_error_px} + for role in roles if role in frame.corners}}) + return tuple(rows), movement + + def _observe_installations(self, view, base_role, poses, *, prefix=""): + if base_role not in poses: + return + parent = poses[base_role] + p = transform_matrix(parent.translation_xyz_m, parent.quaternion_xyzw) + for role, pose in poses.items(): + if role == base_role: + continue + relative = np.linalg.inv(p) @ transform_matrix(pose.translation_xyz_m, pose.quaternion_xyzw) + buffer = self.installations.setdefault(f"{prefix}{view}:{role}", deque(maxlen=30)) + buffer.append(SquareTagPose( + quaternion_xyzw=tuple(Rotation.from_matrix(relative[:3, :3]).as_quat()), + translation_xyz_m=tuple(relative[:3, 3]), reprojection_error_px=pose.reprojection_error_px)) + + def fingerprint_poses(self): + def payload(pose): + return {"rotation_xyzw": list(pose.quaternion_xyzw), "translation_xyz_m": list(pose.translation_xyz_m)} + return ({view: payload(pose) for view, pose in self.locked_poses.items()}, + {name: payload(self._median_pose(rows)) for name, rows in self.installations.items() + if len(rows) >= self.reference_lock.minimum_frames}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py deleted file mode 100644 index 7c7ddba..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py +++ /dev/null @@ -1,160 +0,0 @@ -"""ROS-free session state machine shared by live and replay runners.""" - -from __future__ import annotations - -from dataclasses import dataclass -from enum import Enum -from typing import Sequence - -from ..core import CalibrationProfile, SampleRecord, validate_profile -from ..core.solver import ( - SessionSolution, - SessionSolver, - TaskEvaluation, - TaskEvaluator, -) - - -class SessionState(str, Enum): - CREATED = "CREATED" - PREFLIGHT = "PREFLIGHT" - PREPARING = "PREPARING" - CAPTURING = "CAPTURING" - EVALUATING = "EVALUATING" - RESCAN = "RESCAN" - SOLVING = "SOLVING" - VALIDATING = "VALIDATING" - PUBLISHING = "PUBLISHING" - COMPLETE = "COMPLETE" - FAILED = "FAILED" - - -@dataclass(frozen=True) -class ControllerSnapshot: - state: SessionState - completed_task_keys: tuple[str, ...] - current_task_key: str | None - failure_reason: str | None - - -class SessionController: - """Advance one typed task graph without depending on ROS clocks/messages.""" - - def __init__( - self, - profile: CalibrationProfile, - evaluator: TaskEvaluator, - solver: SessionSolver, - ) -> None: - validate_profile(profile) - self.profile = profile - self.evaluator = evaluator - self.solver = solver - self.state = SessionState.CREATED - self._task_index = 0 - self._samples: list[SampleRecord] = [] - self.last_evaluation: TaskEvaluation | None = None - self.solution: SessionSolution | None = None - self.failure_reason: str | None = None - - @property - def current_task(self): - if self._task_index >= len(self.profile.motion.tasks): - return None - return self.profile.motion.tasks[self._task_index] - - @property - def samples(self) -> tuple[SampleRecord, ...]: - return tuple(self._samples) - - def snapshot(self) -> ControllerSnapshot: - completed = tuple( - task.key for task in self.profile.motion.tasks[: self._task_index] - ) - task = self.current_task - return ControllerSnapshot( - state=self.state, - completed_task_keys=completed, - current_task_key=None if task is None else task.key, - failure_reason=self.failure_reason, - ) - - def start(self) -> None: - self._require(SessionState.CREATED) - self.state = SessionState.PREFLIGHT - - def finish_preflight(self, *, passed: bool, reason: str = "") -> None: - self._require(SessionState.PREFLIGHT) - if not passed: - self._fail(reason or "static preflight failed") - return - self.state = SessionState.PREPARING - - def task_pose_ready(self) -> None: - if self.state not in {SessionState.PREPARING, SessionState.RESCAN}: - self._raise_transition("task pose can only follow prepare or rescan") - if self.current_task is None: - self._raise_transition("there is no task left to capture") - self.state = SessionState.CAPTURING - - def submit_task_samples( - self, samples: Sequence[SampleRecord] - ) -> TaskEvaluation: - self._require(SessionState.CAPTURING) - task = self.current_task - if task is None: - self._raise_transition("there is no active task") - rows = tuple(samples) - if not rows or any(row.task_key != task.key for row in rows): - raise ValueError("captured samples do not belong to the active task") - self.state = SessionState.EVALUATING - evaluation = self.evaluator.evaluate_task(self.profile, task, rows) - self.last_evaluation = evaluation - if not evaluation.accepted: - if evaluation.rescan_measurements or evaluation.rescan_cycles: - self.state = SessionState.RESCAN - else: - self._fail("task evaluation failed without a safe rescan scope") - return evaluation - self._samples.extend(rows) - self._task_index += 1 - self.state = ( - SessionState.SOLVING - if self.current_task is None - else SessionState.PREPARING - ) - return evaluation - - def solve(self) -> SessionSolution: - self._require(SessionState.SOLVING) - solution = self.solver.solve_session(self.profile, self.samples) - self.solution = solution - if solution.passed: - self.state = SessionState.VALIDATING - else: - self._fail("session solver rejected the captured samples") - return solution - - def finish_release_validation( - self, *, passed: bool, reason: str = "" - ) -> None: - self._require(SessionState.VALIDATING) - if not passed: - self._fail(reason or "release validation failed") - return - self.state = SessionState.PUBLISHING - - def finish_publication(self) -> None: - self._require(SessionState.PUBLISHING) - self.state = SessionState.COMPLETE - - def _fail(self, reason: str) -> None: - self.failure_reason = str(reason) - self.state = SessionState.FAILED - - def _require(self, expected: SessionState) -> None: - if self.state != expected: - self._raise_transition(f"expected {expected.value}") - - def _raise_transition(self, detail: str) -> None: - raise RuntimeError(f"invalid session transition from {self.state.value}: {detail}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py new file mode 100644 index 0000000..3152f4b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -0,0 +1,645 @@ +"""Transport-neutral owner of the live calibration session. + +State transitions, capture submission and artifact commit share one RLock. +The lock order is always coordinator state, then finalization worker; fitting +and pose estimation never hold the state lock while doing expensive work. +""" + +from __future__ import annotations + +from collections import deque +import hashlib +import json +import threading +import traceback + +import numpy as np + +from ..acquisition import StateSample, interpolate_state_u8 +from ..core.domain.profile import CalibrationProfile +from ..storage import append_jsonl, atomic_write_json +from .engine import ACQUISITION_POLICY_VERSION +from .reference_lock import ReferenceLock +from .acquisition import load_capture +from .artifacts.controller import FinalizationController, FinalizationInputs, FinalizationProtocolError +from .cameras import CameraObservations +from .capture import CaptureFrame, ObservationCapture +from .execution import SessionExecution +from .inputs import AdapterFactory, CameraModelInput, DetectionInput, RuntimePorts, ServiceResult +from .motion_execution import MotionExecution +from .parameters import RuntimeParameters +from .resume import ResumeDecision, ResumeVerifier, fingerprint_from_mapping +from .safety import SafetyPolicy, SafetySample +from .session import CalibrationPhase as Phase +from .snapshot import DeviceReadiness, CameraReadiness, RuntimeSnapshot, build_snapshot, legacy_state + + +class CalibrationCoordinator: + def __init__(self, profile: CalibrationProfile, parameters: RuntimeParameters, + ports: RuntimePorts, adapter_factory: AdapterFactory, *, finalization=None): + self.profile, self.parameters, self.ports = profile, parameters, ports + self.step_data_lock = threading.RLock() + self.model_name = profile.key.model + self.command_names = tuple(profile.command.names) + self.command_count = profile.command.command_count + self.command_unit = profile.command.unit + self.feedback_lower = tuple(profile.command.minimum_feedback_values) + self.feedback_upper = tuple(profile.command.maximum_feedback_values) + self.execution = SessionExecution(profile) + self.safety = SafetyPolicy(profile) + self.reference_lock = self._new_reference_lock() + self.trackers = parameters.new_trackers(profile.vision.view_names) + self.capture = ObservationCapture(profile, reference_lock=self.reference_lock, + extrinsics=parameters.extrinsics, tag_size_m=parameters.tag_size_m, trackers=self.trackers) + self.latest_feedback: tuple[float, ...] = () + self.state_history: deque[StateSample] = deque(maxlen=2000) + self.state_receive_times: deque[float] = deque(maxlen=300) + self.command_history: deque[StateSample] = deque(maxlen=2000) + self.command_direction_history = deque(maxlen=2000) + self._command_directions = [""]*self.command_count + self.raw_records = [] + self.last_command = None + self.commanded_speed = None + self.step_speed_ready_at = 0.0 + self.reason = "waiting_for_feedback_and_camera_info" + self._started_at: float | None = None + self._observation_epoch = 0 + self._pause_effects_applied = False + self.segment = None + self._segment_number = 0 + self._motion = None + self._steady_capture_after_ns = None + self._resume_message = "not_requested" + self._resumed_count = 0 + self._unit_rows = [] + self._capture_unit_key = None + self._steady_rows = [] + self._reference_capture_key = None + self._reference_observation_started = None + self._current_fingerprint = {} + self.final_json = "" + self.final_urdf = "" + for path, key in ((parameters.source_urdf, "source_urdf_sha256"), + (parameters.camera_extrinsics_file, "camera_extrinsics_sha256")): + if hashlib.sha256(path.read_bytes()).hexdigest() != parameters.protected_inputs[key]: + raise ValueError(f"protected input changed:{key}") + from ..profiles.validator import validate_executable_profile + validate_executable_profile(profile, parameters.source_urdf) + parameters.session_dir.mkdir(parents=True, exist_ok=True) + self.raw_path = parameters.session_dir / "raw_samples.jsonl" + append_jsonl(self.raw_path, { + "kind": "session_start", "sample_schema_version": profile.artifacts.output_schema_version, + "profile_id": profile.key.profile_id, "serial_number": parameters.serial_number, + "curve_input_domain": profile.curve_input_domain, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + **parameters.protected_inputs, + "resume_checkpoint_requested": parameters.resume_raw_samples_path is not None, + }) + self.cameras = CameraObservations(parameters.extrinsics, self.raw_path, ports.monotonic) + self.sdk_adapter = adapter_factory(self._write_position, self._write_speed, self.feedback_fresh) + self.finalization = finalization or FinalizationController(profile, parameters.session_dir) + + @property + def state(self) -> str: + return legacy_state(self.execution.session.phase) + + @property + def started(self) -> bool: + return self._started_at is not None + + def start(self) -> ServiceResult: + with self.step_data_lock: + if self.execution.session.phase != Phase.READY or not self.parameters.commands_enabled: + return ServiceResult(False, self.reason) + devices = self._device_status(self.ports.monotonic()) + if not devices.ready: + self.execution.session.devices_unavailable() + self.reason = "; ".join(devices.waiting_for) + return ServiceResult(False, self.reason) + # Swap complete capture objects: an in-flight preview PnP callback + # may finish later, but cannot populate the new formal reference. + self.reference_lock = self._new_reference_lock() + self.reference_lock.begin_session() + self.trackers = self.parameters.new_trackers(self.profile.vision.view_names) + self.capture = ObservationCapture(self.profile, reference_lock=self.reference_lock, + extrinsics=self.parameters.extrinsics, tag_size_m=self.parameters.tag_size_m, trackers=self.trackers) + self.state_history.clear() + self.command_history.clear() + self.command_direction_history.clear() + self._command_directions = [""]*self.command_count + self.raw_records.clear() + self.execution.session.start(resume_requested=self.parameters.resume_raw_samples_path is not None) + self._started_at = self.ports.monotonic() + self._observation_epoch += 1 + self.reason = "moving_to_safe_baseline" + self._publish_torque() + return ServiceResult(True, "标定已开始；安全基准后锁定参考") + + def abort(self) -> ServiceResult: + with self.step_data_lock: + if self.state == "PASSED": + return ServiceResult(False, "calibration_already_published") + self._pause("operator_abort_holding_current_position") + self.execution.session.abort() + self.reason = "operator_abort_holding_current_position" + return ServiceResult(True, self.reason) + + def receive_feedback(self, names, positions, stamp_ns: int) -> None: + with self.step_data_lock: + self._accept_feedback(names, positions, stamp_ns) + + def receive_camera_info(self, view: str, message: CameraModelInput) -> None: + with self.step_data_lock: + self.cameras.accept(view, message, started=self.started, pause=self._pause, trackers=self.trackers) + + def receive_detections(self, observation: DetectionInput) -> None: + view, stamp = observation.view, observation.stamp_ns + with self.step_data_lock: + self.cameras.detections_received_at[view] = self.ports.monotonic() + if view not in self.cameras.matrices or self.state in {"PASSED", "PAUSED", "ABORTED", "FAILED"}: + return + capture, motion = self.capture, self._motion + epoch = self._observation_epoch + steady_after = self._steady_capture_after_ns + locking = self._reference_capture_key or self.execution.session.phase == Phase.REFERENCE_LOCKING + matrix = self.cameras.matrices[view].copy() + width, height = self.cameras.image_sizes[view] + feedback_history = tuple(self.state_history) + command_history = tuple(self.command_history) + direction_history = tuple(self.command_direction_history) + roles = {tag.tag_id: tag.role for tag in self._view(view).tags} + corners = {} + for detection in observation.detections: + points = np.asarray(detection.corners, dtype=float) + if (detection.tag_id not in roles or points.shape != (4, 2) or not np.all(np.isfinite(points)) + or detection.hamming > self.parameters.detection.maximum_hamming or detection.decision_margin < self.parameters.detection.minimum_decision_margin + or np.min(np.linalg.norm(points-np.roll(points, -1, axis=0), axis=1)) < self.parameters.detection.minimum_edge_pixels + or np.min(points) < 2 or np.max(points[:, 0]) > width-3 or np.max(points[:, 1]) > height-3): + continue + corners[roles[detection.tag_id]] = points + matched = interpolate_state_u8(feedback_history, stamp, maximum_skew_ns=self.parameters.maximum_state_image_skew_ns) + # Commands are sample-and-held by the transport, NOT interpolated + # from a future setpoint or read from the current motion timer. + command = next((row.position_u8 for row in reversed(command_history) + if row.stamp_ns <= stamp and stamp-row.stamp_ns <= self.parameters.maximum_state_image_skew_ns), None) + frame = CaptureFrame(view, stamp, matrix, corners, + None if matched is None else matched[0], command, 0 if matched is None else matched[1], + next((d for t, d in reversed(direction_history) if t <= stamp), ())) + record_motion = motion + if motion is not None and motion.phase == "steady" and (steady_after is None or stamp < steady_after): + record_motion = None + # Pose estimation is deliberately outside the state lock. Start swaps + # capture objects; pause/abort invalidates the epoch; movement changes + # the motion identity. None of their late results may enter the session. + rows, movement = capture.consume(frame, record_motion, collect_installations=locking) + with self.step_data_lock: + if (epoch != self._observation_epoch or capture is not self.capture + or motion is not self._motion): + return + if motion is not None and motion.phase == "steady" and steady_after != self._steady_capture_after_ns: + return + if movement is not None: + self._pause(f"fixed_reference_moved:{view}:{movement.drift_px}") + return + for row in rows: + self.raw_records.append(row) + self._unit_rows.append(row) + if motion is not None and motion.phase == "steady": + self._steady_rows.append(row) + append_jsonl(self.raw_path, row) + + def tick(self) -> None: + with self.step_data_lock: + self._advance() + + def snapshot(self) -> RuntimeSnapshot: + with self.step_data_lock: + return build_snapshot(self) + + def feedback_fresh(self) -> bool: + with self.step_data_lock: + return bool(self.state_receive_times and self.ports.monotonic()-self.state_receive_times[-1] <= 1.0) + + def close(self) -> None: + with self.step_data_lock: + self._observation_epoch += 1 + self.finalization.close() + + def _advance(self): + if self.state in {"PASSED", "PAUSED", "ABORTED", "FAILED"}: + return + now = self.ports.monotonic() + session = self.execution.session + fresh = self.sdk_adapter.health() + if not self.started: + devices = self._device_status(now) + if devices.ready: + if session.phase == Phase.WAIT_DEVICE: + session.device_ready() + self.reason = "ready_for_operator_start" + else: + session.devices_unavailable() + self.reason = "; ".join(devices.waiting_for) + return + current = self.last_command + if current is None: + if not self.state_history: + if now-self._started_at > self.profile.acquisition.feedback_stale_seconds: + self._pause("feedback_stale:开始后超过一秒没有新鲜反馈") + return # Require a post-Start observation, not a preview sample. + current = self.sdk_adapter.initial_command(self.latest_feedback) + decision = self.safety.evaluate(SafetySample(now, + self.state_receive_times[-1] if self.state_receive_times else None, + tuple(current), tuple(self.latest_feedback), fresh, + competing_controller=self.ports.command_publisher_count() > 1, + motion_expected=self.segment.motion_expected if self.segment else False, + motion_id=self.segment.identity if self.segment else "", + motion_goals=self.segment.goals if self.segment else ())) + if not decision.safe: + self._pause(f"{decision.code}:{decision.reason}:{decision.details}") + return + if self.finalization.started: + self._publish_command(list(current)) + self._poll_finalization() + return + phase = session.phase + if phase == Phase.REFERENCE_LOCKING: + self._publish_command(list(current)) + if not self.reference_lock.locking_enabled: + self.reference_lock.start_locking() + self.reason = "locking_fixed_base_references:missing="+",".join(self.reference_lock.missing_views) + if self.reference_lock.locked and set(self.capture.locked_poses) == set(self.profile.vision.view_names): + session.reference_locked() + if session.phase != Phase.REFERENCE_POSES: + self._save_reference() + return + if phase == Phase.RESUME_VERIFY: + self._verify_resume() + return + if phase == Phase.EVALUATE: + action = self.execution.action + quality = self.execution.evaluate(self.raw_records) + append_jsonl(self.raw_path, {"kind": "scan_unit_complete", "task_name": action.scan_unit.task_key, + "cycle": action.scan_unit.cycle, "direction": action.scan_unit.direction, + "attempt": action.retry+1, "passed": quality.passed, + "failures": list(quality.failures), "metrics": dict(quality.metrics)}) + if session.phase == Phase.PAUSED: + self._pause("sweep_quality_failed:"+",".join(quality.failures)) + return + if phase == Phase.FIT: + self._motion = None + self._finalize() + return + motion = self.execution.motion(current) + if motion is None: + if phase == Phase.PREPARE and session.current_unit is None: + session.preparation_complete() + return + self._pause(f"unhandled_session_phase:{phase.value}") + return + if self.segment is None: + if self.commanded_speed != motion.speed: + self._publish_speed(motion.speed) + self.step_speed_ready_at = now+float(self.parameters.speed_settle_seconds) + return + if now < self.step_speed_ready_at: + return + self._segment_number += 1 + self._motion = motion + self._steady_capture_after_ns = None + self._reference_capture_key = None + self._reference_observation_started = None + self._begin_motion_capture(motion) + self.segment = MotionExecution(self.profile, motion, initial_command=current, + initial_feedback=self.latest_feedback, now=now, identity=str(self._segment_number)) + shaped = self.segment.sample(now) + self._publish_command(list(shaped)) + self.segment.observe(self.latest_feedback, stamp=self.state_receive_times[-1], now=now) + self.reason = motion.phase+":"+str(motion.task_key) + steady_complete = False + if motion.phase == "steady": + if self.segment.steady_ready(now): + if self._steady_capture_after_ns is None: + self._steady_capture_after_ns = self.ports.clock_ns() + from .scan_quality import observation_streams + task = next(t for t in self.profile.motion.tasks if t.key == motion.task_key) + steady_complete = all(len({r["image_stamp_ns"] for r in self._steady_rows + if r.get(field) == joint and r.get("view") == spec.view + and r.get("sample_phase") == "steady"}) >= self.profile.acquisition.steady_minimum_samples + for field, joint, spec in observation_streams(self.profile, task)) + else: + self._steady_capture_after_ns = None + self._steady_rows.clear() + if self.segment.arrived(now) or steady_complete: + if motion.phase == "reference_pose": + waypoint = next(w for w in self.profile.motion.resume_verification_waypoints if w.key == motion.task_key) + if self._reference_observation_started is None: + self._reference_observation_started = now + self._reference_capture_key = waypoint.key + required = [f"{waypoint.key}/{view.name}:{tag.role}" + for view in self.profile.vision.views for tag in view.tags + if tag.tag_id in waypoint.tag_ids_by_view.get(view.name, ())] + enough = all(len(self.capture.installations.get(key, ())) >= self.reference_lock.minimum_frames for key in required) + if not enough and now-self._reference_observation_started < 2.0: + return + # Missing installation evidence invalidates reuse, not the + # entire fresh capture. Never forge a matching fingerprint. + self._reference_capture_key = None + self.execution.motion_complete() + if phase == Phase.REFERENCE_POSES and session.phase != phase: + self._save_reference() + self.segment, self._motion = None, None + + def _begin_motion_capture(self, motion): + """Keep direction statistics across its separate steady checkpoints.""" + self._steady_rows = [] + if motion.recording: + key = (motion.task_key, motion.cycle, motion.direction, motion.attempt) + if key != self._capture_unit_key: + self._unit_rows = [] + self._capture_unit_key = key + + def _device_status(self, now): + """Readiness is live, and independent of whether a Tag is visible. + + The two-second camera window is only a pre-Start condition. During + scans, transient visual loss still uses the post-direction data gate. + """ + health = self.sdk_adapter.health() + waiting = [] + if not health.connected: + waiting.append("SDK 未连接") + if len(self.latest_feedback) != self.command_count or not health.feedback_fresh: + waiting.append("等待完整、新鲜的 SDK 反馈") + if not health.position_mode: + waiting.append("SDK 尚未确认位置控制模式") + if health.active_faults: + waiting.append("SDK 活动故障：" + ",".join(health.active_faults)) + if self.ports.command_publisher_count() > 1: + waiting.append("存在其他机械手指令发布者") + cameras = {} + for view in self.profile.vision.view_names: + info_at = self.cameras.info_received_at.get(view) + detection_at = self.cameras.detections_received_at.get(view) + info_ready = (view in self.cameras.matrices and info_at is not None + and 0 <= now-info_at <= 2.0) + detection_ready = detection_at is not None and 0 <= now-detection_at <= 2.0 + error = self.cameras.errors.get(view, "") + cameras[view] = CameraReadiness(info_ready, detection_ready, error) + if error: + waiting.append(f"{view} 相机内参与外参不匹配或无效：{error}") + elif not info_ready: + waiting.append(f"{view} 未收到有效、新鲜的 camera_info（检查相机是否出图）") + if not detection_ready: + waiting.append(f"{view} 未收到新鲜的 Tag 检测消息（检查图像、整流和检测节点）") + return DeviceReadiness(not waiting, tuple(waiting), cameras) + + def _accept_feedback(self, names, positions, stamp: int) -> None: + state = self.sdk_adapter.parse_feedback(names, positions) + if state is None: + return + violation = next(( + (index, value) + for index, value in enumerate(state) + 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_feedback = 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 + if stamp <= 0: + stamp = self.ports.clock_ns() + if not self.state_history or stamp > self.state_history[-1].stamp_ns: + self.state_history.append(StateSample(stamp, state)) + self.latest_feedback = state + self.state_receive_times.append(self.ports.monotonic()) + + def _publish_command(self, values): + if self.parameters.commands_enabled: + self.sdk_adapter.publish_position(values) + + def _write_position(self, values): + if not self.parameters.commands_enabled: + return + bounded = self.sdk_adapter.validate_command(values) + self.ports.publish_position(bounded) + self.last_command = bounded + if self.command_history: + for index, (previous, current) in enumerate(zip(self.command_history[-1].position_u8, bounded)): + if abs(current-previous) > 1e-9: + self._command_directions[index] = "increasing" if current > previous else "decreasing" + stamp = self.ports.clock_ns() + self.command_history.append(StateSample(stamp, bounded)) + self.command_direction_history.append((stamp, tuple(self._command_directions))) + + def _publish_speed(self, speed): + indices = self.profile.command.command_index_by_joint.values() + # Position and speed slots are mapped inside the protocol adapter. + for index in sorted(set(indices)): + self.sdk_adapter.set_speed(index, speed) + + def _write_speed(self, _slot, speed): + if self.command_unit != "u8": + self.commanded_speed = speed + return + if not self.parameters.commands_enabled or self.commanded_speed == int(speed): + return + # Position and speed vectors have separate layouts in the SDK protocol. + count = max(self.profile.command.speed_slot_by_command_index.values(), default=self.command_count-1)+1 + self.ports.publish_setting({"setting_cmd": "set_speed", "params": { + "hand_type": self.profile.key.side, "speed": [int(speed)]*count}}) + self.commanded_speed = int(speed) + + def _publish_torque(self): + if self.parameters.commands_enabled and self.command_unit == "u8": + self.ports.publish_setting({"setting_cmd": "set_max_torque_limits", "params": { + "hand_type": self.profile.key.side, + "torque": [self.parameters.torque_u8]*self.command_count}}) + + def _pause(self, reason: str) -> None: + with self.step_data_lock: + if self.state in {"PASSED", "ABORTED"} or self._pause_effects_applied: + return + self.finalization.cancel() + self._observation_epoch += 1 + self._pause_effects_applied = True + if self.execution.session.phase != Phase.PAUSED: + from .reporting.reasons_zh import reason_zh + code, explanation, suggestion = reason_zh({"reason": str(reason)}, model_name=self.model_name) + self.execution.session.pause(code, explanation, {"raw_reason": str(reason)}, suggestion=suggestion) + self._pause_locked(reason) + + def _pause_locked(self, reason: str) -> None: + self.reason = str(reason) + # In radian mode, hold the last command rather than feeding an + # uncalibrated feedback value back into the command domain. + if self.last_command is not None: + self._publish_command(list(self.last_command)) + # No prior command means no movement was authorized. In particular, + # never echo out-of-domain feedback as a "hold" during preview. + step = self._motion + target = [] if step is None else list(step.target) + errors = ( + [] + if len(target) != len(self.latest_feedback) + else [ + abs(float(actual) - float(expected)) + for actual, expected in zip(self.latest_feedback, 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_feedback), + 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 _new_reference_lock(self): + return ReferenceLock( + { + view.name: next( + tag.tag_id for tag in view.tags if tag.fixed_reference + ) + for view in self.profile.vision.views + }, + minimum_frames=self.profile.acquisition.fixed_reference_minimum_frames, + maximum_corner_drift_px=( + self.profile.acquisition.fixed_reference_maximum_drift_px + ), + confirmation_frames=( + self.profile.acquisition.fixed_reference_confirmation_frames + ), + ) + + def _save_reference(self): + fixed, moving = self.capture.fingerprint_poses() + self._current_fingerprint = {"profile_id": self.profile.key.profile_id, + "acquisition_policy_version": self.execution.session.engine.checkpoint_token, + "protected_hashes": {**self.parameters.protected_inputs, "intrinsics_sha256": hashlib.sha256( + json.dumps({v: m.tolist() for v, m in sorted(self.cameras.matrices.items())}, sort_keys=True).encode()).hexdigest()}, + "fixed_corners_by_view": self.reference_lock.fingerprint(), "fixed_poses": fixed, + "moving_tag_poses": moving} + append_jsonl(self.raw_path, {"kind": "fixed_base_reference_locked", **self._current_fingerprint}) + + def _verify_resume(self): + decision = ResumeDecision(False, "基准变化，已放弃旧断点并重新采集") + rows = [] + try: + rows = load_capture(self.parameters.resume_raw_samples_path) + headers = [r for r in rows if r.get("kind") == "session_start"] + references = [r for r in rows if r.get("kind") == "fixed_base_reference_locked"] + if len(headers) != 1 or len(references) != 1: + raise ValueError("missing unique checkpoint header/reference") + header = headers[0] + if header.get("serial_number") != self.parameters.serial_number or not self.execution.session.engine.resume_compatible(header): + raise ValueError("checkpoint identity/policy changed") + complete = {(r["task_name"], r["cycle"], r["direction"]) for r in rows + if r.get("kind") == "scan_unit_complete" and r.get("passed") is True} + used_tasks = {key[0] for key in complete} + from .scan_quality import observation_streams + required = {f"{spec.view}:{role}" for task in self.profile.motion.tasks if task.key in used_tasks + for _, _, spec in observation_streams(self.profile, task) + for role in (spec.parent_role, spec.child_role) + if not any(tag.role == role and tag.fixed_reference for tag in self._view(spec.view).tags)} + verifier = ResumeVerifier(required_fixed_views=self.profile.vision.view_names, + required_fixed_poses=self.profile.vision.view_names, required_moving_poses=required, + required_hashes=self._current_fingerprint["protected_hashes"]) + previous = {**references[0], "acquisition_policy_version": header["acquisition_policy_version"]} + from .resume import select_installation_evidence + old, new = select_installation_evidence(fingerprint_from_mapping(previous), + fingerprint_from_mapping(self._current_fingerprint), required) + decision = verifier.compare(old, new) + if decision.reuse: + from dataclasses import replace + decision = replace(decision, completed_units=tuple(sorted(complete))) + except (OSError, ValueError, KeyError, TypeError) as error: + decision = ResumeDecision(False, decision.reason, (str(error),)) + decision = self.execution.restore(decision, rows) + if decision.reuse: + used = set(decision.completed_units) + imported = [r for r in rows if (r.get("task_name"), r.get("cycle"), r.get("direction")) in used] + for row in imported: + self.raw_records.append(row) + append_jsonl(self.raw_path, row) + self._resumed_count = len(used) + self._resume_message = decision.reason + append_jsonl(self.raw_path, {"kind": "resume_verification", "reuse": decision.reuse, + "reason": decision.reason, "incompatible_fields": list(decision.incompatible_fields), + "completed_units": list(decision.completed_units)}) + + def _view(self, name: str): + return next(view for view in self.profile.vision.views if view.name == name) + + 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 _finalize(self) -> None: + if self.finalization.started or self.execution.session.phase != Phase.FIT: + return + self.reason = "fitting_validating_and_writing_artifacts" + self.finalization.start(FinalizationInputs( + self.parameters.session_dir, self.parameters.serial_number, self.parameters.source_urdf, + self._current_fingerprint.get("protected_hashes", self.parameters.protected_inputs), + tuple(self.raw_records), + )) + + def _poll_finalization(self) -> None: + try: + result = self.finalization.poll(self._finalizer_event, self._authorize_commit) + if result is None: + return + correction = result[2] + except FinalizationProtocolError as error: + self._pause(f"finalization_protocol_error:{error}") + return + except Exception as error: + atomic_write_json(self.parameters.session_dir / "failure_diagnostic.json", { + "profile_id": self.profile.key.profile_id, "reason": str(error), + "traceback": traceback.format_exc(), "source_urdf_unchanged": True, + }) + self._pause(f"fit_or_publication_failed:{error}") + return + self.execution.session.published() + self.reason = "partial_calibration_passed" if self.profile.scope.default_scope != "full" else "full_calibration_passed" + self.final_json = str(self.parameters.session_dir / self.profile.artifacts.calibration_filename.format( + serial_number=self.parameters.serial_number, side=self.profile.key.side, model=self.model_name.lower())) + self.final_urdf = str(correction.path) + + def _finalizer_event(self, event: str) -> None: + session = self.execution.session + if event == "fit_complete": + session.fit_complete() + elif event == "holdout_complete": + session.holdout_complete(passed=True) + elif event == "artifacts_built": + session.artifacts_built() + elif event == "urdf_validated": + session.urdf_validated(passed=True) + else: + raise ValueError(f"unknown finalizer event:{event}") + + def _authorize_commit(self) -> None: + if self.execution.session.phase != Phase.PUBLISH: + raise RuntimeError("finalization did not complete all acceptance stages") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py index f24ddd9..c64d8b3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/engine.py @@ -2,7 +2,7 @@ from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass import math from typing import Any, Mapping, Sequence @@ -11,7 +11,7 @@ import numpy as np from ..core import CalibrationProfile, TaskSpec, validate_profile -ACQUISITION_POLICY_VERSION = "unified_engine_v1" +from ..core.domain.profile import ACQUISITION_POLICY_VERSION TRAINING_CYCLES = (0, 1, 2) HOLDOUT_CYCLE = 3 @@ -34,23 +34,90 @@ class SweepQuality: 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) + +def evaluate_observability( + feedback_progress_01: Sequence[float], + *, + minimum_span: float, + minimum_valid_samples: int, + minimum_bins: int, + maximum_unobserved_fraction: float, + total_frames: int, + joint_frame_rate: float, + feedback_hz: float, + detection_rate: float, + bin_count: int = 256, + include_endpoint_gaps: bool = False, +) -> SweepQuality: + """Evaluate the one model-independent post-sweep data gate.""" + if int(bin_count) < 32: + raise ValueError("normalized sweep bin count must be at least 32") + values = np.asarray(feedback_progress_01, dtype=float) + if values.ndim != 1: + raise ValueError("normalized sweep samples must be a vector") + valid = np.isfinite(values) & (values >= 0.0) & (values <= 1.0) + invalid_count = int(np.count_nonzero(~valid)) + # Out-of-domain data is not an observation of the nearest endpoint. + values = values[valid] + 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:]) + ] + gap_scope = ( + "full_command_domain" if include_endpoint_gaps + else "observed_feedback_span" + ) + missing_runs = ( + [bins[0], bin_count - 1 - bins[-1], *internal_missing_runs] + if bins and include_endpoint_gaps + else internal_missing_runs + if bins + else [bin_count] + ) + maximum_gap = max(missing_runs, default=0) + allowed_gap = max( + 1, int(math.floor(bin_count * maximum_unobserved_fraction)) + ) + failures: list[str] = [] + if values.size < int(minimum_valid_samples): + failures.append(f"frames={values.size}") + if span < float(minimum_span): + failures.append("feedback_span") + if len(bins) < int(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}") + if feedback_hz <= 0.0: + warnings.append("feedback_rate_unavailable") + metrics = { + "policy_version": ACQUISITION_POLICY_VERSION, + "valid_frames": int(values.size), + "discarded_domain_samples": invalid_count, + "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) class CalibrationEngine: - """One policy kernel shared by all model-specific ROS/SDK wrappers.""" + """Schedule and post-direction quality policy used by the single session executor.""" def __init__(self, profile: CalibrationProfile) -> None: validate_profile(profile) @@ -64,14 +131,16 @@ class CalibrationEngine: units: list[ScanUnit] = [] for task in self.profile.motion.tasks: speed = self._formal_speed(task) + forward = "increasing" if task.end_value > task.start_value else "decreasing" + reverse = "decreasing" if forward == "increasing" else "increasing" for cycle in (*TRAINING_CYCLES, HOLDOUT_CYCLE): units.extend(( ScanUnit( - task.key, cycle, "decreasing", task.start_value, + task.key, cycle, forward, task.start_value, task.end_value, speed, ), ScanUnit( - task.key, cycle, "increasing", task.end_value, + task.key, cycle, reverse, task.end_value, task.start_value, speed, ), )) @@ -81,6 +150,8 @@ class CalibrationEngine: """Only physical-angle profiles perform a single <=3 degree jog.""" if self.profile.command.unit != "rad": return None + if self.profile.acquisition.mapping_probe_maximum_rad <= 0: + return None distance = task.end_value - task.start_value return math.copysign( min(abs(distance), self.profile.acquisition.mapping_probe_maximum_rad), @@ -109,70 +180,26 @@ class CalibrationEngine: 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)) + quality = evaluate_observability( + feedback_progress_01, + minimum_span=minimum_span, + minimum_valid_samples=policy.minimum_valid_samples, + minimum_bins=policy.minimum_bins, + maximum_unobserved_fraction=policy.maximum_unobserved_fraction, + total_frames=total_frames, + joint_frame_rate=joint_frame_rate, + feedback_hz=feedback_hz, + detection_rate=detection_rate, + bin_count=bin_count, + include_endpoint_gaps=False, ) - 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) + if policy.require_endpoint_observations: + values = np.asarray(feedback_progress_01, dtype=float) + values = values[np.isfinite(values) & (values >= 0) & (values <= 1)] + if not values.size or float(np.min(values)) > policy.endpoint_tolerance_01 or float(np.max(values)) < 1.0 - policy.endpoint_tolerance_01: + return SweepQuality(False, (*quality.failures, "endpoint_observation"), quality.warnings, quality.metrics) + return quality def resume_compatible(self, session_start: Mapping[str, Any]) -> bool: return ( @@ -181,129 +208,7 @@ class CalibrationEngine: 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": @@ -317,6 +222,6 @@ class CalibrationEngine: __all__ = [ - "ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult", + "ACQUISITION_POLICY_VERSION", "CalibrationEngine", "HOLDOUT_CYCLE", "ScanUnit", "SweepQuality", "TRAINING_CYCLES", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py new file mode 100644 index 0000000..7f29f0b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/execution.py @@ -0,0 +1,154 @@ +"""Translate the single session's actions into declarative motion effects. + +No ROS dependency, hand-identity branch, alternate retry loop or serializer. +The same driver is usable with real SDK transport and a virtual clock. +""" + +from dataclasses import replace +import math + +from .session import CalibrationPhase as Phase, CalibrationSession +from .motion_execution import MotionCommand +from .scan_quality import evaluate_capture_unit +from .trajectory import (build_calibration_preparation_waypoints, + build_calibration_return_waypoints, build_calibration_motion_command) + + +class SessionExecution: + def __init__(self, profile): + self.profile = profile + self.session = CalibrationSession(profile) + self.first_cycle_spans = {} + self.completed_units = set() + self.last_quality = None + self._effects = [] + self._action_key = None + self._entered_task = None + + @property + def action(self): + return self.session.action() + + def motion(self, current_command): + """Return the next effect for the current phase, without advancing it.""" + action = self.action + key = (action.phase, action.scan_unit, action.retry) + if key != self._action_key: + self._effects = list(self._make_effects(current_command)) + self._action_key = key + return self._effects[0] if self._effects else None + + def _make_effects(self, current): + action = self.action + params = self.profile.motion.speed_parameters + baseline_speed = float(params.get("baseline_rad_s", 0.1) if self.profile.command.unit == "rad" + else params.get("baseline_u8", params.get("preflight_u8", 1))) + if action.phase == Phase.REFERENCE_POSES: + waypoints = self.profile.motion.resume_verification_waypoints + result = [MotionCommand("reference_pose", waypoint.command, baseline_speed, task_key=waypoint.key) + for waypoint in waypoints] + result.extend(MotionCommand("reference_return", target, baseline_speed) + for target in build_calibration_return_waypoints(profile=self.profile, current_command=waypoints[-1].command)) + return tuple(result) + if action.phase in {Phase.BASELINE, Phase.RETURN_BASELINE}: + return tuple(MotionCommand("baseline" if action.phase == Phase.BASELINE else "return", target, baseline_speed) + for target in build_calibration_return_waypoints(profile=self.profile, current_command=current)) + unit = action.scan_unit + if unit is None: + return () + task = next(t for t in self.profile.motion.tasks if t.key == unit.task_key) + common = dict(task_key=task.key, command_index=task.command_index, + cycle=unit.cycle, attempt=action.retry+1) + if action.phase == Phase.PREPARE: + entry = [] + if self._entered_task != task.key: + pose = list(current) + for waypoint in task.entry_waypoints: + for index, value in waypoint: + pose[index] = value + entry.append(MotionCommand("clearance", tuple(pose), unit.speed, **common)) + self._entered_task = task.key + current = tuple(pose) + return tuple(entry) + tuple(MotionCommand("prepare", target, unit.speed, **common) + for target in build_calibration_preparation_waypoints(task, profile=self.profile, + current_command=current, start_value=unit.start)) + if action.phase == Phase.MAPPING_PROBE: + delta = self.session.engine.mapping_probe_delta(task) + start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile)) + target = list(start) + # Probe toward this task's other endpoint. The complete two-leg + # motion is <=3 degrees from the already prepared pose. + target[task.command_index] += math.copysign(abs(delta), unit.end-unit.start) + speed = float(task.preflight_speed) + return (MotionCommand("mapping_probe", tuple(target), speed, **common), + MotionCommand("mapping_return", start, speed, **common)) + if action.phase in {Phase.SWEEP, Phase.RESCAN}: + target = tuple(build_calibration_motion_command(task, unit.end, profile=self.profile)) + from .steady import steady_targets + start = tuple(build_calibration_motion_command(task, unit.start, profile=self.profile)) + effects = [MotionCommand("sweep", target, unit.speed, direction=unit.direction, **common), + MotionCommand("steady_prepare", start, unit.speed, **common)] + effects.extend(MotionCommand("steady", + tuple(build_calibration_motion_command(task, value, profile=self.profile)), + unit.speed, direction=unit.direction, steady_index=index, **common) + for index, value in enumerate(steady_targets(self.profile, unit))) + return tuple(effects) + return () + + def motion_complete(self): + if not self._effects: + raise RuntimeError("cannot complete a motion that was not requested") + self._effects.pop(0) + if self._effects: + return + phase = self.session.phase + if phase == Phase.BASELINE: + self.session.baseline_complete() + elif phase == Phase.REFERENCE_POSES: + self.session.reference_poses_complete() + elif phase == Phase.RETURN_BASELINE: + self.session.return_complete() + elif phase == Phase.PREPARE: + self.session.preparation_complete() + elif phase == Phase.MAPPING_PROBE: + self.session.mapping_probe_complete() + elif phase in {Phase.SWEEP, Phase.RESCAN}: + self.session.sweep_complete() + else: + raise RuntimeError(f"unexpected completed motion:{phase.value}") + + def evaluate(self, records): + if self.session.phase != Phase.EVALUATE: + raise RuntimeError("sweep evaluation outside EVALUATE") + action = self.action + quality = evaluate_capture_unit(self.profile, action.scan_unit, action.retry+1, + records, first_cycle_spans=self.first_cycle_spans) + self.last_quality = quality + if quality.passed: + unit = action.scan_unit + self.completed_units.add((unit.task_key, unit.cycle, unit.direction)) + self.session.evaluation_complete(quality) + return quality + + def restore(self, decision, records): + """Recheck each durable unit using the current, hash-bound policy.""" + self.first_cycle_spans.clear() + completed = set(decision.completed_units) if decision.reuse else set() + valid = [] + for unit in self.session.engine.scan_units(): + key = (unit.task_key, unit.cycle, unit.direction) + if key not in completed: + continue + attempts = [int(r.get("attempt", 1)) for r in records + if (r.get("task_name"), r.get("cycle"), r.get("direction")) == key] + quality = evaluate_capture_unit(self.profile, unit, max(attempts, default=1), + records, first_cycle_spans=self.first_cycle_spans) + if quality.passed: + valid.append(key) + checked = replace(decision, completed_units=tuple(valid)) + self.session.resume_checked(checked) + self.completed_units = set(valid) + return checked + + +__all__ = ["SessionExecution"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py new file mode 100644 index 0000000..c5ea961 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/inputs.py @@ -0,0 +1,54 @@ +"""Transport-neutral observations and explicit coordinator I/O ports.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Callable, Mapping, Sequence + +from .adapters.base import SdkAdapter + + +@dataclass(frozen=True) +class CameraModelInput: + width: int + height: int + k: tuple[float, ...] + d: tuple[float, ...] + r: tuple[float, ...] + p: tuple[float, ...] + + +@dataclass(frozen=True) +class TagDetection: + tag_id: int + corners: tuple[tuple[float, float], ...] + hamming: int + decision_margin: float + + +@dataclass(frozen=True) +class DetectionInput: + view: str + stamp_ns: int + detections: tuple[TagDetection, ...] + + +@dataclass(frozen=True) +class ServiceResult: + success: bool + message: str + + +@dataclass(frozen=True) +class RuntimePorts: + clock_ns: Callable[[], int] + monotonic: Callable[[], float] + command_publisher_count: Callable[[], int] + publish_position: Callable[[Sequence[float]], None] + publish_setting: Callable[[Mapping[str, object]], None] + + +AdapterFactory = Callable[ + [Callable[[Sequence[float]], None], Callable[[int, float], None], Callable[[], bool]], + SdkAdapter, +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py new file mode 100644 index 0000000..9b1975a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -0,0 +1,143 @@ +"""Clock-driven trajectory execution, independent of ROS and hand identity. + +The session owns business transitions. This module executes one whole segment +and supplies its stable feedback-space safety goals; it never retries or fits. +""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass +import math + +from .safety import MotionGoal +from .trajectory import cosine_position_trajectory_u8, cosine_ramp_velocity_trajectory + + +@dataclass(frozen=True) +class MotionCommand: + phase: str + target: tuple[float, ...] + speed: float + task_key: str | None = None + command_index: int | None = None + cycle: int | None = None + direction: str | None = None + attempt: int = 1 + steady_index: int | None = None + + @property + def recording(self): + return self.direction is not None + + +class MotionExecution: + def __init__(self, profile, command: MotionCommand, *, initial_command, + initial_feedback, now: float, identity: str): + self.profile, self.command, self.identity = profile, command, str(identity) + self.start = tuple(float(v) for v in initial_command) + self.start_feedback = tuple(float(v) for v in initial_feedback) + self.started_at = float(now) + self.last_feedback_stamp = None + self.history = deque(maxlen=10) + self.stamped_history = deque(maxlen=200) + self.finished_at = None + self.phase = 0.0 + self.duration = 0.0 + self.fraction = 0.0 + layout = profile.command + for vector in (self.start, command.target): + if len(vector) != layout.command_count or any(not math.isfinite(v) or not lo <= v <= hi + for v, lo, hi in zip(vector, layout.minimum_values, layout.maximum_values)): + raise ValueError("motion command outside declared command domain") + if len(self.start_feedback) != layout.command_count: + raise ValueError("motion starts without complete feedback") + self.resolution = 0.5 if layout.unit == "u8" else 0.001 + self.stability = 2.0 if layout.unit == "u8" else 0.02 + probe = command.phase in {"mapping_probe", "mapping_return"} + # A mapping jog proves direction and basic electrical movement, not + # servo accuracy. Baseline corrections no larger than the mapping + # jog are not a "significant movement" request: nominal encoder bias + # at an already-open stop must not manufacture a stall. + self.minimum_request = (2.0 if layout.unit == "u8" else self.resolution if probe + else max(profile.acquisition.mapping_probe_maximum_rad, math.radians(3))) + self.required_fraction = 0.10 if probe else 0.80 + self.deltas = tuple(end-start for start, end in zip(self.start, command.target)) + self.moving = tuple(i for i, delta in enumerate(self.deltas) + if abs(delta) > self.minimum_request and i not in layout.disabled_indices) + self.goals = tuple(MotionGoal(i, self.start_feedback[i], + self.start_feedback[i] + self.required_fraction*self.deltas[i], self.resolution, self.resolution) + for i in self.moving) + + def sample(self, now: float) -> tuple[float, ...]: + distance = max((abs(v) for v in self.deltas), default=0.0) + elapsed = max(0.0, float(now)-self.started_at) + params = self.profile.motion.speed_parameters + if self.profile.command.unit == "u8": + travelled, phase, duration = cosine_position_trajectory_u8(0, distance, + elapsed, float(params.get("command_trajectory_full_range_seconds", 6.0))) + else: + speed = self.command.speed + caps = self.profile.command.maximum_velocity + if caps and distance > 0: + speed = min([speed] + [caps[i]*distance/abs(delta) + for i, delta in enumerate(self.deltas) if delta != 0]) + travelled, phase, duration = cosine_ramp_velocity_trajectory(0, distance, + elapsed, speed, float(params.get("trajectory_ramp_seconds", 0.4))) + self.phase, self.duration = phase, duration + fraction = 1.0 if distance == 0 else travelled/distance + self.fraction = fraction + values = tuple(start+delta*fraction for start, delta in zip(self.start, self.deltas)) + return tuple(float(round(v)) for v in values) if self.profile.command.unit == "u8" else values + + def observe(self, feedback, *, stamp, now): + # A fast timer is not ten independent feedback samples. + if stamp == self.last_feedback_stamp: + return + self.last_feedback_stamp = stamp + self.history.append(tuple(feedback)) + self.stamped_history.append((float(now), tuple(feedback))) + if self.phase >= 1.0 and self.finished_at is None: + self.finished_at = float(now) + + @property + def motion_expected(self): + # A cosine ramp initially requests less than encoder resolution. Start + # the watchdog only after meaningful demand, then keep the segment ID. + return bool(self.moving and self.phase > 0 and + any(abs(self.deltas[i])*self.fraction > self.minimum_request for i in self.moving)) + + def arrived(self, now): + if self.command.phase == "steady": + # Data sufficiency is evaluated after the direction; never wait + # indefinitely for a Tag or exact command/feedback equality. + return (self.finished_at is not None and + now-self.finished_at >= self.profile.acquisition.steady_timeout_seconds) + if self.phase < 1 or self.finished_at is None or len(self.history) < 3: + return False + hold = float(self.profile.motion.speed_parameters.get("endpoint_hold_seconds", 0.25)) + if now-self.finished_at < hold: + return False + for i in self.moving: + projected = math.copysign(1, self.deltas[i])*(self.history[-1][i]-self.start_feedback[i]) + if projected+self.resolution < self.required_fraction*abs(self.deltas[i]): + return False + if max(row[i] for row in self.history)-min(row[i] for row in self.history) > self.stability: + return False + return True + + def steady_ready(self, now): + if self.command.phase != "steady" or self.finished_at is None: + return False + window = self.profile.acquisition.steady_window_seconds + rows = [(stamp, value) for stamp, value in self.stamped_history + if stamp >= max(self.finished_at, now-window-0.05)] + if len(rows) < 3 or rows[-1][0]-rows[0][0] < window: + return False + i = self.command.command_index + threshold = max(2*self.resolution, 0.005*(self.profile.command.maximum_feedback_values[i] + - self.profile.command.minimum_feedback_values[i])) + return max(v[i] for _, v in rows)-min(v[i] for _, v in rows) <= threshold + + +__all__ = ["MotionCommand", "MotionExecution"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py deleted file mode 100644 index 580fb6c..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py +++ /dev/null @@ -1 +0,0 @@ -"""ROS message/service shells selected through the local profile registry.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py deleted file mode 100644 index b167647..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py +++ /dev/null @@ -1,23 +0,0 @@ -"""Generic ROS-node dispatcher selected by the registered profile identity.""" - -from __future__ import annotations - -import argparse -import sys - -from ...compat import legacy_default_profile_key -from ...core import ProfileKey -from ...models import get_default_registry - - -def main(args: list[str] | None = None) -> None: - arguments = list(sys.argv[1:] if args is None else args) - selector = argparse.ArgumentParser(add_help=False) - selector.add_argument("--profile-id", default=None) - selected, remaining = selector.parse_known_args(arguments) - key = ( - legacy_default_profile_key() - if selected.profile_id is None - else ProfileKey.parse(selected.profile_id) - ) - get_default_registry().get(key).engine.node_main(remaining) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py new file mode 100644 index 0000000..6a341cb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py @@ -0,0 +1,51 @@ +"""Resolved runtime inputs, independent of ROS parameter and message types.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Mapping + +from ..core.geometry.extrinsics import CameraExtrinsics +from ..core.geometry.pnp import PoseTrackingParameters, SquareTagPoseTracker + + +@dataclass(frozen=True) +class DetectionPolicy: + maximum_hamming: int + minimum_decision_margin: float + minimum_edge_pixels: float + + +@dataclass(frozen=True) +class RuntimeParameters: + serial_number: str + session_dir: Path + source_urdf: Path + camera_extrinsics_file: Path + extrinsics: CameraExtrinsics + protected_inputs: Mapping[str, str] + resume_raw_samples_path: Path | None + command_topic: str + state_topic: str + setting_topic: str + camera_info_topics: Mapping[str, str] + detection_topics: Mapping[str, str] + commands_enabled: bool + speed_settle_seconds: float + torque_u8: int + maximum_state_image_skew_ns: int + command_rate_hz: float + tag_size_m: float + detection: DetectionPolicy + tracking: PoseTrackingParameters + + def new_trackers(self, views): + return {view: SquareTagPoseTracker( + maximum_reprojection_error_px=self.tracking.maximum_reprojection_error_px, + reprojection_tie_px=self.tracking.reprojection_tie_px, + maximum_pose_jump_rad=self.tracking.maximum_pose_jump_rad, + maximum_translation_jump_m=self.tracking.maximum_translation_jump_m, + maximum_tag_tilt_rad=self.tracking.maximum_tag_tilt_rad, + reset_after_seconds=self.tracking.reset_after_seconds, + ) for view in views} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reference_lock.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reference_lock.py new file mode 100644 index 0000000..73b5d4a --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reference_lock.py @@ -0,0 +1,171 @@ +"""Session-scoped fixed-Tag locking shared by every hand profile.""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass +from typing import Mapping, Sequence + +import numpy as np + + +@dataclass(frozen=True) +class ReferenceObservation: + view: str + tag_id: int + corners_px: tuple[tuple[float, float], ...] + + +@dataclass(frozen=True) +class ReferenceMovement: + view: str + tag_id: int + drift_px: float + confirmation_frames: int + + +class ReferenceLock: + """Discard preview data, robustly lock once, then never rewrite it.""" + + def __init__( + self, + fixed_tag_by_view: Mapping[str, int], + *, + minimum_frames: int = 10, + maximum_corner_drift_px: float = 5.0, + confirmation_frames: int = 10, + ) -> None: + if minimum_frames < 3: + raise ValueError("reference lock requires at least three frames") + if maximum_corner_drift_px <= 0.0 or confirmation_frames < 1: + raise ValueError("reference movement limits are invalid") + self.fixed_tag_by_view = { + str(view): int(tag_id) for view, tag_id in fixed_tag_by_view.items() + } + if not self.fixed_tag_by_view: + raise ValueError("at least one fixed reference Tag is required") + self.minimum_frames = int(minimum_frames) + self.maximum_corner_drift_px = float(maximum_corner_drift_px) + self.confirmation_frames = int(confirmation_frames) + self.preview_corners: dict[str, np.ndarray] = {} + self._buffers = { + view: deque(maxlen=max(self.minimum_frames, 30)) + for view in self.fixed_tag_by_view + } + self._references: dict[str, np.ndarray] = {} + self._drift_counts = {view: 0 for view in self.fixed_tag_by_view} + self._latest_drift = {view: 0.0 for view in self.fixed_tag_by_view} + self.session_started = False + self.locking_enabled = False + + @property + def locked(self) -> bool: + return set(self._references) == set(self.fixed_tag_by_view) + + @property + def missing_views(self) -> tuple[str, ...]: + return tuple( + view for view in self.fixed_tag_by_view if view not in self._references + ) + + @property + def references(self) -> Mapping[str, np.ndarray]: + return {view: value.copy() for view, value in self._references.items()} + + @property + def latest_drift_px(self) -> Mapping[str, float]: + return dict(self._latest_drift) + + def begin_session(self) -> None: + """Make all observations collected before Start preview-only.""" + self.session_started = True + self.locking_enabled = False + self._references.clear() + for buffer in self._buffers.values(): + buffer.clear() + for view in self._drift_counts: + self._drift_counts[view] = 0 + self._latest_drift[view] = 0.0 + + def start_locking(self) -> None: + """Start formal collection only after the baseline trajectory ends.""" + if not self.session_started: + raise RuntimeError("the calibration session has not started") + self.locking_enabled = True + self._references.clear() + for buffer in self._buffers.values(): + buffer.clear() + for view in self._drift_counts: + self._drift_counts[view] = 0 + self._latest_drift[view] = 0.0 + + def observe( + self, + view: str, + tag_id: int, + corners_px: Sequence[Sequence[float]], + ) -> ReferenceMovement | None: + selected_view = str(view) + if self.fixed_tag_by_view.get(selected_view) != int(tag_id): + return None + corners = np.asarray(corners_px, dtype=float) + if corners.shape != (4, 2) or not np.all(np.isfinite(corners)): + return None + if not self.session_started: + self.preview_corners[selected_view] = corners.copy() + return None + if not self.locking_enabled: + return None + reference = self._references.get(selected_view) + if reference is None: + buffer = self._buffers[selected_view] + buffer.append(corners.copy()) + if len(buffer) >= self.minimum_frames: + self._references[selected_view] = np.median( + np.asarray(tuple(buffer), dtype=float), axis=0 + ) + buffer.clear() + return None + drift = float(np.max(np.linalg.norm(corners - reference, axis=1))) + self._latest_drift[selected_view] = drift + self._drift_counts[selected_view] = ( + self._drift_counts[selected_view] + 1 + if drift > self.maximum_corner_drift_px + else 0 + ) + if self._drift_counts[selected_view] < self.confirmation_frames: + return None + return ReferenceMovement( + selected_view, + int(tag_id), + drift, + self._drift_counts[selected_view], + ) + + def fingerprint(self) -> dict[str, list[list[float]]]: + if not self.locked: + raise ValueError("fixed references have not been locked") + return { + view: [[float(value) for value in point] for point in corners] + for view, corners in sorted(self._references.items()) + } + + def compatible_with( + self, + fingerprint: Mapping[str, Sequence[Sequence[float]]], + ) -> bool: + if not self.locked or set(fingerprint) != set(self._references): + return False + for view, reference in self._references.items(): + previous = np.asarray(fingerprint[view], dtype=float) + if previous.shape != (4, 2) or not np.all(np.isfinite(previous)): + return False + drift = float( + np.max(np.linalg.norm(reference - previous, axis=1)) + ) + if drift > self.maximum_corner_drift_px: + return False + return True + + +__all__ = ["ReferenceLock", "ReferenceMovement", "ReferenceObservation"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py index c926162..4c111bc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py @@ -1,3 +1,6 @@ -from .codes import CalibrationErrorCode +"""Shared calibration progress and diagnostic messages.""" -__all__ = ["CalibrationErrorCode"] +from .progress_zh import progress_fraction, render_progress_zh +from .reasons_zh import reason_zh + +__all__ = ["progress_fraction", "reason_zh", "render_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py deleted file mode 100644 index 09d16c1..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py +++ /dev/null @@ -1,14 +0,0 @@ -"""Model-independent runtime error categories.""" - -from enum import Enum - - -class CalibrationErrorCode(str, Enum): - PROFILE_INVALID = "profile_invalid" - PREFLIGHT_FAILED = "preflight_failed" - MOTION_FAILED = "motion_failed" - ACQUISITION_FAILED = "acquisition_failed" - TASK_QUALITY_FAILED = "task_quality_failed" - SESSION_SOLVE_FAILED = "session_solve_failed" - URDF_VALIDATION_FAILED = "urdf_validation_failed" - PUBLICATION_FAILED = "publication_failed" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py new file mode 100644 index 0000000..2c0513c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/progress_zh.py @@ -0,0 +1,202 @@ +"""Unified Chinese console progress renderer for every hand profile.""" + +from __future__ import annotations + +from typing import Any, Callable, Mapping + +from ...operator_report import ( + ProgressEstimator, + render_compact_progress_header_zh, +) + + +STATE_LABELS = { + "WAIT_DEVICES": "等待设备反馈和相机内参", + "READY": "设备就绪", + "RUNNING": "标定中", + "FINALIZING": "拟合、验证并生成 URDF", + "PASSED": "通过", + "PAUSED": "已暂停", + "ABORTED": "已中止", +} +PHASE_LABELS = { + "baseline": "安全恢复基准形态", + "preflight": "任务运动预检", + "prepare": "扫描起点准备", + "retry_prepare": "自动重扫起点准备", + "resume_prepare": "断点恢复起点准备", + "sweep": "正式扫描", + "clearance": "手指避让", + "clearance_outer": "小指/无名指避让", + "clearance_splay": "侧摆避让", + "return_splay_zero": "侧摆回零", + "return_middle_open": "展开中指", + "return_outer_open": "展开小指/无名指", +} +DIRECTION_LABELS = {"decreasing": "递减", "increasing": "递增"} + + +def progress_fraction(status: Mapping[str, Any]) -> tuple[float, int, int]: + state = str(status.get("state", "")) + step_count = max(0, int(status.get("step_count", 0) or 0)) + step_index = int(status.get("step_index", -1) or 0) + step_fraction = float(status.get("step_fraction", 0.0) or 0.0) + if state == "PASSED": + overall = 1.0 + elif step_count and step_index >= 0: + overall = min(1.0, max(0.0, (step_index + step_fraction) / step_count)) + else: + overall = 0.0 + current = min(step_count, max(0, step_index + 1)) if step_count else 0 + return overall, current, step_count + + +def _duration_zh(seconds: float | None) -> str: + if seconds is None or seconds < 0.0: + return "计算中" + value = int(round(seconds)) + return f"{value // 60}分{value % 60:02d}秒" + + +def render_progress_zh( + status: Mapping[str, Any], + *, + task_labels: Mapping[str, str], + reason_renderer: Callable[ + [Mapping[str, Any]], tuple[str, str, str] + ], + estimator: ProgressEstimator | None = None, +) -> str: + """Render the shared progress/status contract with model labels injected.""" + state = str(status.get("state", "")) + overall, _current_step, _step_count = progress_fraction(status) + task = status.get("task_name") + phase = status.get("phase") + if task: + task_text = task_labels.get(str(task), str(task)) + elif phase == "baseline": + task_text = "全手基准姿态" + elif state == "WAIT_DEVICES": + task_text = "等待设备连接" + elif state == "READY": + task_text = "等待开始" + else: + task_text = "无" + eta = _duration_zh(estimator.remaining(overall) if estimator else None) + cycle = status.get("cycle") + cycle_text = "-" if cycle is None else str(int(cycle) + 1) + direction_text = DIRECTION_LABELS.get(str(status.get("direction")), "-") + command_unit = str(status.get("command_unit", "u8")) + target = ( + status.get("target_rad") + if command_unit == "rad" + else status.get("target_u8") + ) + requested = status.get("current_command_u8", target) + actual = status.get("actual_u8") + if command_unit == "rad": + requested = "未知" if requested is None else f"{float(requested):.3f} rad" + actual_text = "未知" if actual is None else f"{float(actual):.3f} rad" + else: + requested = "未知" if requested is None else f"{float(requested):.1f}" + actual_text = "未知" if actual is None else f"{float(actual):.1f}" + valid = int(status.get("valid_frames", 0) or 0) + total = int(status.get("total_frames", 0) or 0) + rate = float(status.get("tag_detection_rate", 0.0) or 0.0) + joint_rate = float( + status.get( + "joint_frame_rate", (float(valid) / total) if total else 0.0 + ) or 0.0 + ) + recognized = [int(value) for value in status.get("recognized_tag_ids", [])] + unrecognized = [ + int(value) for value in status.get("unrecognized_tag_ids", []) + ] + if str(status.get("reason", "")).startswith("locking_fixed_base_references"): + unrecognized = [ + int(value) + for value in status.get("reference_missing_tag_ids", unrecognized) + ] + recognized_text = "/".join(f"ID{value}" for value in recognized) or "无" + unrecognized_text = "/".join(f"ID{value}" for value in unrecognized) or "无" + attempt = int(status.get("attempt", 1) or 1) + lines = render_compact_progress_header_zh( + serial_number=str(status.get("serial_number", "?")), + progress=overall, + eta=eta, + stage=PHASE_LABELS.get(str(phase), STATE_LABELS.get(state, state)), + cycle=cycle_text, + repetitions=4, + task=task_text, + requested=requested, + actual=actual_text, + direction=direction_text, + tag_status=( + f"已识别 {recognized_text}；未识别/不合格 {unrecognized_text}；" + f"本方向各Tag最低 {rate:.1%}；联合 {valid}/{total} 帧" + f"（{joint_rate:.1%}）" + ), + ready_cameras=3, + feedback_hz=float(status.get("feedback_hz", 0.0) or 0.0), + valid_frames=valid, + automatic_retry_count=max(0, attempt - 1), + ) + required_by_view = status.get("required_tag_ids_by_view", {}) + if isinstance(required_by_view, Mapping) and required_by_view: + lines.append( + "任务所需Tag：" + "；".join( + f"{view}=" + "/".join(f"ID{int(tag_id)}" for tag_id in ids) + for view, ids in required_by_view.items() + ) + ) + speed_u8 = status.get("speed_u8") + if speed_u8 is not None: + seconds = float( + status.get("command_trajectory_full_range_seconds", 0.0) or 0.0 + ) + lines.append(f"运动：速度档 {int(speed_u8)}；全行程 {seconds:.1f} 秒余弦轨迹") + speed_rad_s = status.get("speed_rad_s") + if speed_rad_s is not None: + seconds = float( + status.get("command_trajectory_duration_seconds", 0.0) or 0.0 + ) + lines.append( + f"运动：峰值 {float(speed_rad_s):.3f} rad/s；" + f"本段 {seconds:.1f} 秒平滑限速轨迹" + ) + 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) + 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):.{digits}f}{suffix}" + for name, value in zip(command_names, latest_state) + ) + error_channel = status.get("maximum_error_channel") + maximum_error = status.get(f"maximum_error_{command_unit}") + error_text = ( + "未知" + if error_channel is None or maximum_error is None + else f"{error_channel}={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}" + ) + if state in {"PAUSED", "ABORTED"}: + _code, problem, suggestion = reason_renderer(status) + lines.extend((f"原因：{problem}", f"建议：{suggestion}")) + if status.get("reference_locked"): + lines.append("基准已锁定：请勿移动手掌、相机、支架或Tag") + return "\n".join(lines) + + +__all__ = ["progress_fraction", "render_progress_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py new file mode 100644 index 0000000..c1bd9ed --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -0,0 +1,168 @@ +"""Shared Chinese operator explanations for common calibration failures.""" + +from __future__ import annotations + +import re +from typing import Any, Mapping + + +def reason_zh( + status: Mapping[str, Any], *, model_name: str +) -> tuple[str, str, str]: + """Map a common runtime reason to a stable code, explanation and action.""" + reason = str(status.get("reason", "unknown")) + if "compact_byte_requires_measured_full_command_domain" in reason: + return ("FIT-COMPACT-DOMAIN-507", "实测指令范围没有完整覆盖 0–255，不能生成完整的 256 项查表。", + "核对扫描范围和端点稳态数据；不得用反馈表、外推或裁剪补齐缺失指令。") + if "compact_command_table_holdout_failed" in reason: + return ("FIT-COMPACT-HOLDOUT-508", "精简单张指令查表未通过独立视觉验收，已禁止发布。", + "查看具体角度和空间误差；若回差过大，单张表不能准确表达实机，不自动回退或重新调参。") + if "steady_" in reason and ("failed" in reason or "incomplete" in reason or "missing" in reason): + return ("FIT-COMMAND-MAPPING-506", "稳态指令与独立视觉观测不足或不一致，已禁止发布指令映射。", + "查看具体关节、方向、稳态点及诊断；反馈曲线不能替代缺失的指令标定。") + live_failures = { + "duplicate_controller": ("DEVICE-COMMAND-CONFLICT-204", "检测到另一个命令控制节点。", "退出 GUI 或其他控制节点，只保留本次标定。"), + "fixed_reference_moved": ("OBS-BASE-DRIFT-105", "已锁定的固定基准连续 10 帧漂移超过 5 px。", "检查手掌、相机、支架和固定 Tag；重新固定后开始新采集。"), + "sdk_disconnected": ("DEVICE-COMMUNICATION-202", "SDK 通信或健康报告持续失效。", "检查供电、连接及 SDK 日志；不要直接重复强推动作。"), + "wrong_control_mode": ("DEVICE-MODE-205", "SDK 未确认位置控制模式。", "确认 POSITION 模式，退出竞争控制进程后重新开始。"), + "hardware_fault": ("DEVICE-ACTIVE-FAULT-201", "SDK 报告活动硬件故障。", "根据下方原始错误码检查机械阻挡、温度和通信。"), + "feedback_stale": ("DEVICE-FEEDBACK-203", "SDK 反馈连续超过 1 秒未更新。", "检查 SDK 回读和总线；终端进度不变不等于反馈失联。"), + "physical_range_exceeded": ("DEVICE-PHYSICAL-RANGE-204", "命令或反馈越过声明的物理范围。", "核对通道、单位、编码器范围；不要通过扩大边界掩盖异常。"), + "protected_camera_model_changed": ("OBS-CAMERA-MODEL-106", "开始后相机投影模型或图像尺寸发生变化。", "中止后重新确认相机参数；涉及机位关系变化时重标外参。"), + "finalization_protocol_error": ("FIT-PROTOCOL-502", "拟合与发布阶段顺序异常，已禁止发布。", "保留当前会话和日志，检查后台验收的完整诊断。"), + "invalid_motion_target": ("MOTION-CONTRACT-304", "运动段缺少有效的反馈推进目标。", "检查 Profile 和轨迹诊断；不要继续发送未经验证的运动。"), + } + if reason.split(":", 1)[0] in live_failures: + return live_failures[reason.split(":", 1)[0]] + runner_failures = { + "calibration_node_process_exited": "标定状态发布进程已退出，不能继续本次采集。", + "calibration_stack_exited": "标定 ROS 进程栈已退出。", + "calibration_node_initial_status_timeout": "启动后未收到标定节点状态；这不是机械堵转诊断。", + "calibration_device_not_ready": "设备尚未满足启动条件，未自动开始任务运动。", + "calibration_start_service_unavailable": "设备报告就绪，但标定开始服务不可用。", + "calibration_start_response_timeout": "开始请求未收到确认，不能判断任务是否已启动；运行栈将退出。", + "calibration_start_rejected": "标定节点拒绝了开始请求。", + "ros_context_shutdown": "ROS 通信上下文已关闭。", + "independent_release_evidence_missing": "本次数据已通过旧格式会话校验，但缺少该格式要求的独立发布证据。", + "existing_calibration_publishers": "检测到已有 SDK 或控制进程，不能重复启动当前标定栈。", + } + if reason.split(":", 1)[0] in runner_failures: + return ( + "RUNTIME-LIFECYCLE-500", runner_failures[reason.split(":", 1)[0]], + "查看原始原因、runner_diagnostic.json 和 calibration.log；确认进程及设备状态后再启动。", + ) + # Legacy transport spellings are decoded here, not in a product runner. + if reason.startswith(("o12_feedback_stream_timeout", "feedback_stream_timeout")): + return ("DEVICE-FEEDBACK-203", "SDK 反馈连续超过 1 秒未更新。", + "检查供电、SDK 和实际总线连接；不要把终端进度不变当成反馈失联。") + if reason.startswith("o12_active_motor_fault"): + return ("DEVICE-ACTIVE-FAULT-201", f"SDK 返回活动电机故障：{status.get('error_faults', reason)}。", + "检查对应电机错误码、机械阻挡和温度；排除活动故障后再恢复。") + if reason.startswith("o12_new_communication_fault"): + return ("DEVICE-COMMUNICATION-202", f"SDK 运行中报告新的通信故障：{status.get('error_faults', reason)}。", + "检查 HCAN 连接、供电和相关电机通信。") + if reason.startswith(("feedback_outside_registered_feedback_domain", "command_outside_physical_range")): + return ("DEVICE-PHYSICAL-RANGE-204", f"命令或反馈超出登记的物理范围：{reason}。", + "核对通道、单位和硬件限位；不得扩大软件边界来掩盖异常。") + if reason.startswith(("mapping_preflight_no_target_motion", "mapping_preflight_wrong_feedback_direction")): + return ("DEVICE-MAPPING-301", f"固定通道点动未证实要求的目标反馈运动或方向：{reason}。", + "检查 SDK 通道与机械运动；不要自动交换映射或增加强推幅度。") + if "standard_urdf_mimic_not_expressive" in reason: + return ( + "FIT-STANDARD-MIMIC-504", + "独立视觉数据不能由同一条标准 URDF 线性 mimic 在规定精度内解释，已禁止发布。", + "检查 Tag 刚性和重复性；若机构确为非线性，应明确报告标准 URDF 表达能力不足，不能用 JSON 或端点比例补偿。", + ) + if "corrected range exceeds source mechanical range" in reason or "URDF mimic reachable range exceeds limits" in reason: + return ( + "URDF-PHYSICAL-RANGE-505", + f"修正范围或递归 mimic 与原始 CAD 机械限位不一致：{reason}。", + "保留原始 CAD 和测量数据，核实限位及坐标定义；不要扩大限位或裁剪曲线来制造通过结果。", + ) + if reason.startswith("fixed_base_tag_moved:"): + fields = reason.split(":") + view = fields[1] if len(fields) > 1 else "unknown" + view_label = str(status.get("view_label", view)) + match = re.search(r"drift_px=([0-9.]+)", reason) + drift = match.group(1) if match else "未知" + limit = float(status.get("base_corner_drift_limit_px", 5.0) or 5.0) + return ( + "OBS-BASE-DRIFT-105", + f"{view_label}机位的固定基准 Tag 连续漂移，最大角点位移 " + f"{drift} px，超过 {limit:g} px。", + "检查手掌支架、相机和固定基准 Tag 是否松动或被碰触；" + "固定后重新开始。程序已禁止发布本次结果。", + ) + if reason.startswith("sweep_quality_failed:"): + details = reason.split(":", 2)[-1] + return ( + "OBS-SWEEP-QUALITY-104", + "当前方向经过自动重扫后仍未满足采集门限；具体未通过项：" + f"{details}。", + "查看会话诊断中的有效样本、覆盖、分箱和数据空白；" + "处理持续遮挡或同步问题后从断点继续。", + ) + if reason.startswith("multiple_state_publishers:"): + count = status.get("state_publisher_count", "?") + return ( + "DEVICE-DUPLICATE-SDK-203", + f"检测到 {count} 个 {model_name} 状态发布者，可能已有 SDK/GUI 未退出。", + "停止单独启动的 SDK 和 GUI，只保留本次标定自动拉起的进程。", + ) + if reason.startswith("multiple_command_publishers:"): + count = status.get("command_publisher_count", "?") + return ( + "DEVICE-COMMAND-CONFLICT-204", + f"检测到 {count} 个 {model_name} 命令发布者。", + "停止 GUI、手动控制节点或其他标定进程，只保留当前标定命令。", + ) + if reason.startswith("mechanical_stall:"): + return ( + "MOTION-STALL-303", + "目标电机连续两秒没有向目标推进，程序已保持当前位置。", + "检查碰撞、摩擦和机械端点；不要连续重启强推。", + ) + if reason.startswith("motion_timeout:"): + return ( + "MOTION-TIMEOUT-302", + "当前运动在规定时间内没有完成。", + "检查 SDK 反馈、电机状态和机械阻挡后重新开始。", + ) + if reason.startswith("fit_or_publication_failed:"): + if "mimic_residual_exceeds:" in reason or "coupling_residual_exceeds:" in reason: + joint = re.search(r"joint=([^:]+)", reason) + model = re.search(r"model=([^:]+)", reason) + multiplier = re.search(r"(?:multiplier|linear_term)=([0-9.]+)", reason) + p95 = re.search(r"p95_deg=([0-9.]+)", reason) + maximum = re.search(r"maximum_deg=([0-9.]+)", reason) + return ( + "FIT-MIMIC-503", + f"{joint.group(1) if joint else '未知关节'} 的 " + f"{model.group(1) if model else 'linear_mimic'} " + "耦合模型未达到精度门限：" + f"线性项 {multiplier.group(1) if multiplier else '未知'}，" + f"P95={p95.group(1) if p95 else '未知'}°、" + f"最大={maximum.group(1) if maximum else '未知'}°。", + "原始视觉曲线已保留；请检查 Tag 刚性、遮挡与机械重复性，" + "不要放宽门限或发布错误 URDF。", + ) + detail = reason.split(":", 1)[1] if ":" in reason else "未知" + return ( + "FIT-PUBLISH-501", + f"拟合、holdout 或 URDF 安全写回失败：{detail}。", + "保留本会话和源 URDF；查看诊断日志。", + ) + if reason.startswith("operator_abort"): + return ( + "OPERATOR-ABORT-001", + "操作员主动中止了本次标定。", + "排除现场问题后重新开始。", + ) + return ( + "CALIBRATION-500", + f"{model_name} 标定停止，原始原因：{reason}。", + "保留会话目录和运行日志，并复制诊断信息给开发者。", + ) + + +__all__ = ["reason_zh"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py new file mode 100644 index 0000000..4c63766 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/resume.py @@ -0,0 +1,324 @@ +"""Reference- and input-bound checkpoint compatibility for every model.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import json +import math +from pathlib import Path +import re +from typing import Any, Mapping, Sequence + +import numpy as np + +from .engine import ACQUISITION_POLICY_VERSION + + +def discover_resume_candidate( + session_root: Path, *, profile_id: str, serial_number: str, + protected_hashes: Mapping[str, str], +) -> Path | None: + """Locate evidence, never authorize its reuse or publish from a checkpoint. + + The online owner must still verify the current physical reference and every + reused Tag installation. A newer empty/truncated attempt cannot shadow an + older complete capture. Old source files are never modified. + """ + if not protected_hashes or any(re.fullmatch(r"[0-9a-f]{64}", value) is None + for value in protected_hashes.values()): + raise ValueError("resume discovery requires valid protected hashes") + root = session_root.resolve() + if not root.is_dir(): + return None + released = { + pointer.resolve() for pointer in root.iterdir() + if pointer.name.startswith("latest_") and "passed" in pointer.name and pointer.is_symlink() + } + model, side, layout, _revision = profile_id.split("/") + for candidate in sorted(root.iterdir(), key=lambda path: path.name, reverse=True): + if candidate.is_symlink() or not candidate.is_dir() or candidate.name.startswith("latest_"): + continue + if candidate in released or any(candidate.name <= path.name for path in released if path.parent == root): + continue + start = None + reference_found = False + completed_found = False + try: + summary_path = candidate / "calibration_summary_zh.json" + if summary_path.exists(): + summary = json.loads(summary_path.read_text(encoding="utf-8")) + if not isinstance(summary, Mapping) or summary.get("result") != "FAIL": + # A successful capture awaiting a separate release proof + # is not a failed checkpoint. Reusing it cannot create an + # independent second calibration session. + continue + with (candidate / "raw_samples.jsonl").open(encoding="utf-8") as stream: + for line in stream: + if not line.strip(): + continue + row = json.loads(line) + if not isinstance(row, dict): + raise ValueError("invalid checkpoint record") + kind = str(row.get("kind", "")) + if kind == "session_start": + if start is not None: + raise ValueError("multiple checkpoint session headers") + start = row + if kind == "fixed_base_reference_locked": + reference_found = True + if kind == "scan_unit_complete" or kind.endswith("sweep_observation_quality"): + # Old direction records used an explicit empty failure + # list instead of a boolean. This merely finds a source; + # the node still validates every imported unit/view. + passed = row.get("passed") is True or ( + "passed" not in row and row.get("failures") == [] + and int(row.get("valid_frames", 0)) >= 40 + and int(row.get("feedback_bins", 0)) >= 32 + ) + completed_found = completed_found or passed + except (OSError, ValueError): + continue + if start is None or not reference_found or not completed_found: + continue + recorded_id = start.get("profile_id") + if recorded_id is None: + # Read-only legacy journal spelling; the profile hash remains + # mandatory and identifies the exact YAML contract. + if (start.get("model"), start.get("hand_type"), start.get("tag_layout")) != (model, side, layout): + continue + elif recorded_id != profile_id: + continue + if start.get("serial_number", root.name) != serial_number: + continue + if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION: + continue + recorded = start.get("protected_hashes", start) + if not isinstance(recorded, Mapping) or any(recorded.get(key) != value for key, value in protected_hashes.items()): + continue + return candidate + return None + + +@dataclass(frozen=True) +class TagPoseFingerprint: + rotation_xyzw: tuple[float, float, float, float] + translation_xyz_m: tuple[float, float, float] + + +@dataclass(frozen=True) +class ResumeFingerprint: + profile_id: str + protected_hashes: Mapping[str, str] + fixed_corners_by_view: Mapping[str, tuple[tuple[float, float], ...]] + fixed_poses: Mapping[str, TagPoseFingerprint] = field(default_factory=dict) + moving_tag_poses: Mapping[str, TagPoseFingerprint] = field(default_factory=dict) + acquisition_policy_version: str = ACQUISITION_POLICY_VERSION + + +@dataclass(frozen=True) +class ResumeDecision: + reuse: bool + reason: str + incompatible_fields: tuple[str, ...] = () + completed_units: tuple[tuple[str, int, str], ...] = () + + +def select_installation_evidence(previous, current, required_roles): + """Compare a reused Tag at the SAME baseline or named verification pose. + + Unrelated Tags do not authorize reuse, and a missing fingerprint cannot be + replaced with another pose's installation. Do not mutate either journal. + """ + from dataclasses import replace + old, new = {}, {} + shared = previous.moving_tag_poses.keys() & current.moving_tag_poses.keys() + for role in required_roles: + candidates = sorted(key for key in shared if key == role or key.endswith("/"+role)) + if not candidates: + continue # ResumeVerifier will reject the missing required role. + key = role if role in candidates else candidates[0] + old[role], new[role] = previous.moving_tag_poses[key], current.moving_tag_poses[key] + return replace(previous, moving_tag_poses=old), replace(current, moving_tag_poses=new) + + +class ResumeVerifier: + """A mismatch starts fresh; it is never a live safety pause.""" + + def __init__( + self, + *, + maximum_corner_drift_px: float = 5.0, + maximum_rotation_rad: float = math.radians(2.0), + maximum_translation_m: float = 0.005, + required_fixed_views: Sequence[str] = (), + required_fixed_poses: Sequence[str] = (), + required_moving_poses: Sequence[str] = (), + required_hashes: Sequence[str] = (), + ) -> None: + self.maximum_corner_drift_px = float(maximum_corner_drift_px) + self.maximum_rotation_rad = float(maximum_rotation_rad) + self.maximum_translation_m = float(maximum_translation_m) + if not all(math.isfinite(value) and value > 0 for value in ( + self.maximum_corner_drift_px, self.maximum_rotation_rad, self.maximum_translation_m + )): + raise ValueError("resume tolerances must be finite and positive") + self.required_fixed_views = frozenset(required_fixed_views) + self.required_fixed_poses = frozenset(required_fixed_poses) + self.required_moving_poses = frozenset(required_moving_poses) + self.required_hashes = frozenset(required_hashes) + + def compare( + self, previous: ResumeFingerprint, current: ResumeFingerprint + ) -> ResumeDecision: + failures: list[str] = [] + for label, fingerprint in (("previous", previous), ("current", current)): + if not fingerprint.profile_id: + failures.append(f"{label}.profile_id") + if not fingerprint.protected_hashes or not self.required_hashes <= fingerprint.protected_hashes.keys(): + failures.append(f"{label}.required_hashes") + if any(re.fullmatch(r"[0-9a-f]{64}", str(value)) is None for value in fingerprint.protected_hashes.values()): + failures.append(f"{label}.invalid_hash") + if not fingerprint.fixed_corners_by_view or not self.required_fixed_views <= fingerprint.fixed_corners_by_view.keys(): + failures.append(f"{label}.required_fixed_views") + if not fingerprint.fixed_poses or not self.required_fixed_poses <= fingerprint.fixed_poses.keys(): + failures.append(f"{label}.required_fixed_poses") + if not self.required_moving_poses <= fingerprint.moving_tag_poses.keys(): + failures.append(f"{label}.required_moving_poses") + if previous.acquisition_policy_version != ACQUISITION_POLICY_VERSION: + failures.append("acquisition_policy_version") + if current.acquisition_policy_version != ACQUISITION_POLICY_VERSION: + failures.append("current_acquisition_policy_version") + if previous.profile_id != current.profile_id: + failures.append("profile_id") + if dict(previous.protected_hashes) != dict(current.protected_hashes): + failures.append("protected_hashes") + self._compare_corners(previous, current, failures) + self._compare_poses( + "fixed_pose", previous.fixed_poses, current.fixed_poses, failures + ) + self._compare_poses( + "moving_tag_pose", + previous.moving_tag_poses, + current.moving_tag_poses, + failures, + ) + if failures: + return ResumeDecision( + False, + "基准变化，已放弃旧断点并重新采集", + tuple(sorted(set(failures))), + ) + return ResumeDecision(True, "断点基准和 Tag 安装关系验证通过") + + def _compare_corners( + self, + previous: ResumeFingerprint, + current: ResumeFingerprint, + failures: list[str], + ) -> None: + if set(previous.fixed_corners_by_view) != set( + current.fixed_corners_by_view + ): + failures.append("fixed_reference_views") + return + for view, previous_corners in previous.fixed_corners_by_view.items(): + old = np.asarray(previous_corners, dtype=float) + new = np.asarray(current.fixed_corners_by_view[view], dtype=float) + if old.shape != (4, 2) or new.shape != (4, 2): + failures.append(f"fixed_corners[{view}]") + continue + drift = float(np.max(np.linalg.norm(old - new, axis=1))) + if not math.isfinite(drift) or drift > self.maximum_corner_drift_px: + failures.append(f"fixed_corners[{view}]") + + def _compare_poses( + self, + prefix: str, + previous: Mapping[str, TagPoseFingerprint], + current: Mapping[str, TagPoseFingerprint], + failures: list[str], + ) -> None: + if set(previous) != set(current): + failures.append(f"{prefix}s") + return + for name, old in previous.items(): + new = current[name] + old_translation = np.asarray(old.translation_xyz_m, dtype=float) + new_translation = np.asarray(new.translation_xyz_m, dtype=float) + if (old_translation.shape != (3,) or new_translation.shape != (3,) + or not np.all(np.isfinite(old_translation)) or not np.all(np.isfinite(new_translation))): + failures.append(f"{prefix}[{name}].translation") + continue + translation = float( + np.linalg.norm( + old_translation - new_translation + ) + ) + rotation = _quaternion_distance_rad( + old.rotation_xyzw, new.rotation_xyzw + ) + if translation > self.maximum_translation_m: + failures.append(f"{prefix}[{name}].translation") + if rotation > self.maximum_rotation_rad: + failures.append(f"{prefix}[{name}].rotation") + + +def _quaternion_distance_rad( + left: Sequence[float], right: Sequence[float] +) -> float: + first = np.asarray(tuple(left), dtype=float) + second = np.asarray(tuple(right), dtype=float) + if first.shape != (4,) or second.shape != (4,): + return math.inf + if not np.all(np.isfinite(first)) or not np.all(np.isfinite(second)): + return math.inf + first_norm = float(np.linalg.norm(first)) + second_norm = float(np.linalg.norm(second)) + if first_norm <= 0.0 or second_norm <= 0.0: + return math.inf + dot = abs(float(np.dot(first / first_norm, second / second_norm))) + return 2.0 * math.acos(min(1.0, max(-1.0, dot))) + + +def fingerprint_from_mapping(payload: Mapping[str, Any]) -> ResumeFingerprint: + """Strictly decode persisted v2 data; incomplete v1 data cannot be reused.""" + + def poses(key: str) -> dict[str, TagPoseFingerprint]: + return { + str(name): TagPoseFingerprint( + tuple(float(value) for value in value["rotation_xyzw"]), + tuple(float(value) for value in value["translation_xyz_m"]), + ) + for name, value in dict(payload.get(key, {})).items() + } + + return ResumeFingerprint( + profile_id=str(payload.get("profile_id", "")), + protected_hashes={ + str(key): str(value) + for key, value in dict(payload.get("protected_hashes", {})).items() + }, + fixed_corners_by_view={ + str(view): tuple( + tuple(float(coordinate) for coordinate in point) + for point in corners + ) + for view, corners in dict( + payload.get("fixed_corners_by_view", {}) + ).items() + }, + fixed_poses=poses("fixed_poses"), + moving_tag_poses=poses("moving_tag_poses"), + acquisition_policy_version=str( + payload.get("acquisition_policy_version", "") + ), + ) + + +__all__ = [ + "ResumeDecision", + "ResumeFingerprint", + "ResumeVerifier", + "TagPoseFingerprint", + "fingerprint_from_mapping", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/__init__.py new file mode 100644 index 0000000..aa55c25 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/__init__.py @@ -0,0 +1 @@ +"""Thin ROS interfaces for the single calibration session engine.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py new file mode 100644 index 0000000..eab7d6f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/calibration_node.py @@ -0,0 +1,104 @@ +"""ROS identity, parameters and callback wiring for the shared coordinator.""" + +import time + +from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CameraInfo, JointState +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ..adapters.ros_binding import SdkBindingPorts, bind_ros_sdk +from ..coordinator import CalibrationCoordinator +from ..inputs import RuntimePorts +from .io import RosCalibrationIO, _stamp_ns, camera_input, detection_input +from .parameters import load_runtime_parameters, parameter_defaults + + +class UnifiedCalibrationNode(Node): + def __init__(self, profile): + super().__init__(f"{profile.key.model.lower()}_calibration") + for name, default in parameter_defaults(profile).items(): + self.declare_parameter(name, default) + parameters = load_runtime_parameters(profile, lambda name: self.get_parameter(name).value) + self.motion_callback_group = MutuallyExclusiveCallbackGroup() + self.vision_callback_groups = { + view: MutuallyExclusiveCallbackGroup() for view in profile.vision.view_names} + self.io = RosCalibrationIO(self, profile, parameters) + ports = RuntimePorts(lambda: int(self.get_clock().now().nanoseconds), time.monotonic, + lambda: self.count_publishers(parameters.command_topic), + self.io.publish_position, self.io.publish_setting) + + def subscribe_health(receive): + self.create_subscription(String, + parameters.command_topic.rsplit("/", 1)[0]+"/calibration_health", + lambda message: receive(message.data), 10, callback_group=self.motion_callback_group) + + def adapter_factory(publish, set_speed, feedback_fresh): + return bind_ros_sdk(profile, SdkBindingPorts(feedback_fresh, ports.monotonic, subscribe_health), + publish=publish, set_speed=set_speed) + + self.coordinator = CalibrationCoordinator(profile, parameters, ports, adapter_factory) + self.create_subscription(JointState, parameters.state_topic, self._state_callback, 30, + callback_group=self.motion_callback_group) + for view in profile.vision.view_names: + self.create_subscription(CameraInfo, parameters.camera_info_topics[view], + lambda message, selected=view: self._camera_info_callback(selected, message), + qos_profile_sensor_data, callback_group=self.vision_callback_groups[view]) + self.create_subscription(AprilTagDetectionArray, parameters.detection_topics[view], + lambda message, selected=view: self._detections_callback(selected, message), + qos_profile_sensor_data, callback_group=self.vision_callback_groups[view]) + self.create_service(Trigger, f"{profile.namespace}/start", self._start, + callback_group=self.motion_callback_group) + self.create_service(Trigger, f"{profile.namespace}/abort", self._abort, + callback_group=self.motion_callback_group) + self.create_timer(1.0/parameters.command_rate_hz, self._tick, + callback_group=self.motion_callback_group) + self.create_timer(0.5, self._publish_status) + + def _start(self, _request, response): + result = self.coordinator.start() + response.success, response.message = result.success, result.message + return response + + def _abort(self, _request, response): + result = self.coordinator.abort() + response.success, response.message = result.success, result.message + return response + + def _state_callback(self, message): + self.coordinator.receive_feedback(tuple(message.name), tuple(message.position), + _stamp_ns(message.header.stamp)) + + def _camera_info_callback(self, view, message): + self.coordinator.receive_camera_info(view, camera_input(message)) + + def _detections_callback(self, view, message): + self.coordinator.receive_detections(detection_input(view, message)) + + def _tick(self): + self.coordinator.tick() + + def _publish_status(self): + self.io.publish_status(self.coordinator.snapshot()) + + def destroy_node(self): + self.coordinator.close() + return super().destroy_node() + + +def run_profile_node(profile, args=None): + import rclpy + rclpy.init(args=args) + node = UnifiedCalibrationNode(profile) + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + try: + executor.spin() + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown() diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/entrypoint.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/entrypoint.py new file mode 100644 index 0000000..62c11bd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/entrypoint.py @@ -0,0 +1,38 @@ +"""Protected Profile handoff to the online execution implementation.""" + +from __future__ import annotations + +import argparse +import hashlib +from pathlib import Path +import sys + +from ...core import ProfileKey +from ...profiles import load_hand_profile + + +def run_profile_node(profile, args): + from .calibration_node import run_profile_node as run + return run(profile, args) + + +def main(args: list[str] | None = None) -> None: + arguments = list(sys.argv[1:] if args is None else args) + selector = argparse.ArgumentParser(add_help=False) + selector.add_argument("--profile-id", default=None) + selector.add_argument("--profile-config", default=None) + selector.add_argument("--profile-sha256", default=None) + selected, remaining = selector.parse_known_args(arguments) + if not all((selected.profile_id, selected.profile_config, selected.profile_sha256)): + raise ValueError("online calibration requires a protected YAML Profile; use calibrate_hand --config") + key = ProfileKey.parse(selected.profile_id) + path = Path(selected.profile_config) + def verify_hash(): + if hashlib.sha256(path.read_bytes()).hexdigest() != selected.profile_sha256: + raise ValueError("Profile changed between launch and calibration-node startup") + verify_hash() + declared = load_hand_profile(path) + verify_hash() + if declared.key != key: + raise ValueError("protected YAML Profile identity differs from --profile-id") + return run_profile_node(declared, remaining) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py new file mode 100644 index 0000000..edba2c1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/io.py @@ -0,0 +1,59 @@ +"""ROS message conversion and publication; no calibration business state.""" + +from __future__ import annotations + +import json + +from sensor_msgs.msg import JointState +from std_msgs.msg import String + +from ..inputs import CameraModelInput, DetectionInput, TagDetection +from ..status import normalize_status + + +def _stamp_ns(stamp) -> int: + return int(stamp.sec)*1_000_000_000 + int(stamp.nanosec) + + +def camera_input(message) -> CameraModelInput: + return CameraModelInput(int(message.width), int(message.height), + tuple(message.k), tuple(message.d), tuple(message.r), tuple(message.p)) + + +def detection_input(view: str, message) -> DetectionInput: + return DetectionInput(view, _stamp_ns(message.header.stamp), tuple( + TagDetection(int(tag.id), tuple((float(p.x), float(p.y)) for p in tag.corners), + int(tag.hamming), float(tag.decision_margin)) for tag in message.detections)) + + +class RosCalibrationIO: + """Explicit transport object composed by UnifiedCalibrationNode.""" + + def __init__(self, node, profile, parameters): + self.profile = profile + self.serial_number = parameters.serial_number + self.clock = node.get_clock() + self.command_publisher = node.create_publisher(JointState, parameters.command_topic, 10) + self.setting_publisher = node.create_publisher(String, parameters.setting_topic, 10) + self.status_publisher = node.create_publisher(String, f"{profile.namespace}/status", 10) + + def publish_position(self, values) -> None: + message = JointState() + message.header.stamp = self.clock.now().to_msg() + message.name = [] if self.profile.command.feedback_by_index else list(self.profile.command.names) + message.position = list(values) + self.command_publisher.publish(message) + + def publish_setting(self, payload) -> None: + message = String() + message.data = json.dumps(payload) + self.setting_publisher.publish(message) + + def publish_status(self, snapshot) -> None: + message = String() + message.data = json.dumps(normalize_status(snapshot.as_dict(), + profile=self.profile, serial_number=self.serial_number), ensure_ascii=False) + self.status_publisher.publish(message) + + +__all__ = ["RosCalibrationIO"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py new file mode 100644 index 0000000..7ec295e --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py @@ -0,0 +1,160 @@ +"""Declare and convert ROS parameters into explicit coordinator inputs.""" + +from __future__ import annotations + +import math +from pathlib import Path +from typing import Any, Callable + +from ...core.geometry.extrinsics import load_camera_extrinsics +from ...core.geometry.pnp import DEFAULT_POSE_TRACKING_PARAMETERS, PoseTrackingParameters +from ..parameters import DetectionPolicy, RuntimeParameters + + +def parameter_defaults(profile) -> dict[str, Any]: + model = profile.key.model.lower() + namespace = profile.namespace + speed_parameters = profile.motion.speed_parameters + defaults: dict[str, Any] = { + "serial_number": "UNSET", + "session_dir": "", + "source_urdf_path": "", + "source_urdf_expected_sha256": "", + "camera_extrinsics_file": "", + "camera_extrinsics_expected_sha256": "", + "calibration_config_expected_sha256": "", + "tag_config_expected_sha256": "", + "sdk_config_expected_sha256": "", + "sdk_package_expected_sha256": "", + "profile_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", + "commands_enabled": True, + "speed_settle_seconds": float( + speed_parameters.get("speed_settle_seconds", 0.2) + ), + "torque_u8": int(speed_parameters.get("torque_u8", 80)), + "maximum_state_image_skew_ms": 50.0, + "maximum_hamming": 0, + "minimum_decision_margin": 30.0, + "minimum_edge_pixels": 30.0, + "tag_size_m": 0.016, + "pnp_maximum_reprojection_error_px": DEFAULT_POSE_TRACKING_PARAMETERS.maximum_reprojection_error_px, + "pnp_reprojection_tie_px": DEFAULT_POSE_TRACKING_PARAMETERS.reprojection_tie_px, + "pnp_maximum_pose_jump_deg": math.degrees(DEFAULT_POSE_TRACKING_PARAMETERS.maximum_pose_jump_rad), + "pnp_maximum_translation_jump_m": DEFAULT_POSE_TRACKING_PARAMETERS.maximum_translation_jump_m, + "pnp_maximum_tag_tilt_deg": math.degrees(DEFAULT_POSE_TRACKING_PARAMETERS.maximum_tag_tilt_rad), + "pnp_tracker_reset_seconds": DEFAULT_POSE_TRACKING_PARAMETERS.reset_after_seconds, + "command_rate_hz": float(speed_parameters.get("command_rate_hz", 100.0)), + } + for view in profile.vision.view_names: + defaults[f"{view}_camera_info_topic"] = ( + f"{namespace}/{view}/camera/camera_info" + ) + defaults[f"{view}_detections_topic"] = ( + f"{namespace}/{view}/apriltag/detections" + ) + return defaults + + +def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimeParameters: + serial_number = str(value("serial_number")) + session_dir = Path(str(value("session_dir"))).expanduser().resolve() + source_urdf = Path(str(value("source_urdf_path"))).expanduser().resolve() + camera_extrinsics_file = Path( + str(value("camera_extrinsics_file")) + ).expanduser().resolve() + if serial_number in {"", "UNSET"}: + raise ValueError("serial_number is required") + if not str(value("session_dir")) or not source_urdf.is_file(): + raise ValueError("session_dir and immutable source_urdf_path are required") + if not camera_extrinsics_file.is_file(): + raise ValueError( + f"camera_extrinsics_file is required for {profile.key.model} zero solve" + ) + extrinsics = load_camera_extrinsics( + camera_extrinsics_file, + required_views=profile.vision.view_names, + reference_view=profile.vision.extrinsic_reference_view, + quality_limits=profile.vision.extrinsics_quality_limits, + minimum_capture_counts=profile.vision.minimum_capture_counts, + ) + protected_inputs = { + "source_urdf_sha256": str(value("source_urdf_expected_sha256")), + "camera_extrinsics_sha256": str( + value("camera_extrinsics_expected_sha256") + ), + "calibration_config_sha256": str( + value("calibration_config_expected_sha256") + ), + "tag_config_sha256": str(value("tag_config_expected_sha256")), + } + if "sdk_config_sha256" in profile.artifacts.protected_input_fields: + protected_inputs["sdk_config_sha256"] = str( + value("sdk_config_expected_sha256") + ) + if "sdk_package_sha256" in profile.artifacts.protected_input_fields: + protected_inputs["sdk_package_sha256"] = str(value("sdk_package_expected_sha256")) + if "profile_config_sha256" in profile.artifacts.protected_input_fields: + protected_inputs["profile_config_sha256"] = str( + value("profile_config_expected_sha256") + ) + resume_value = str(value("resume_raw_samples_path")).strip() + resume_raw_samples_path = ( + None + if not resume_value + else Path(resume_value).expanduser().resolve() + ) + if ( + set(protected_inputs) + != profile.artifacts.protected_input_fields + or any(len(item) != 64 for item in protected_inputs.values()) + ): + raise ValueError("all profile-protected SHA-256 values are required") + command_topic = str(value("command_topic")) + state_topic = str(value("state_topic")) + setting_topic = str(value("setting_topic")) + camera_info_topics = { + view: str(value(f"{view}_camera_info_topic")) + for view in profile.vision.view_names + } + detection_topics = { + view: str(value(f"{view}_detections_topic")) + for view in profile.vision.view_names + } + skew_ns = int(float(value("maximum_state_image_skew_ms"))*1_000_000) + rate = float(value("command_rate_hz")) + settle = float(value("speed_settle_seconds")) + if not 0 < skew_ns <= 50_000_000: + raise ValueError("image/feedback synchronization bound must be within 50 ms") + if not 0 < rate <= 200: + raise ValueError("command_rate_hz must be in (0, 200]") + if not 0 <= settle <= 5: + raise ValueError("speed_settle_seconds must be in [0, 5]") + return RuntimeParameters( + serial_number=serial_number, session_dir=session_dir, source_urdf=source_urdf, + camera_extrinsics_file=camera_extrinsics_file, extrinsics=extrinsics, + protected_inputs=protected_inputs, resume_raw_samples_path=resume_raw_samples_path, + command_topic=command_topic, state_topic=state_topic, setting_topic=setting_topic, + camera_info_topics=camera_info_topics, detection_topics=detection_topics, + commands_enabled=bool(value("commands_enabled")), speed_settle_seconds=settle, + torque_u8=int(value("torque_u8")), maximum_state_image_skew_ns=skew_ns, + command_rate_hz=rate, tag_size_m=float(value("tag_size_m")), + detection=DetectionPolicy(int(value("maximum_hamming")), + float(value("minimum_decision_margin")), float(value("minimum_edge_pixels"))), + tracking=tracking_parameters(value), + ) + + +def tracking_parameters(value: Callable[[str], Any]) -> PoseTrackingParameters: + """ROS exposes degrees; the shared pose core receives radians.""" + return PoseTrackingParameters( + maximum_reprojection_error_px=float(value("pnp_maximum_reprojection_error_px")), + reprojection_tie_px=float(value("pnp_reprojection_tie_px")), + maximum_pose_jump_rad=math.radians(float(value("pnp_maximum_pose_jump_deg"))), + maximum_translation_jump_m=float(value("pnp_maximum_translation_jump_m")), + maximum_tag_tilt_rad=math.radians(float(value("pnp_maximum_tag_tilt_deg"))), + reset_after_seconds=float(value("pnp_tracker_reset_seconds")), + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 298395b..fde6b92 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -1,58 +1,243 @@ -"""Generic assembly of a registered profile and the session controller.""" +"""One process lifecycle for every product; no delegation to model runners. + +ROS imports are lazy, allowing tests of the actual CLI and lifecycle without +hardware. Pending legacy status/artifact contracts are isolated in compat. +""" from __future__ import annotations import argparse +import os +from pathlib import Path +import subprocess +import time +from typing import Any, Callable, Mapping from ..compat import default_product_config_path -from ..core import ProfileKey -from ..core.solver import SessionSolver, TaskEvaluator -from ..models import ProfileRegistry, get_default_registry -from ..product import load_product_config -from .controller import SessionController +from ..product import ProductConfig, load_product_config +from ..storage import atomic_write_json +from .status import normalize_status, render_status_zh -def build_session_controller( - profile_key: ProfileKey, - evaluator: TaskEvaluator, - solver: SessionSolver, - *, - registry: ProfileRegistry | None = None, -) -> SessionController: - selected_registry = registry or get_default_registry() - registered = selected_registry.get(profile_key) - return SessionController(registered.profile, evaluator, solver) +TERMINAL_STATES = frozenset({"COMPLETE", "PAUSED", "ABORTED", "FAILED"}) -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 drive_session( + monitor: Any, process: Any, *, spin: Callable[[], None], + ok: Callable[[], bool], request_start: Callable[[], Any], + clock: Callable[[], float] = time.monotonic, auto_start: bool = True, + startup_timeout: float = 120.0, service_timeout: float = 10.0, +) -> dict[str, Any]: + """Send Start once; never infer an SDK fault from console/status age. + + Missing references after Start may wait indefinitely. Only startup/service + discovery have runner deadlines; hardware safety belongs to the node. + """ + launched = clock() + ready_since: float | None = None + requested_at: float | None = None + future = None + start_acknowledged = False + latest: dict[str, Any] = {} + + def failed(reason: str) -> dict[str, Any]: + return {**latest, "state": "FAILED", "reason": reason} + + while ok(): + spin() + latest = dict(monitor.status) + state = normalize_status(latest)["state"] + if state in TERMINAL_STATES: + return latest + if process.poll() is not None: + return failed(f"calibration_stack_exited:exit_code={process.returncode}") + now = clock() + if latest and now - monitor.last_status_at > 10.0: + if monitor.count_publishers(monitor.status_topic) == 0: + return failed("calibration_node_process_exited") + if not latest and now - launched > startup_timeout: + return failed("calibration_node_initial_status_timeout") + + if state == "READY" and auto_start and requested_at is None: + if ready_since is None: + ready_since = now + if monitor.start_client.service_is_ready(): + future = request_start() + requested_at = now + elif now - ready_since > service_timeout: + return failed("calibration_start_service_unavailable") + elif ready_since is None and requested_at is None: + if auto_start and now - launched > startup_timeout: + return failed("calibration_device_not_ready:" + str(latest.get("reason", state))) + + if future is not None and not start_acknowledged: + if future.done(): + response = future.result() + if response is None or not response.success: + return failed("calibration_start_rejected:" + str(getattr(response, "message", ""))) + start_acknowledged = True + elif now - float(requested_at) > service_timeout: + return failed("calibration_start_response_timeout") + return failed("ros_context_shutdown") + + +def _write_failure(config: ProductConfig, session: Path, status: Mapping[str, Any]) -> None: + from .runner_support import log_exception_summary, protected_inputs + + report = normalize_status(status, profile=config.calibration_contract.typed_profile) + report.update( + serial_number=config.serial_number, result="FAIL", + log_path=str(session / "calibration.log"), + protected_inputs=protected_inputs(config), + child_exception=log_exception_summary(session / "calibration.log"), + raw_node_status=dict(status), + ) + atomic_write_json(session / "runner_diagnostic.json", report) + print(render_status_zh(report), flush=True) + if report["child_exception"]: + print(f"节点异常：{report['child_exception']}", flush=True) + print(f"诊断日志：{report['log_path']}", flush=True) + + +def run_online( + config: ProductConfig, *, record_bag: bool = False, + commands_enabled: bool = True, allow_resume: bool = True, +) -> int: + import rclpy + from std_srvs.srv import Trigger + + from .artifacts.completion import finish_online_artifacts + from ..hikrobot_camera import configure_fastdds_large_image_transport + from .resume import discover_resume_candidate + from .adapters.ros_topics import sdk_topics + from .runner_support import ( + CalibrationMonitor, ProgressConsole, create_session_directory, + launch_command, overlay_environment, protected_inputs, stop_stack, + ) + + profile = config.calibration_contract.typed_profile + resume = discover_resume_candidate( + config.session_root, profile_id=profile.key.profile_id, + serial_number=config.serial_number, protected_hashes=protected_inputs(config), + ) if allow_resume and commands_enabled else None + session = create_session_directory(config.session_root) + from ..profiles.validator import validate_executable_profile + contract_report = validate_executable_profile(profile, config.source_urdf) + atomic_write_json(session/"measurement_contract.json", contract_report) + if contract_report["cad_constraint_conflicts"]: + print("原始 CAD 存在限位/mimic 冲突，未修改原文件；详见 measurement_contract.json。最终范围验证不通过时禁止发布。", flush=True) + log_path = session / "calibration.log" + configure_fastdds_large_image_transport() + process = None + monitor = None + owns_context = not rclpy.ok() + with log_path.open("a", encoding="utf-8", buffering=1) as log_stream: + try: + environment = overlay_environment(config.sdk_setup) if config.sdk_setup else dict(os.environ) + environment.setdefault("ROS_LOG_DIR", str(session / "ros_logs")) + if owns_context: + rclpy.init() + progress = ProgressConsole(lambda status, estimator: render_status_zh( + normalize_status(status, profile=profile, serial_number=config.serial_number), + estimator, + )) + monitor = CalibrationMonitor( + node_name="calibration_product_runner", namespace=profile.namespace, + progress=progress, + ) + # This runner owns and launches its SDK process. Do not open the + # same device a second time or accept another session's READY. + topics = sdk_topics(profile) + discovery_deadline = time.monotonic() + 1.5 + while rclpy.ok() and time.monotonic() < discovery_deadline: + rclpy.spin_once(monitor, timeout_sec=0.1) + occupied = {topic: monitor.count_publishers(topic) for topic in + (topics.command, topics.feedback, monitor.status_topic)} + if any(occupied.values()): + raise RuntimeError(f"existing_calibration_publishers:{occupied}") + print(f"{config.serial_number} 标定环境正在启动；日志：{log_path}", flush=True) + if resume is not None: + print(f"发现候选断点：{resume.name}；尚未复用，启动后验证基准和 Tag 安装关系。", flush=True) + if not commands_enabled: + print("仅预览：不自动开始、不发送标定运动；Ctrl+C 退出。", flush=True) + process = subprocess.Popen( + launch_command(config, session, record_bag=record_bag, + commands_enabled=commands_enabled, resume_from=resume), + cwd=config.workspace, env=environment, stdout=log_stream, + stderr=subprocess.STDOUT, text=True, start_new_session=True, + ) + status = drive_session( + monitor, process, spin=lambda: rclpy.spin_once(monitor, timeout_sec=0.1), + ok=rclpy.ok, request_start=lambda: monitor.start_client.call_async(Trigger.Request()), + auto_start=commands_enabled, + ) + atomic_write_json(session / "node_status.json", status) + if normalize_status(status)["state"] != "COMPLETE": + _write_failure(config, session, status) + return 3 + if not commands_enabled: + raise RuntimeError("preview session cannot publish calibration artifacts") + outputs = finish_online_artifacts(config, session, status) + print("标定会话完成；结果已通过当前发布链的校验。", flush=True) + for name, path in outputs.items(): + print(f"{name}：{path}", flush=True) + return 0 + except KeyboardInterrupt: + if monitor is not None and monitor.abort_client.service_is_ready(): + monitor.abort_client.call_async(Trigger.Request()) + rclpy.spin_once(monitor, timeout_sec=0.2) + print(f"已请求中止；本次数据保留在 {session}。", flush=True) + return 130 + except Exception as error: + import traceback + traceback.print_exc(file=log_stream) + _write_failure(config, session, {"state": "FAILED", "reason": str(error)}) + return 2 + finally: + # No unsolicited fast opening is commanded by a CLI failure. + try: + if process is not None: + stop_stack(process) + finally: + if monitor is not None: + monitor.destroy_node() + if owns_context and rclpy.ok(): + rclpy.shutdown() + log_stream.flush() + os.fsync(log_stream.fileno()) 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) - # Resolve the installed default lazily. A caller that supplies --config - # must also work directly from a source workspace before the package has - # been installed. - selector.add_argument("--config", default=None) - selector.add_argument("--workspace", default=None) - selected, _ = selector.parse_known_args(args) - config_path = ( - str(default_product_config_path()) - if selected.config is None - else selected.config + parser = argparse.ArgumentParser(description="配置驱动的多型号统一标定") + parser.add_argument("--config", default=None) + parser.add_argument("--workspace", default=None) + parser.add_argument("--validate-only", "--preflight-only", action="store_true") + parser.add_argument("--record-bag", action="store_true") + parser.add_argument("--commands-disabled", action="store_true") + parser.add_argument("--no-resume", action="store_true") + parser.add_argument("--offline-raw", default="") + parser.add_argument("--offline-output", default="") + parser.add_argument("--publish-offline", action="store_true") + selected = parser.parse_args(args) + config = load_product_config( + selected.config or str(default_product_config_path()), workspace=selected.workspace, + check_can=not (selected.validate_only or selected.offline_raw or selected.commands_disabled), ) - product = load_product_config( - config_path, - workspace=selected.workspace, - check_can=False, - ) - registered = get_default_registry().get(product.profile_key) - registered.engine.cli_main(args) + if selected.validate_only: + import json + from ..profiles.validator import validate_executable_profile + print(json.dumps(validate_executable_profile(config.calibration_contract.typed_profile, + config.source_urdf), ensure_ascii=False, indent=2)) + print(f"配置与受保护输入一致：{config.profile_key.profile_id}。此检查不代表 URDF 精度或硬件验收通过。") + return + if selected.offline_raw: + from .artifacts.replay import replay_capture + replay_capture(config, Path(selected.offline_raw), + output=Path(selected.offline_output) if selected.offline_output else None, + publish=selected.publish_offline) + return + if selected.offline_output or selected.publish_offline: + parser.error("--offline-output/--publish-offline require --offline-raw") + 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/runtime/runner_support.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py new file mode 100644 index 0000000..05b0b21 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py @@ -0,0 +1,273 @@ +"""Model-independent process, status and session support for CLI runners.""" + +from __future__ import annotations + +from datetime import datetime +import json +import os +from pathlib import Path +import re +import signal +import subprocess +import sys +import time +from typing import Any, Callable, Mapping + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ..operator_report import ProgressEstimator +from ..product import ProductConfig + + +class ProgressConsole: + """Render only changed status snapshots on either a TTY or a log stream.""" + + def __init__(self, renderer: Callable[..., str]) -> None: + self.last_text = "" + self.estimator = ProgressEstimator.start() + self.renderer = renderer + + def update(self, status: Mapping[str, Any]) -> None: + text = self.renderer(status, self.estimator) + if text == self.last_text: + return + self.last_text = text + if sys.stdout.isatty(): + sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") + sys.stdout.flush() + else: + print(text, flush=True) + + +class CalibrationMonitor(Node): + """Observe the common status topic and expose common control services.""" + + def __init__( + self, + *, + node_name: str, + namespace: str, + progress: ProgressConsole, + ) -> None: + super().__init__(node_name) + base = namespace.rstrip("/") + self.status_topic = f"{base}/status" + self.status: dict[str, Any] = {} + self.last_status_at = 0.0 + self.progress = progress + self.create_subscription(String, self.status_topic, self._on_status, 10) + self.start_client = self.create_client(Trigger, f"{base}/start") + self.abort_client = self.create_client(Trigger, f"{base}/abort") + + def _on_status(self, message: String) -> None: + try: + value = json.loads(message.data) + except json.JSONDecodeError: + return + if isinstance(value, dict): + self.status = value + self.last_status_at = time.monotonic() + self.progress.update(value) + + +def create_session_directory(root: Path) -> Path: + """Create a unique timestamped session without overwriting prior evidence.""" + root.mkdir(parents=True, exist_ok=True) + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + for number in range(10000): + session = root / (stamp if number == 0 else f"{stamp}_{number:04d}") + try: + session.mkdir() + except FileExistsError: + continue + return session + raise RuntimeError("cannot allocate a new calibration session") + + +def launch_command( + config: ProductConfig, + session: Path, + *, + record_bag: bool, + commands_enabled: bool, + sdk_startup_speed_u8: int | None = None, + resume_from: Path | None = None, +) -> list[str]: + """Build the single launch invocation entirely from the product contract.""" + arguments = { + "model": config.model, + "hand_type": config.side, + "tag_layout": config.tag_layout, + "serial_number": config.serial_number, + "source_urdf_path": str(config.source_urdf), + "source_urdf_expected_sha256": config.source_urdf_sha256, + "camera_extrinsics_file": str(config.camera_extrinsics), + "camera_extrinsics_expected_sha256": config.camera_extrinsics_sha256, + "calibration_config": str(config.calibration_config), + "calibration_config_expected_sha256": config.calibration_config_sha256, + "tag_config": str(config.tag_config), + "tag_config_expected_sha256": config.tag_config_sha256, + "vendor_sdk_config": "" if config.sdk_config is None else str(config.sdk_config), + "sdk_config_expected_sha256": config.sdk_config_sha256, + "profile_config_expected_sha256": getattr( + config, "profile_config_sha256", "" + ), + "profile_config": str(getattr(config, "profile_config", None) or ""), + "output_root": str(config.output_root), + "session_dir": str(session), + "commands_enabled": str(commands_enabled).lower(), + "vendor_sdk_python_package": str(getattr(config, "sdk_python_package", None) or ""), + "sdk_package_expected_sha256": getattr(config, "sdk_package_sha256", ""), + "record_bag": str(record_bag).lower(), + } + startup_speed = sdk_startup_speed_u8 + if startup_speed is None: + startup_speed = config.calibration_contract.typed_profile.motion.speed_parameters.get("baseline_u8") + if startup_speed is not None: + arguments["calibration_speed"] = str(int(startup_speed)) + if resume_from is not None: + arguments["resume_raw_samples_path"] = str( + resume_from / "raw_samples.jsonl" + ) + if config.can_interface: + arguments["can_interface"] = config.can_interface + for view, camera in config.cameras.items(): + arguments[f"{view}_camera_serial"] = camera["serial_number"] + arguments[f"{view}_camera_name"] = camera["camera_name"] + arguments[f"{view}_camera_info_url"] = camera["camera_info"] + return [ + "ros2", + "launch", + "linkerhand_calibration", + "unified_calibration.launch.py", + # ros2 launch rejects name:= with no value. Unused optional SDK + # arguments already have empty defaults in the launch description. + *(f"{name}:={value}" for name, value in arguments.items() if value != ""), + ] + + +def wait_until( + monitor: CalibrationMonitor, + process: subprocess.Popen[Any], + predicate: Callable[[Mapping[str, Any]], bool], + *, + timeout: float | None, + status_stale_after: float | None = None, + finalizing_stale_after: float = 180.0, +) -> bool: + """Wait for a status predicate while detecting process and status loss.""" + 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() + if status_stale_after is not None and monitor.last_status_at > 0.0: + stale_limit = ( + finalizing_stale_after + if monitor.status.get("state") == "FINALIZING" + else float(status_stale_after) + ) + if now - monitor.last_status_at > stale_limit: + status_publishers = monitor.count_publishers( + monitor.status_topic + ) + # A busy fit or a delayed console message is not loss of SDK + # feedback. The motion owner monitors feedback independently. + # Stop only when ROS discovery also says that owner is gone. + if status_publishers == 0: + monitor.status = { + **monitor.status, + "state": "FAILED", + "reason": "calibration_node_process_exited", + } + return False + if timeout is not None and now - started > timeout: + return False + return False + + +def stop_stack(process: subprocess.Popen[Any]) -> None: + """Stop the launch process group without leaking child processes.""" + try: + # The launch parent may have exited while its SDK/camera children still + # own this process group. The caller created it with start_new_session. + os.killpg(process.pid, signal.SIGINT) + except ProcessLookupError: + return + for timeout, stop_signal in ((15.0, signal.SIGTERM), (5.0, signal.SIGKILL)): + try: + process.wait(timeout=timeout) + return + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, stop_signal) + except ProcessLookupError: + return + process.wait(timeout=5.0) + + +def overlay_environment(setup: Path) -> dict[str, str]: + """Load a reviewed vendor overlay into a child-process environment.""" + completed = subprocess.run( + ["bash", "-c", 'source "$1" >/dev/null 2>&1 && env -0', "bash", str(setup)], + check=True, + stdout=subprocess.PIPE, + ) + environment = dict(os.environ) + for item in completed.stdout.split(b"\0"): + if b"=" in item: + key, value = item.split(b"=", 1) + environment[key.decode()] = value.decode(errors="surrogateescape") + return environment + + +def log_exception_summary(log_path: Path) -> str: + """Return the final child exception without flooding the operator view.""" + 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:]): + payload = ansi.sub("", raw).strip().rsplit("] ", 1)[-1].strip() + if payload.startswith(prefixes): + return payload + return "" + + +def protected_inputs(config: ProductConfig) -> dict[str, str]: + """Return exactly the immutable inputs available on a product contract.""" + values = { + "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, + "sdk_package_sha256": getattr(config, "sdk_package_sha256", ""), + "profile_config_sha256": getattr(config, "profile_config_sha256", ""), + } + return {key: value for key, value in values.items() if value} + + +__all__ = [ + "CalibrationMonitor", + "ProgressConsole", + "create_session_directory", + "launch_command", + "log_exception_summary", + "overlay_environment", + "protected_inputs", + "stop_stack", + "wait_until", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py new file mode 100644 index 0000000..27143ac --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/safety.py @@ -0,0 +1,194 @@ +"""Minimal, model-independent live safety policy. + +Only conditions that can damage hardware, corrupt the coordinate reference, or +make command ownership ambiguous are stops. Vision quality and normal tendon +coupling deliberately do not enter this module. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Sequence + +from ..core import CalibrationProfile +from .adapters import HardwareHealth + + +@dataclass(frozen=True) +class MotionGoal: + """One axis of a whole segment, in the Adapter's feedback coordinates.""" + + channel: int + start_feedback: float + end_feedback: float + completion_tolerance: float + progress_resolution: float + + +@dataclass(frozen=True) +class SafetySample: + now_seconds: float + feedback_timestamp_seconds: float | None + command: tuple[float, ...] + feedback: tuple[float, ...] | None + health: HardwareHealth + competing_controller: bool = False + reference_moved: bool = False + operator_abort: bool = False + motion_expected: bool = False + target_channel: int | None = None + target_value: float | None = None + motion_id: str = "" + motion_goals: tuple[MotionGoal, ...] = () + + +@dataclass(frozen=True) +class SafetyDecision: + safe: bool + code: str = "" + reason: str = "" + details: str = "" + + +class SafetyPolicy: + """Evaluate the complete and intentionally small list of live stops.""" + + def __init__(self, profile: CalibrationProfile) -> None: + self.profile = profile + self._motion_id: str | None = None + self._axis_progress: dict[int, tuple[float, float, float]] = {} + + def evaluate(self, sample: SafetySample) -> SafetyDecision: + if sample.operator_abort: + return self._stop("operator_abort", "操作者已中止标定") + if sample.competing_controller: + return self._stop( + "duplicate_controller", "检测到另一个命令控制节点" + ) + if sample.reference_moved: + return self._stop( + "fixed_reference_moved", + "已锁定的手掌、相机、支架或固定基准 Tag 发生移动", + ) + health = sample.health + if not health.connected: + return self._stop("sdk_disconnected", "SDK 通信已失联") + if not health.position_mode: + return self._stop("wrong_control_mode", "机械手不在位置控制模式") + if health.active_faults: + return self._stop( + "hardware_fault", + "SDK 报告活动硬件故障", + ",".join(health.active_faults), + ) + if (sample.feedback is None or not health.feedback_fresh + or sample.feedback_timestamp_seconds is None + or not math.isfinite(float(sample.now_seconds)) + or not math.isfinite(float(sample.feedback_timestamp_seconds)) + or float(sample.feedback_timestamp_seconds) > float(sample.now_seconds) + 1e-6 + or float(sample.now_seconds) - float(sample.feedback_timestamp_seconds) + > self.profile.acquisition.feedback_stale_seconds): + return self._stop("feedback_stale", "反馈连续超过允许时间未更新") + try: + self._validate_vector(sample.command, feedback=False) + if sample.feedback is not None: + self._validate_vector(sample.feedback, feedback=True) + except ValueError as error: + return self._stop("physical_range_exceeded", str(error)) + stall = self._evaluate_progress(sample) + if stall is not None: + return stall + return SafetyDecision(True) + + def _evaluate_progress( + self, sample: SafetySample + ) -> SafetyDecision | None: + """Track signed feedback advance, not a moving set-point's distance. + + Each segment has a stable identity and may move several avoidance axes. + Updating a streaming position command cannot reset its stall clock. + """ + if not sample.motion_expected or sample.feedback is None: + self._reset_progress() + return None + goals = sample.motion_goals + if not goals and sample.target_channel is not None and sample.target_value is not None: + # Compatibility for existing scalar callers. Live execution supplies + # explicit whole-segment goals, including feedback zero conventions. + index = int(sample.target_channel) + if not 0 <= index < self.profile.command.command_count: + return self._stop("invalid_motion_target", "运动目标通道不在 Profile 中") + span = self.profile.command.maximum_values[index] - self.profile.command.minimum_values[index] + tolerance = max(1e-4, 0.002*span) + goals = (MotionGoal(index, sample.feedback[index], float(sample.target_value), tolerance, tolerance),) + if not goals: + return self._stop("invalid_motion_target", "运动段缺少反馈推进目标") + motion_id = sample.motion_id or "legacy_scalar_motion" + if self._motion_id != motion_id: + self._axis_progress.clear() + self._motion_id = motion_id + now = float(sample.now_seconds) + seen: set[int] = set() + for goal in goals: + index = goal.channel + if (index in seen or not 0 <= index < self.profile.command.command_count + or not all(math.isfinite(v) for v in (goal.start_feedback, goal.end_feedback, goal.completion_tolerance, goal.progress_resolution)) + or goal.completion_tolerance < 0 or goal.progress_resolution <= 0): + return self._stop("invalid_motion_target", "运动段推进目标无效") + seen.add(index) + feedback = float(sample.feedback[index]) + if abs(goal.end_feedback - goal.start_feedback) <= goal.completion_tolerance: + continue + direction = math.copysign(1.0, goal.end_feedback - goal.start_feedback) + projected = direction * feedback + previous = self._axis_progress.get(index) + if previous is None or previous[2] != direction: + self._axis_progress[index] = (projected, now, direction) + continue + best, last_advance, _ = previous + # Passing the requested travel is completion, not a new stall + # while holding slightly beyond an encoder-biased endpoint. + if direction * (goal.end_feedback - feedback) <= goal.completion_tolerance or projected >= best + goal.progress_resolution: + self._axis_progress[index] = (projected, now, direction) + continue + if now - last_advance >= self.profile.acquisition.stall_timeout_seconds: + return self._stop("mechanical_stall", + "已要求明显运动，但目标反馈连续两秒没有向目标推进", + f"segment={motion_id},channel={index},feedback={feedback:.9g},goal={goal.end_feedback:.9g}") + return None + + def _reset_progress(self) -> None: + self._motion_id = None + self._axis_progress.clear() + + def _validate_vector( + self, values: Sequence[float], *, feedback: bool + ) -> None: + layout = self.profile.command + if len(values) != layout.command_count: + raise ValueError("命令或反馈通道数量与 Profile 不一致") + lower = ( + layout.minimum_feedback_values if feedback else layout.minimum_values + ) + upper = ( + layout.maximum_feedback_values if feedback else layout.maximum_values + ) + kind = "反馈" if feedback else "命令" + for index, value in enumerate(values): + numeric = float(value) + if ( + not math.isfinite(numeric) + or numeric < lower[index] + or numeric > upper[index] + ): + raise ValueError( + f"{kind}越过物理范围: channel={index}:value={numeric}" + ) + + @staticmethod + def _stop(code: str, reason: str, details: str = "") -> SafetyDecision: + return SafetyDecision(False, code, reason, details) + + +__all__ = ["MotionGoal", "SafetyDecision", "SafetyPolicy", "SafetySample"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py new file mode 100644 index 0000000..652893f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/scan_quality.py @@ -0,0 +1,75 @@ +"""One post-direction data policy for primary and secondary observations.""" + +from __future__ import annotations + +import math +from typing import Mapping, Sequence + +from .engine import CalibrationEngine, ScanUnit, SweepQuality + + +def observation_streams(profile, task): + """(record field, joint identity, measurement) required by one task.""" + for name in task.joints: + yield "joint", name, profile.measurement.measurements[name] + secondary = profile.measurement.cross_view_sources.get(name) + if secondary is not None: + yield "observation_joint", name, profile.measurement.measurements[secondary] + + +def evaluate_capture_unit(profile, unit: ScanUnit, attempt: int, + records: Sequence[Mapping], *, first_cycle_spans: dict[str, float]) -> SweepQuality: + """No rates, coupling, endpoint tracking error or Tag percentages stop a scan. + + Count unique images separately for EVERY required joint/view. A good + proximal Tag cannot hide missing distal or secondary-camera observations. + Reference travel is learned only from an accepted first training cycle. + """ + engine = CalibrationEngine(profile) + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + native = profile.command.unit + lower, upper = sorted((task.start_value, task.end_value)) + span = upper - lower + if span <= 0: + raise ValueError("a scan needs a nonzero declared range") + failures, warnings, metrics, references = [], [], {}, {} + for field, name, spec in observation_streams(profile, task): + identity = f"{field}:{name}:{spec.view}:{unit.direction}" + samples = {} + for row in records: + if row.get("sample_phase", "sweep") != "sweep": + continue + if (row.get(field) != name or row.get("view") != spec.view + or row.get("task_name") != unit.task_key + or row.get("cycle") != unit.cycle or row.get("direction") != unit.direction + or int(row.get("attempt", 1)) != attempt): + continue + stamp = row.get("image_stamp_ns") + if stamp is None or not isinstance(stamp, int) or stamp <= 0: + continue + value = row.get(f"feedback_{native}") + if value is None or not math.isfinite(float(value)): + continue + progress = (float(value) - lower) / span + # Physical encoder bounds are distinct from the requested sweep. + # Samples outside it are not clipped into fake endpoint bins. + if 0 <= progress <= 1: + samples[(spec.view, stamp)] = progress + policy = profile.acquisition + minimum = policy.legacy_minimum_span_01 if native == "u8" else ( + policy.physical_first_cycle_minimum_span_01 if unit.cycle == 0 else + policy.physical_repeat_minimum_fraction * first_cycle_spans.get(identity, 1.0)) + if native != "u8" and unit.cycle > 0 and identity not in first_cycle_spans: + failures.append(f"{identity}:missing_training_travel") + decision = engine.evaluate_sweep(tuple(samples.values()), minimum_span=minimum, + total_frames=len(samples), joint_frame_rate=1.0, feedback_hz=0.0, + detection_rate=1.0, bin_count=256) + failures.extend(f"{identity}:{reason}" for reason in decision.failures) + warnings.extend(f"{identity}:{reason}" for reason in decision.warnings) + metrics[identity] = dict(decision.metrics) + references[identity] = float(decision.metrics["feedback_span"]) + from .steady import steady_failures + failures.extend(steady_failures(profile, unit, attempt, records)) + if not failures and unit.cycle == 0: + first_cycle_spans.update(references) + return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py new file mode 100644 index 0000000..46f3e96 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/session.py @@ -0,0 +1,335 @@ +"""The single online calibration state machine. + +ROS and SDK adapters execute requested actions. This object owns ordering, +retry policy, pause semantics, progress and resume fallback for every profile. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum +from typing import Mapping + +from ..core import CalibrationProfile, CalibrationStatus, PauseStatus, TaskStatus +from .engine import CalibrationEngine, ScanUnit, SweepQuality +from .resume import ResumeDecision + + +class CalibrationPhase(str, Enum): + WAIT_DEVICE = "WAIT_DEVICE" + READY = "READY" + BASELINE = "BASELINE" + REFERENCE_LOCKING = "REFERENCE_LOCKING" + REFERENCE_POSES = "REFERENCE_POSES" + RESUME_VERIFY = "RESUME_VERIFY" + PREPARE = "PREPARE" + MAPPING_PROBE = "MAPPING_PROBE" + SWEEP = "SWEEP" + EVALUATE = "EVALUATE" + RESCAN = "RESCAN" + RETURN_BASELINE = "RETURN_BASELINE" + FIT = "FIT" + HOLDOUT_VALIDATE = "HOLDOUT_VALIDATE" + BUILD_ARTIFACTS = "BUILD_ARTIFACTS" + VALIDATE_URDF = "VALIDATE_URDF" + PUBLISH = "PUBLISH" + COMPLETE = "COMPLETE" + PAUSED = "PAUSED" + ABORTED = "ABORTED" + FAILED = "FAILED" + + +@dataclass(frozen=True) +class SessionAction: + phase: CalibrationPhase + scan_unit: ScanUnit | None = None + retry: int = 0 + + +class CalibrationSession: + """Deterministic orchestration kernel with no model-specific branches.""" + + def __init__(self, profile: CalibrationProfile) -> None: + self.profile = profile + self.engine = CalibrationEngine(profile) + self.phase = CalibrationPhase.WAIT_DEVICE + self._units = self.engine.scan_units() + self._unit_index = 0 + self._retry_by_unit: dict[tuple[str, int, str], int] = {} + self._mapping_probed_channels: set[int] = set() + self._reused_units: set[tuple[str, int, str]] = set() + self._resume_requested = False + self._resume_message = "" + self._pause = PauseStatus() + self._reference_locked = False + self._rescan_pending = False + + @property + def current_unit(self) -> ScanUnit | None: + if self._unit_index >= len(self._units): + return None + return self._units[self._unit_index] + + def device_ready(self) -> None: + self._require(CalibrationPhase.WAIT_DEVICE) + self.phase = CalibrationPhase.READY + + def devices_unavailable(self) -> None: + """Revoke readiness before Start without pausing a running scan.""" + self._require_any(CalibrationPhase.WAIT_DEVICE, CalibrationPhase.READY) + self.phase = CalibrationPhase.WAIT_DEVICE + + def start(self, *, resume_requested: bool = False) -> None: + self._require(CalibrationPhase.READY) + self._resume_requested = bool(resume_requested) + self._reference_locked = False + self.phase = CalibrationPhase.BASELINE + + def baseline_complete(self) -> None: + self._require(CalibrationPhase.BASELINE) + self.phase = CalibrationPhase.REFERENCE_LOCKING + + def reference_locked(self) -> None: + self._require(CalibrationPhase.REFERENCE_LOCKING) + self._reference_locked = True + if self.profile.motion.resume_verification_waypoints: + self.phase = CalibrationPhase.REFERENCE_POSES + return + self._after_reference_poses() + + def reference_poses_complete(self): + self._require(CalibrationPhase.REFERENCE_POSES) + self._after_reference_poses() + + def _after_reference_poses(self): + self.phase = ( + CalibrationPhase.RESUME_VERIFY + if self._resume_requested + else CalibrationPhase.PREPARE + ) + + def resume_checked(self, decision: ResumeDecision) -> None: + self._require(CalibrationPhase.RESUME_VERIFY) + self._resume_message = decision.reason + if not decision.reuse: + self._unit_index = 0 + self._retry_by_unit.clear() + self._reused_units.clear() + else: + valid = { + (unit.task_key, unit.cycle, unit.direction) + for unit in self._units + } + requested = { + (str(task), int(cycle), str(direction)) + for task, cycle, direction in decision.completed_units + } + if not requested.issubset(valid): + raise ValueError("resume contains an unknown scan unit") + self._reused_units = requested + self._advance_reused_units() + self.phase = CalibrationPhase.PREPARE + + def preparation_complete(self) -> None: + self._require(CalibrationPhase.PREPARE) + unit = self.current_unit + if unit is None: + self.phase = CalibrationPhase.RETURN_BASELINE + return + task = self._task(unit.task_key) + if ( + self.engine.mapping_probe_delta(task) is not None + and task.command_index not in self._mapping_probed_channels + ): + self.phase = CalibrationPhase.MAPPING_PROBE + else: + self.phase = CalibrationPhase.RESCAN if self._rescan_pending else CalibrationPhase.SWEEP + self._rescan_pending = False + + def mapping_probe_complete(self) -> None: + self._require(CalibrationPhase.MAPPING_PROBE) + unit = self.current_unit + if unit is None: + raise RuntimeError("mapping probe has no current task") + self._mapping_probed_channels.add(self._task(unit.task_key).command_index) + self.phase = CalibrationPhase.SWEEP + + def sweep_complete(self) -> None: + self._require_any(CalibrationPhase.SWEEP, CalibrationPhase.RESCAN) + self.phase = CalibrationPhase.EVALUATE + + def evaluation_complete(self, quality: SweepQuality) -> None: + self._require(CalibrationPhase.EVALUATE) + unit = self.current_unit + if unit is None: + self.fail("missing_scan_unit", "扫描单元状态丢失") + return + key = (unit.task_key, unit.cycle, unit.direction) + retries = self._retry_by_unit.get(key, 0) + if not quality.passed: + if self.engine.permits_retry("sweep_acquisition", retries): + self._retry_by_unit[key] = retries + 1 + self._rescan_pending = True + # Never reverse a failed direction by jumping straight back to + # its first set-point. Re-execute the ordinary safe preparation. + self.phase = CalibrationPhase.PREPARE + return + self.pause( + "sweep_quality_failed", + "当前方向同速重扫一次后仍没有足够有效数据", + {"failures": quality.failures, "metrics": dict(quality.metrics)}, + ) + return + self._unit_index += 1 + self._advance_reused_units() + if self._unit_index < len(self._units): + # A skipped checkpoint may leave a gap even within the same task. + # The executor elides already-satisfied waypoints without motion. + self.phase = CalibrationPhase.PREPARE + return + self.phase = CalibrationPhase.RETURN_BASELINE + + def return_complete(self) -> None: + self._require(CalibrationPhase.RETURN_BASELINE) + self.phase = CalibrationPhase.FIT + + def fit_complete(self) -> None: + self._require(CalibrationPhase.FIT) + self.phase = CalibrationPhase.HOLDOUT_VALIDATE + + def holdout_complete(self, *, passed: bool, reason: str = "") -> None: + self._require(CalibrationPhase.HOLDOUT_VALIDATE) + if not passed: + self.fail("holdout_failed", reason or "独立第四轮验证未通过") + return + self.phase = CalibrationPhase.BUILD_ARTIFACTS + + def artifacts_built(self) -> None: + self._require(CalibrationPhase.BUILD_ARTIFACTS) + self.phase = CalibrationPhase.VALIDATE_URDF + + def urdf_validated(self, *, passed: bool, reason: str = "") -> None: + self._require(CalibrationPhase.VALIDATE_URDF) + if not passed: + self.fail("urdf_validation_failed", reason or "URDF 校验失败") + return + self.phase = CalibrationPhase.PUBLISH + + def published(self) -> None: + self._require(CalibrationPhase.PUBLISH) + self.phase = CalibrationPhase.COMPLETE + + def pause( + self, code: str, reason: str, details: Mapping[str, object] | None = None, + *, suggestion: str = "排除原因后从安全基准恢复", + ) -> None: + self._pause = PauseStatus(code, reason, suggestion, details or {}) + self.phase = CalibrationPhase.PAUSED + + def abort(self) -> None: + self._pause = PauseStatus("operator_abort", "操作者已中止标定") + self.phase = CalibrationPhase.ABORTED + + def fail(self, code: str, reason: str) -> None: + self._pause = PauseStatus(code, reason) + self.phase = CalibrationPhase.FAILED + + def action(self) -> SessionAction: + unit = self.current_unit + retries = 0 + if unit is not None: + retries = self._retry_by_unit.get( + (unit.task_key, unit.cycle, unit.direction), 0 + ) + return SessionAction(self.phase, unit, retries) + + def status(self) -> CalibrationStatus: + unit = self.current_unit + task_phases = {CalibrationPhase.PREPARE, CalibrationPhase.MAPPING_PROBE, + CalibrationPhase.SWEEP, CalibrationPhase.EVALUATE, CalibrationPhase.RESCAN, + CalibrationPhase.PAUSED, CalibrationPhase.ABORTED} + if self.phase not in task_phases: + unit = None + task = next( + ( + value + for value in self.profile.motion.tasks + if unit is not None and value.key == unit.task_key + ), + None, + ) + progress = ( + 1.0 + if self.phase == CalibrationPhase.COMPLETE + else 0.90*self._unit_index / max(1, len(self._units)) + ) + required: dict[str, tuple[int, ...]] = {} + if self.phase in {CalibrationPhase.REFERENCE_LOCKING, CalibrationPhase.RESUME_VERIFY}: + required = {view.name: tuple(tag.tag_id for tag in view.tags if tag.fixed_reference) + for view in self.profile.vision.views} + if task is not None: + measurement_names = set(task.joints) + measurement_names.update( + self.profile.measurement.cross_view_sources[name] + for name in task.joints + if name in self.profile.measurement.cross_view_sources + ) + measurements = [ + self.profile.measurement.measurements[name] + for name in measurement_names + ] + for view in self.profile.vision.views: + roles = { + role for measurement in measurements if measurement.view == view.name + for role in (measurement.parent_role, measurement.child_role) if role is not None + } + ids = tuple(tag.tag_id for tag in view.tags if tag.role in roles) + if ids: + required[view.name] = ids + return CalibrationStatus( + state=self.phase.value, + phase=self.phase.value, + overall_progress_01=progress, + reference_locked=self._reference_locked, + reference_message=( + "基准已锁定：请勿移动手掌、相机、支架或Tag" + if self._reference_locked + else "" + ), + task=TaskStatus( + key="" if task is None else task.key, + label="" if task is None else " / ".join(task.joints), + cycle=None if unit is None else unit.cycle + 1, + direction="" if unit is None else unit.direction, + required_tag_ids_by_view=required, + ), + resume={"message": self._resume_message}, + pause=self._pause, + ) + + def _require(self, expected: CalibrationPhase) -> None: + if self.phase != expected: + raise RuntimeError( + f"invalid transition from {self.phase.value}; " + f"expected {expected.value}" + ) + + def _require_any(self, *expected: CalibrationPhase) -> None: + if self.phase not in expected: + allowed = ",".join(value.value for value in expected) + raise RuntimeError( + f"invalid transition from {self.phase.value}; expected {allowed}" + ) + + def _task(self, key: str): + return next(task for task in self.profile.motion.tasks if task.key == key) + + def _advance_reused_units(self) -> None: + while self._unit_index < len(self._units): + unit = self._units[self._unit_index] + if (unit.task_key, unit.cycle, unit.direction) not in self._reused_units: + break + self._unit_index += 1 + + +__all__ = ["CalibrationPhase", "CalibrationSession", "SessionAction"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py new file mode 100644 index 0000000..1ed083f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/snapshot.py @@ -0,0 +1,127 @@ +"""Typed live status assembly; wire dictionaries are created only at the edge.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, replace +from typing import TYPE_CHECKING, Mapping + +from ..core.domain.status import CalibrationStatus, MotionStatus +from .scan_quality import evaluate_capture_unit +from .session import CalibrationPhase as Phase + +if TYPE_CHECKING: + from .coordinator import CalibrationCoordinator + + +_LEGACY_STATES = { + Phase.WAIT_DEVICE: "WAIT_DEVICES", Phase.READY: "READY", + Phase.COMPLETE: "PASSED", Phase.PAUSED: "PAUSED", + Phase.ABORTED: "ABORTED", Phase.FAILED: "FAILED", + **{phase: "FINALIZING" for phase in ( + Phase.FIT, Phase.HOLDOUT_VALIDATE, Phase.BUILD_ARTIFACTS, + Phase.VALIDATE_URDF, Phase.PUBLISH, + )}, +} + + +def legacy_state(phase: Phase) -> str: + return _LEGACY_STATES.get(phase, "RUNNING") + + +@dataclass(frozen=True) +class CameraReadiness: + camera_info_ready: bool + detections_ready: bool + error: str + + +@dataclass(frozen=True) +class DeviceReadiness: + ready: bool + waiting_for: tuple[str, ...] + cameras: Mapping[str, CameraReadiness] + + +@dataclass(frozen=True) +class RuntimeSnapshot: + calibration: CalibrationStatus + model: str + side: str + profile_id: str + serial_number: str + reason: str + session_dir: str + latest_base_drift_px: Mapping[str, float] + devices: DeviceReadiness | None + steady: bool = False + steady_node: int | None = None + + def as_dict(self) -> dict: + value = self.calibration.as_dict() + value.update(model=self.model, side=self.side, profile_id=self.profile_id, + serial_number=self.serial_number, reason=self.reason, + session_dir=self.session_dir, latest_base_drift_px=dict(self.latest_base_drift_px)) + if self.devices is not None: + value["devices"] = asdict(self.devices) + if self.steady: + value["acquisition"]["steady_node"] = self.steady_node + return value + + +def build_snapshot(host: CalibrationCoordinator) -> RuntimeSnapshot: + """Called under the coordinator lock; retains the public status fields.""" + session, profile, parameters = host.execution.session, host.profile, host.parameters + status = session.status() + task, motion = status.task, host._motion + if session.phase == Phase.REFERENCE_POSES and motion is not None: + waypoint = next((w for w in profile.motion.resume_verification_waypoints + if w.key == motion.task_key), None) + if waypoint is not None: + task = replace(task, required_tag_ids_by_view=waypoint.tag_ids_by_view, + label="Tag 安装验证姿态："+waypoint.key) + required = task.required_tag_ids_by_view + required_ids = {tag for ids in required.values() for tag in ids} + missing = {tag for view, ids in required.items() + for tag in host.capture.last_missing.get(view, ids)} & required_ids + filtered = {tag for ids in host.capture.last_filtered.values() for tag in ids} & required_ids + steady = motion is not None and motion.phase in {"steady", "steady_prepare"} + steady_node = None + if steady and motion.steady_index is not None: + steady_node = motion.steady_index+1 + task = replace(task, label=task.label+f"（稳态点 {steady_node}）") + index = None if motion is None else motion.command_index + health = host.sdk_adapter.health() + quality = host.execution.last_quality if session.phase == Phase.PAUSED else None + if session.phase in {Phase.SWEEP, Phase.RESCAN, Phase.EVALUATE}: + quality = evaluate_capture_unit(profile, session.current_unit, + host.execution.action.retry+1, tuple(host._unit_rows), + first_cycle_spans=dict(host.execution.first_cycle_spans)) + acquisition = replace(status.acquisition, retry_count=host.execution.action.retry) + if quality is not None and quality.metrics: + metrics = tuple(quality.metrics.values()) + acquisition = replace(acquisition, + valid_samples=min(m["valid_frames"] for m in metrics), + coverage_01=min(m["feedback_span"] for m in metrics), + bins=min(m["feedback_bins"] for m in metrics), + maximum_gap=max(m["maximum_bin_gap"] for m in metrics)) + status = replace(status, task=task, acquisition=acquisition, + phase=motion.phase if steady else status.phase, + recognized_tag_ids=tuple(sorted(required_ids-missing-filtered)), + missing_tag_ids=tuple(sorted(missing)), rejected_tag_ids=tuple(sorted(filtered)), + log_path=str(parameters.session_dir/"calibration.log"), + hardware={"feedback_hz": round(host._feedback_hz(), 2), "mode_verified": health.position_mode, + "faults": health.active_faults, "feedback_fresh": health.feedback_fresh, "diagnostic": health.diagnostic}, + motion=MotionStatus(channel="" if index is None else host.command_names[index], + unit=host.command_unit, speed=None if motion is None else motion.speed, + command=None if index is None or host.last_command is None else host.last_command[index], + feedback=None if index is None or not host.latest_feedback else host.latest_feedback[index]), + resume={"message": host._resume_message, "completed_unit_count": host._resumed_count, + "source_session": "" if parameters.resume_raw_samples_path is None + else parameters.resume_raw_samples_path.parent.name}, + outputs={"json": host.final_json, "urdf": host.final_urdf, + "publication_pointer": str(parameters.session_dir.parent/profile.artifacts.publication_pointer) + if session.phase == Phase.COMPLETE else ""}) + return RuntimeSnapshot(status, profile.key.model, profile.key.side, profile.key.profile_id, + parameters.serial_number, host.reason, str(parameters.session_dir), + dict(host.reference_lock.latest_drift_px), + None if host.started else host._device_status(host.ports.monotonic()), steady, steady_node) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py new file mode 100644 index 0000000..0d99792 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/status.py @@ -0,0 +1,155 @@ +"""One console/status contract, with a read-only bridge for legacy messages.""" + +from __future__ import annotations + +from typing import Any, Mapping + +from ..core.domain.profile import CalibrationProfile +from ..core.domain.status import CalibrationStatus +from .reporting.reasons_zh import reason_zh + + +_STATES = {"WAIT_START": "READY", "WAIT_DEVICES": "WAIT_DEVICE", "PASSED": "COMPLETE"} +_PHASES = { + "WAIT_DEVICE": "等待设备", "READY": "设备就绪", "BASELINE": "安全恢复基准", + "REFERENCE_LOCKING": "锁定基准（等待所需 Tag）", "RESUME_VERIFY": "验证断点基准", + "REFERENCE_POSES": "采集 Tag 安装验证姿态", + "PREPARE": "扫描起点准备", "MAPPING_PROBE": "固定通道映射点动", + "SWEEP": "正式扫描", "EVALUATE": "方向数据检查", "RESCAN": "同速重扫", + "RETURN_BASELINE": "按避让顺序安全收尾", + "FIT": "训练数据拟合", "HOLDOUT_VALIDATE": "第四轮独立验证", + "BUILD_ARTIFACTS": "生成产物", "VALIDATE_URDF": "校验导出 URDF", + "PUBLISH": "发布", "COMPLETE": "会话完成", "PAUSED": "已暂停", + "ABORTED": "已中止", "FAILED": "失败", "RUNNING": "标定中", + "FINALIZING": "拟合与产物校验", "FITTING": "拟合与验证", + "baseline": "安全恢复基准", "preflight": "固定通道映射点动", + "prepare": "扫描起点准备", "retry_prepare": "同速重扫起点准备", "sweep": "正式扫描", + "steady_prepare": "稳态指令采样起点准备", "steady": "稳态指令→rad 采样", +} + + +def required_tags(profile: CalibrationProfile, task_key: str) -> dict[str, list[int]]: + task = next((task for task in profile.motion.tasks if task.key == task_key), None) + if task is None: + return {} + names = set(task.joints) + names.update(profile.measurement.cross_view_sources[name] for name in task.joints + if name in profile.measurement.cross_view_sources) + measurements = [profile.measurement.measurements[name] for name in names] + result = {} + for view in profile.vision.views: + roles = {role for item in measurements if item.view == view.name + for role in (item.parent_role, item.child_role) if role is not None} + ids = [tag.tag_id for tag in view.tags if tag.role in roles] + if ids: + result[view.name] = ids + return result + + +def normalize_status( + value: Mapping[str, Any], *, profile: CalibrationProfile | None = None, + serial_number: str = "", +) -> dict[str, Any]: + """Normalize transport fields only; never invent quality or motion evidence.""" + result = CalibrationStatus().as_dict() + result.update(value) + state = _STATES.get(str(value.get("state", "WAIT_DEVICE")), str(value.get("state", "WAIT_DEVICE"))) + result["state"] = state + result["serial_number"] = serial_number or value.get("serial_number", "?") + if profile is not None: + result.update(model=profile.key.model, side=profile.key.side, + profile_id=profile.key.profile_id) + if isinstance(value.get("task"), Mapping) and "overall_progress_01" in value: + return result + active = value.get("active", {}) + active = active if isinstance(active, Mapping) else {} + row = {**value, **active} + key = str(row.get("task_name") or "") + selected = next((task for task in profile.motion.tasks if task.key == key), None) if profile else None + cycle = row.get("cycle") + # The old nested active contract is already one-based; old flat nodes are + # zero-based. The public v1 status uses one-based cycles everywhere. + cycle = None if cycle is None else int(cycle) + (0 if active else 1) + result["task"] = { + "key": key, "label": " / ".join(selected.joints) if selected else key, + "cycle": cycle, "cycle_count": 4, "direction": row.get("direction", ""), + "required_tag_ids_by_view": required_tags(profile, key) if profile and key + else value.get("required_tag_ids_by_view", {}), + } + unit = str(value.get("command_unit", profile.command.unit if profile else "u8")) + result["motion"] = { + "command": row.get("current_command_u8", row.get("current_motion_target_u8", row.get("target_rad", row.get("target_u8")))), + "feedback": row.get("actual_u8"), "unit": unit, + "speed": row.get("speed_rad_s", row.get("speed_u8")), + } + sample = row.get("sample", {}) + sample = sample if isinstance(sample, Mapping) else {} + result["acquisition"] = { + "valid_samples": row.get("valid_frames", 0), + "coverage_01": row.get("feedback_span_01", float(sample.get("span_u8", 0)) / 255.0 if unit == "u8" else 0.0), + "bins": row.get("feedback_bins", sample.get("bin_count", 0)), + "maximum_gap": row.get("maximum_bin_gap", sample.get("maximum_gap", 0)), + "retry_count": row.get("automatic_retry_count", max(0, int(row.get("attempt", 1) or 1) - 1)), + } + progress = value.get("progress") + if progress is None: + count = int(value.get("step_count", 0) or 0) + progress = (int(value.get("step_index", 0) or 0) + float(value.get("step_fraction", 0) or 0)) / count if count else 0.0 + result["overall_progress_01"] = 1.0 if state == "COMPLETE" else min(1.0, max(0.0, float(progress))) + result["phase"] = str(row.get("phase") or state) + result["missing_tag_ids"] = value.get("missing_tag_ids", value.get("unrecognized_tag_ids", ())) + result["hardware"] = { + "feedback_hz": value.get("feedback_hz"), + "mode_verified": value.get("position_mode_verified"), + "faults": value.get("error_faults", value.get("error_codes")), + "feedback_fresh": value.get("hand_state_fresh"), + } + if state in {"PAUSED", "ABORTED", "FAILED"}: + code, reason, suggestion = reason_zh(value, model_name=profile.key.model if profile else "机械手") + result["pause"] = {"code": code, "reason": reason, "suggestion": suggestion, + "details": {"raw_reason": str(value.get("reason", ""))}} + return result + + +def render_status_zh(value: Mapping[str, Any], estimator=None) -> str: + """Render v1 without assumptions about channel, task, or camera counts.""" + progress = float(value.get("overall_progress_01", 0.0)) + eta = value.get("estimated_remaining_seconds") + if eta is None and estimator is not None: + eta = estimator.remaining(progress) + eta_text = "计算中" if eta is None else f"{int(eta) // 60}分{int(eta) % 60:02d}秒" + task, motion, acquisition = (value.get(name, {}) for name in ("task", "motion", "acquisition")) + unit = motion.get("unit", "") + def angle(item): + return "未知" if item is None else f"{float(item):.3f} {unit}" + def tags(ids): + return "/".join(f"ID{int(item)}" for item in ids) or "无" + lines = [ + f"[{value.get('model', '未知型号')} / {value.get('side', '?')} / {value.get('serial_number', '?')}] {progress:6.1%} 预计剩余 {eta_text}", + f"阶段：{_PHASES.get(str(value.get('phase')), value.get('phase'))}（第 {task.get('cycle') or '-'}/{task.get('cycle_count', 4)} 轮）", + f"任务：{task.get('label') or task.get('key') or '无'}；方向：{task.get('direction') or '-'}", + f"命令/反馈：{angle(motion.get('command'))}/{angle(motion.get('feedback'))}", + "任务所需 Tag：" + ("；".join(f"{view}={tags(ids)}" for view, ids in task.get("required_tag_ids_by_view", {}).items()) or "无"), + f"Tag：已识别 {tags(value.get('recognized_tag_ids', ()))}；缺失 {tags(value.get('missing_tag_ids', ()))}；过滤 {tags(value.get('rejected_tag_ids', ()))}", + f"有效样本 {acquisition.get('valid_samples', 0)}；覆盖 {float(acquisition.get('coverage_01', 0)):.1%}；分箱 {acquisition.get('bins', 0)}；内部空白 {acquisition.get('maximum_gap', 0)}；重扫 {acquisition.get('retry_count', 0)} 次", + ] + waiting = value.get("devices", {}).get("waiting_for", ()) + if waiting: + lines.append("等待项：" + "；".join(waiting)) + if motion.get("speed") is not None: + lines.append(f"运动速度：{motion['speed']} {'rad/s' if unit == 'rad' else 'SDK速度档'}") + if value.get("reference_locked"): + lines.append("基准已锁定：请勿移动手掌、相机、支架或Tag") + resume = value.get("resume", {}) + if resume: + message = resume.get('message', resume.get('reason', '')) + message = {"not_requested": "本次完整重采，未请求恢复"}.get(message, message) + lines.append(f"断点：{message}；复用 {resume.get('completed_unit_count', 0)} 单元；来源 {resume.get('source_session') or '-'}") + hardware = value.get("hardware", {}) + lines.append(f"SDK：反馈 {hardware.get('feedback_hz', '未知')} Hz；模式确认 {hardware.get('mode_verified', '未知')}；故障 {hardware.get('faults', '未知')}") + pause = value.get("pause", {}) + if pause.get("reason"): + lines.extend((f"原因 [{pause.get('code', '')}]：{pause['reason']}", f"建议：{pause.get('suggestion', '')}")) + if pause.get("details"): + lines.append(f"诊断：{pause['details']}") + return "\n".join(lines) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py new file mode 100644 index 0000000..9407f94 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/steady.py @@ -0,0 +1,35 @@ +"""Bounded, direction-specific command observations, separate from sweeps.""" + +import math +import numpy as np + + +def steady_targets(profile, unit): + lo, hi = sorted((unit.start, unit.end)) + train = np.linspace(lo, hi, profile.acquisition.steady_training_nodes) + values = train if unit.cycle != 3 else np.r_[lo, (train[:-1]+train[1:])/2, hi] + if profile.command.unit == "u8": + values = np.floor(values+0.5) + values = sorted(set(float(v) for v in values)) + return tuple(values if unit.direction == "increasing" else reversed(values)) + + +def steady_failures(profile, unit, attempt, records): + from .scan_quality import observation_streams + task = next(t for t in profile.motion.tasks if t.key == unit.task_key) + failures = [] + selected = [r for r in records if r.get("sample_phase") == "steady" + and r.get("task_name") == unit.task_key and r.get("cycle") == unit.cycle + and r.get("direction") == unit.direction and int(r.get("attempt", 1)) == attempt] + for field, name, spec in observation_streams(profile, task): + for index, target in enumerate(steady_targets(profile, unit)): + images = {r.get("image_stamp_ns") for r in selected + if r.get("sample_phase") == "steady" and r.get(field) == name + and r.get("view") == spec.view and r.get("task_name") == unit.task_key + and r.get("cycle") == unit.cycle and r.get("direction") == unit.direction + and int(r.get("attempt", 1)) == attempt and r.get("steady_index") == index + and math.isclose(float(r.get("steady_target", float("nan"))), target, abs_tol=1e-9) + and r.get("image_stamp_ns", 0) > 0} + if len(images) < profile.acquisition.steady_minimum_samples: + failures.append(f"steady:{field}:{name}:{spec.view}:node={index}:samples={len(images)}") + return tuple(failures) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py new file mode 100644 index 0000000..d83aa0c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/trajectory.py @@ -0,0 +1,271 @@ +"""Profile-driven trajectory and avoidance completion rules.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Sequence + +import numpy as np + +from ..core import CalibrationProfile, TaskSpec + + +@dataclass(frozen=True) +class AvoidanceArrival: + arrived: bool + progress_01: float + direction_correct: bool + stable: bool + + +def smoothstep_position(start: float, end: float, progress_01: float) -> float: + """Endpoint-zero-velocity cosine interpolation.""" + if not all(math.isfinite(float(value)) for value in (start, end, progress_01)): + raise ValueError("trajectory inputs must be finite") + phase = min(1.0, max(0.0, float(progress_01))) + blend = 0.5 - 0.5 * math.cos(math.pi * phase) + return float(start) + blend * (float(end) - float(start)) + + +def cosine_position_trajectory_u8( + start_u8: float, + target_u8: float, + elapsed_seconds: float, + full_range_duration_seconds: float, +) -> tuple[float, float, float]: + """Return a bounded byte-command trajectory with zero endpoint speed.""" + if full_range_duration_seconds <= 0.0: + raise ValueError("full_range_duration_seconds must be positive") + duration = ( + float(full_range_duration_seconds) + * abs(float(target_u8) - float(start_u8)) + / 255.0 + ) + if duration <= 0.0: + return float(target_u8), 1.0, 0.0 + phase = min(1.0, max(0.0, float(elapsed_seconds) / duration)) + return smoothstep_position(start_u8, target_u8, phase), phase, duration + + +def cosine_position_trajectory( + start_value: float, + target_value: float, + elapsed_seconds: float, + maximum_speed: float, +) -> tuple[float, float, float]: + """Cosine trajectory whose peak velocity does not exceed the limit.""" + if not all(math.isfinite(float(value)) for value in (start_value, target_value, elapsed_seconds)): + raise ValueError("trajectory values and clock must be finite") + speed = float(maximum_speed) + if not math.isfinite(speed) or speed <= 0.0: + raise ValueError("maximum_speed must be positive") + distance = abs(float(target_value) - float(start_value)) + if distance <= 0.0: + return float(target_value), 1.0, 0.0 + duration = math.pi * distance / (2.0 * speed) + phase = min(1.0, max(0.0, float(elapsed_seconds) / duration)) + blend = 0.5 - 0.5 * math.cos(math.pi * phase) + return ( + float(start_value) + (float(target_value) - float(start_value)) * blend, + phase, + duration, + ) + + +def cosine_ramp_velocity_trajectory( + start_value: float, + target_value: float, + elapsed_seconds: float, + maximum_speed: float, + ramp_seconds: float, +) -> tuple[float, float, float]: + """Velocity-limited trajectory with cosine ramps and a constant-speed core.""" + if not all(math.isfinite(float(value)) for value in (start_value, target_value, elapsed_seconds)): + raise ValueError("trajectory values and clock must be finite") + speed = float(maximum_speed) + ramp = float(ramp_seconds) + if not math.isfinite(speed) or speed <= 0.0: + raise ValueError("maximum_speed must be positive") + if not math.isfinite(ramp) or ramp <= 0.0: + raise ValueError("ramp_seconds must be positive") + start = float(start_value) + target = float(target_value) + 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( + 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_value: float, + *, + profile: CalibrationProfile, +) -> list[float]: + """Build one task command from the profile baseline and avoidance values.""" + values = list(profile.command.baseline_values) + for index, value in task.auxiliary_commands: + values[int(index)] = float(value) + values[int(task.command_index)] = float(command_value) + if profile.command.unit == "u8": + return [int(value) for value in values] + return values + + +def build_calibration_preparation_waypoints( + task: TaskSpec, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, + start_value: float | None = None, +) -> tuple[tuple[float, ...], ...]: + """Execute declarative avoidance groups, then the measured channel.""" + # A return sweep or its retry starts at the opposite endpoint. Never + # silently substitute the task's first endpoint and reverse/jump the hand. + start = task.start_value if start_value is None else float(start_value) + if start not in (task.start_value, task.end_value): + raise ValueError("scan preparation must target a declared sweep endpoint") + final = tuple(build_calibration_motion_command(task, start, profile=profile)) + return _grouped_waypoints(current_command, final, task.preparation_groups, + profile=profile, measured_channel=task.command_index) + + +def build_calibration_return_waypoints( + target_command: Sequence[float] | None = None, + *, + profile: CalibrationProfile, + current_command: Sequence[float] | None = None, + **_: object, +) -> tuple[tuple[float, ...], ...]: + """Return in the profile's reviewed channel-group order.""" + target = ( + tuple(float(value) for value in target_command) + if target_command is not None + else tuple(profile.command.baseline_values) + ) + if len(target) != profile.command.command_count: + raise ValueError("return command has the wrong channel count") + return _grouped_waypoints(current_command, target, profile.motion.return_groups, profile=profile) + + +def _grouped_waypoints(current_command, target, groups, *, profile, measured_channel=None): + current = list(profile.command.baseline_values if current_command is None else current_command) + final = list(target) + count = profile.command.command_count + if len(current) != count or len(final) != count: + raise ValueError("waypoint command count differs from profile") + for vector in (current, final): + if any(not math.isfinite(float(v)) or not lo <= v <= hi for v, lo, hi in zip(vector, profile.command.minimum_values, profile.command.maximum_values)): + raise ValueError("waypoint is outside command coordinates") + for index in profile.command.disabled_indices: + final[index] = current[index] + declared = [tuple(group) for group in groups] + if not declared: + return (tuple(final),) + handled = {index for group in declared for index in group} + remaining = tuple(index for index in range(count) if index not in handled and index != measured_channel and index not in profile.command.disabled_indices) + if remaining: + # Undeclared auxiliaries precede the target, never follow it. + if declared and measured_channel in declared[-1]: + declared.insert(len(declared)-1, remaining) + else: + declared.append(remaining) + if measured_channel is not None and measured_channel not in handled: + declared.append((measured_channel,)) + output = [] + for group in declared: + changed = False + for index in group: + if current[index] != final[index]: + current[index] = final[index] + changed = True + if changed: + output.append(tuple(current)) + return tuple(output or [tuple(final)]) + + +def avoidance_arrival( + *, + start_feedback: float, + requested_target: float, + feedback_history: Sequence[float], + trajectory_complete: bool, + minimum_requested_fraction: float = 0.80, + stability_tolerance: float = 0.02, +) -> AvoidanceArrival: + """Accept a safely reached waypoint without exact servo equality.""" + values = np.asarray(tuple(feedback_history), dtype=float) + requested = float(requested_target) - float(start_feedback) + if values.ndim != 1 or values.size == 0 or not np.all(np.isfinite(values)) or not math.isfinite(requested): + return AvoidanceArrival(False, 0.0, False, False) + if not 0 < minimum_requested_fraction <= 1 or not math.isfinite(stability_tolerance) or stability_tolerance <= 0: + raise ValueError("invalid avoidance completion policy") + stable = bool(values.size >= 3 and float(np.ptp(values[-min(10, values.size):])) <= float(stability_tolerance)) + if abs(requested) < 1e-9: + close = abs(float(values[-1]) - float(requested_target)) <= stability_tolerance + return AvoidanceArrival(bool(trajectory_complete and stable and close), 1.0 if close else 0.0, close, stable) + measured = float(values[-1]) - float(start_feedback) + progress = measured / requested + direction_correct = progress >= 0.0 + stable = bool( + values.size >= 3 + and float(np.ptp(values[-min(10, values.size):])) + <= float(stability_tolerance) + ) + return AvoidanceArrival( + bool( + trajectory_complete + and direction_correct + and progress >= float(minimum_requested_fraction) + and stable + ), + float(progress), + direction_correct, + stable, + ) + + +__all__ = [ + "AvoidanceArrival", + "avoidance_arrival", + "build_calibration_motion_command", + "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", + "cosine_position_trajectory_u8", + "smoothstep_position", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py index 41eeb86..c626bfa 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py @@ -1,4 +1,4 @@ """Compatibility import for the installed Chinese model diagnostics.""" -from .models.g20.reporting_zh import * # noqa: F401,F403 -from .models.g20.reporting_zh import _task_text +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.reporting_zh import * # noqa: F401,F403 +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.reporting_zh import _task_text diff --git a/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py b/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py index 9465c69..f23027c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py @@ -1,7 +1,7 @@ -"""One-release module alias for the relocated model-specific ROS node.""" +"""Legacy module spelling of the only online calibration entry.""" import sys -from .models.g20 import node as _implementation +from .runtime.ros import entrypoint as _implementation sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/trajectory.py b/src/linkerhand_calibration/linkerhand_calibration/trajectory.py index 9a32ac8..cb3d9a7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/trajectory.py +++ b/src/linkerhand_calibration/linkerhand_calibration/trajectory.py @@ -21,7 +21,6 @@ import math from typing import Any, Iterable, Mapping, Sequence import numpy as np -from scipy.optimize import least_squares from scipy.spatial.transform import Rotation from .core import ( @@ -40,11 +39,9 @@ _ROOT_ROLES = ("t3", "t4", "t5") DEFAULT_PASSIVE_IP_MULTIPLIER = 1.02 -def _vector3(value: Sequence[float], *, name: str) -> np.ndarray: - vector = np.asarray(value, dtype=float) - if vector.shape != (3,) or not np.all(np.isfinite(vector)): - raise ValueError(f"{name} must contain three finite values") - return vector +from .core.fitting.trajectory_geometry import ( + _vector3, _plane_basis, _fit_plane_axis, _fit_circle_with_axis, _project_radial, _signed_angle, _reference_radial, _angle_for_circle, _low_command_median, _orient_circle_positive, _fit_joint_curve, _regularize_coupled_zero_tail +) def _record_translations( @@ -64,204 +61,22 @@ def _record_translations( } -def _plane_basis(axis: Sequence[float]) -> tuple[np.ndarray, np.ndarray]: - normal = _vector3(axis, name="axis") - normal /= np.linalg.norm(normal) - candidates = np.eye(3) - seed = candidates[int(np.argmin(np.abs(candidates @ normal)))] - first = np.cross(normal, seed) - first /= np.linalg.norm(first) - second = np.cross(normal, first) - second /= np.linalg.norm(second) - return first, second -def _fit_plane_axis(point_sets: Sequence[np.ndarray]) -> tuple[np.ndarray, float]: - centred: list[np.ndarray] = [] - for points in point_sets: - array = np.asarray(points, dtype=float) - if array.ndim != 2 or array.shape[1] != 3 or len(array) < 3: - raise ValueError("each trajectory must contain at least three 3-D points") - # The arithmetic centroid of points in a plane remains in that plane - # and transforms correctly under a camera rotation. A component-wise - # median generally does neither. - centred.append(array - np.mean(array, axis=0)) - matrix = np.concatenate(centred, axis=0) - _, _, vh = np.linalg.svd(matrix, full_matrices=False) - axis = vh[-1] - axis /= np.linalg.norm(axis) - residuals = matrix @ axis - plane_rms = float(np.sqrt(np.mean(np.square(residuals)))) - return axis, plane_rms -def _fit_circle_with_axis( - points_xyz: Sequence[Sequence[float]], - axis_xyz: Sequence[float], -) -> dict[str, Any]: - points = np.asarray(points_xyz, dtype=float) - if ( - points.ndim != 2 - or points.shape[1] != 3 - or len(points) < 6 - or not np.all(np.isfinite(points)) - ): - raise ValueError("a circle requires at least six finite 3-D points") - axis = _vector3(axis_xyz, name="circle axis") - axis /= np.linalg.norm(axis) - basis_x, basis_y = _plane_basis(axis) - origin = np.mean(points, axis=0) - local = points - origin - xy = np.column_stack((local @ basis_x, local @ basis_y)) - - design = np.column_stack((2.0 * xy[:, 0], 2.0 * xy[:, 1], np.ones(len(xy)))) - target = np.sum(np.square(xy), axis=1) - initial_x, initial_y, constant = np.linalg.lstsq( - design, target, rcond=None - )[0] - initial_radius = math.sqrt( - max( - float(constant + initial_x * initial_x + initial_y * initial_y), - np.finfo(float).eps, - ) - ) - - def residual(parameters: np.ndarray) -> np.ndarray: - centre = parameters[:2] - radius = float(parameters[2]) - return np.linalg.norm(xy - centre, axis=1) - radius - - fitted = least_squares( - residual, - np.asarray([initial_x, initial_y, initial_radius], dtype=float), - loss="soft_l1", - f_scale=0.0005, - max_nfev=2000, - ) - centre_xy = fitted.x[:2] - radius = abs(float(fitted.x[2])) - radial_residuals = residual( - np.asarray([centre_xy[0], centre_xy[1], radius], dtype=float) - ) - centre_xyz = origin + centre_xy[0] * basis_x + centre_xy[1] * basis_y - plane_offsets = (points - centre_xyz) @ axis - centre_xyz += float(np.median(plane_offsets)) * axis - plane_residuals = (points - centre_xyz) @ axis - return { - "axis_xyz": [float(value) for value in axis], - "center_xyz_m": [float(value) for value in centre_xyz], - "radius_m": radius, - "radial_rms_m": float( - np.sqrt(np.mean(np.square(radial_residuals))) - ), - "plane_rms_m": float( - np.sqrt(np.mean(np.square(plane_residuals))) - ), - } -def _project_radial( - point_xyz: Sequence[float], - circle: Mapping[str, Any], -) -> np.ndarray: - point = _vector3(point_xyz, name="trajectory point") - centre = _vector3(circle["center_xyz_m"], name="circle centre") - axis = _vector3(circle["axis_xyz"], name="circle axis") - axis /= np.linalg.norm(axis) - radial = point - centre - radial -= float(radial @ axis) * axis - if float(np.linalg.norm(radial)) < 1.0e-9: - raise ValueError("trajectory point lies on the fitted rotation axis") - return radial -def _signed_angle( - reference_radial_xyz: Sequence[float], - observed_radial_xyz: Sequence[float], - axis_xyz: Sequence[float], -) -> float: - reference = _vector3(reference_radial_xyz, name="reference radial") - observed = _vector3(observed_radial_xyz, name="observed radial") - axis = _vector3(axis_xyz, name="angle axis") - axis /= np.linalg.norm(axis) - reference -= float(reference @ axis) * axis - observed -= float(observed @ axis) * axis - reference /= np.linalg.norm(reference) - observed /= np.linalg.norm(observed) - return math.atan2( - float(axis @ np.cross(reference, observed)), - float(np.clip(reference @ observed, -1.0, 1.0)), - ) -def _reference_radial( - records: Sequence[Mapping[str, Any]], - points: Sequence[np.ndarray], - circle: Mapping[str, Any], -) -> np.ndarray: - references = [ - _project_radial(point, circle) - for record, point in zip(records, points) - if int(record.get("command_u8", -1)) == 255 - ] - if not references: - raise ValueError("trajectory is missing command-255 reference points") - radial = np.median(np.asarray(references, dtype=float), axis=0) - axis = _vector3(circle["axis_xyz"], name="circle axis") - axis /= np.linalg.norm(axis) - radial -= float(radial @ axis) * axis - if float(np.linalg.norm(radial)) < 1.0e-9: - raise ValueError("command-255 reference is degenerate") - return radial -def _angle_for_circle( - point_xyz: Sequence[float], - circle: Mapping[str, Any], -) -> float: - return _signed_angle( - circle["reference_radial_xyz_m"], - _project_radial(point_xyz, circle), - circle["axis_xyz"], - ) -def _low_command_median( - records: Sequence[Mapping[str, Any]], - values: Sequence[float], -) -> float: - selected = [ - float(value) - for record, value in zip(records, values) - if int(record.get("command_u8", 255)) <= 16 - ] - if not selected: - selected = [ - float(value) - for _, value in sorted( - zip(records, values), - key=lambda item: int(item[0].get("command_u8", 255)), - )[: max(3, len(records) // 20)] - ] - return float(np.median(selected)) -def _orient_circle_positive( - circle: dict[str, Any], - records: Sequence[Mapping[str, Any]], - points: Sequence[np.ndarray], -) -> dict[str, Any]: - reference = _reference_radial(records, points, circle) - circle["reference_radial_xyz_m"] = [ - float(value) for value in reference - ] - values = [_angle_for_circle(point, circle) for point in points] - if _low_command_median(records, values) < 0.0: - axis = -_vector3(circle["axis_xyz"], name="circle axis") - circle["axis_xyz"] = [float(value) for value in axis] - values = [_angle_for_circle(point, circle) for point in points] - circle["observed_arc_rad"] = float(max(values) - min(values)) - return circle def _rotation_about_axis( @@ -629,145 +444,8 @@ def maximum_center_non_target_drift_rad( return float(maximum) -def _fit_joint_curve( - records: Sequence[Mapping[str, Any]], - values: Sequence[float], - *, - 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 - } - for record, value in zip(records, values): - direction = str(record["direction"]) - command = int(record["command_u8"]) - by_direction[direction][command].append(float(value)) - - fitted: dict[str, list[float]] = {} - maximum_correction = 0.0 - for direction in DIRECTIONS: - commands = np.asarray( - [ - command - for command, samples in enumerate(by_direction[direction]) - if samples - ], - dtype=int, - ) - 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" - ) - raw = np.asarray( - [ - float(np.median(by_direction[direction][command])) - for command in commands - ], - dtype=float, - ) - if not preserve_direction_offset: - raw -= raw[-1] - projected_samples = isotonic_nonincreasing(raw) - if not preserve_direction_offset: - projected_samples -= projected_samples[-1] - maximum_correction = max( - maximum_correction, - float(np.max(np.abs(projected_samples - raw))), - ) - curve = np.interp( - np.arange(256, dtype=float), - commands.astype(float), - projected_samples, - ) - if not preserve_direction_offset: - curve -= curve[255] - if endpoint_reference is not None: - curve = _regularize_coupled_zero_tail( - curve, - endpoint_reference[f"{direction}_rad"], - ) - fitted[direction] = [ - round(float(value), 8) for value in curve - ] - - decreasing = np.asarray(fitted[DIRECTION_DECREASING], dtype=float) - increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float) - hysteresis = float(np.max(np.abs(decreasing - increasing))) - combined = 0.5 * (decreasing + increasing) - if not preserve_direction_offset: - combined -= combined[255] - return ( - { - "angle_rad": [round(float(value), 8) for value in combined], - "decreasing_rad": fitted[DIRECTION_DECREASING], - "increasing_rad": fitted[DIRECTION_INCREASING], - }, - maximum_correction, - hysteresis, - ) -def _regularize_coupled_zero_tail( - values: Sequence[float], - reference_values: Sequence[float], - *, - maximum_tail_commands: int = 16, - zero_tolerance_rad: float = 1.0e-10, -) -> np.ndarray: - """Replace a short noise-created zero tail using a coupled joint shape. - - The passive IP and active MCP share motor 15. Close to command 255 the - IP centre trajectory is small enough that measurement noise can become - negative. Isotonic projection correctly prevents a negative angle, but - otherwise turns all remaining commands into an artificial zero plateau. - Continue the last resolved IP/MCP ratio over a short tail instead. This - changes neither the resolved part of the curve nor the exact 255 zero. - """ - curve = np.asarray(values, dtype=float).copy() - reference = np.asarray(reference_values, dtype=float) - if curve.shape != (256,) or reference.shape != (256,): - raise ValueError("endpoint curves must each contain 256 values") - if not np.all(np.isfinite(curve)) or not np.all(np.isfinite(reference)): - raise ValueError("endpoint curves must be finite") - maximum_tail = int(maximum_tail_commands) - tolerance = float(zero_tolerance_rad) - if maximum_tail < 2 or tolerance < 0.0: - raise ValueError("invalid endpoint regularization settings") - - anchor = 254 - while anchor >= 0 and abs(float(curve[anchor])) <= tolerance: - anchor -= 1 - tail_commands = 255 - anchor - if ( - anchor < 0 - or tail_commands < 2 - or tail_commands > maximum_tail - or curve[anchor] <= tolerance - or reference[anchor] <= tolerance - or abs(float(curve[255])) > tolerance - or abs(float(reference[255])) > tolerance - ): - curve[255] = 0.0 - return curve - - ratio = float(curve[anchor] / reference[anchor]) - continuation = np.maximum( - 0.0, - ratio * reference[anchor + 1 :], - ) - continuation = np.minimum.accumulate(continuation) - continuation = np.minimum(continuation, float(curve[anchor])) - continuation[-1] = 0.0 - curve[anchor + 1 :] = continuation - return curve def _derive_mimic_joint_curves( diff --git a/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py b/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py index edfb635..fc89a2f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py +++ b/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py @@ -2,6 +2,6 @@ import sys -from .models.g20 import zero_solver as _implementation +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20 import zero_solver as _implementation sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/zero_calibration.py b/src/linkerhand_calibration/linkerhand_calibration/zero_calibration.py index 31aee58..c47ce68 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/zero_calibration.py +++ b/src/linkerhand_calibration/linkerhand_calibration/zero_calibration.py @@ -7,20 +7,13 @@ from typing import Any, Mapping, Sequence import cv2 import numpy as np -from scipy.optimize import least_squares -def wrap_angle_rad(angle_rad: float) -> float: - """Wrap one finite angle to [-pi, pi).""" - angle = float(angle_rad) - if not math.isfinite(angle): - raise ValueError("angle must be finite") - return (angle + math.pi) % (2.0 * math.pi) - math.pi +from .core.fitting.circle_geometry import ( + wrap_angle_rad, signed_angle_difference_rad, circular_mean_rad, circular_median_rad, circular_std_rad, maximum_pairwise_angle_difference_rad, _fit_circle, _trajectory_arc_rad +) -def signed_angle_difference_rad(angle_rad: float, reference_rad: float) -> float: - """Return the wrapped signed rotation from reference to angle.""" - return wrap_angle_rad(float(angle_rad) - float(reference_rad)) def tag_x_axis_angle_rad( @@ -38,61 +31,12 @@ def tag_x_axis_angle_rad( ) -def circular_mean_rad(angles_rad: Sequence[float]) -> float: - """Return the circular mean of at least one finite angle.""" - angles = np.asarray(angles_rad, dtype=float) - if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)): - raise ValueError("angles_rad must contain finite angles") - vector = complex( - float(np.mean(np.cos(angles))), - float(np.mean(np.sin(angles))), - ) - if abs(vector) < 1.0e-12: - raise ValueError("angles do not have a unique circular mean") - return wrap_angle_rad(math.atan2(vector.imag, vector.real)) -def circular_median_rad(angles_rad: Sequence[float]) -> float: - """Return a robust circular median around the circular mean.""" - angles = np.asarray(angles_rad, dtype=float) - reference = circular_mean_rad(angles) - deltas = np.asarray( - [signed_angle_difference_rad(value, reference) for value in angles], - dtype=float, - ) - return wrap_angle_rad(reference + float(np.median(deltas))) -def circular_std_rad(angles_rad: Sequence[float]) -> float: - """Return circular standard deviation in radians.""" - angles = np.asarray(angles_rad, dtype=float) - if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)): - raise ValueError("angles_rad must contain finite angles") - resultant = float( - math.hypot( - float(np.mean(np.cos(angles))), - float(np.mean(np.sin(angles))), - ) - ) - resultant = min(1.0, max(1.0e-15, resultant)) - return math.sqrt(max(0.0, -2.0 * math.log(resultant))) -def maximum_pairwise_angle_difference_rad( - angles_rad: Sequence[float], -) -> float: - """Return the largest wrapped distance between any two angles.""" - angles = [float(value) for value in angles_rad] - if not angles or not all(math.isfinite(value) for value in angles): - raise ValueError("angles_rad must contain finite angles") - return max( - ( - abs(signed_angle_difference_rad(left, right)) - for index, left in enumerate(angles) - for right in angles[index + 1:] - ), - default=0.0, - ) def detect_reference_alignment_line( @@ -253,72 +197,8 @@ def summarize_zero_frames( } -def _fit_circle(points_xy: np.ndarray) -> tuple[np.ndarray, float]: - """Fit a geometric circle with robust nonlinear least squares.""" - points = np.asarray(points_xy, dtype=float) - if points.ndim != 2 or points.shape[1] != 2 or len(points) < 3: - raise ValueError("circle fit requires at least three 2D points") - origin = np.mean(points, axis=0) - centered = points - origin - matrix = np.column_stack( - (2.0 * centered[:, 0], 2.0 * centered[:, 1], np.ones(len(points))) - ) - rhs = np.sum(np.square(centered), axis=1) - solution, _, rank, _ = np.linalg.lstsq(matrix, rhs, rcond=None) - if rank < 3: - raise ValueError("trajectory points are degenerate") - centre0 = solution[:2] + origin - radius0_squared = ( - float(solution[2]) + float(np.dot(solution[:2], solution[:2])) - ) - if radius0_squared <= 0.0: - raise ValueError("trajectory circle radius is invalid") - radius0 = math.sqrt(radius0_squared) - - def residual(parameters: np.ndarray) -> np.ndarray: - centre = parameters[:2] - radius = parameters[2] - return np.linalg.norm(points - centre, axis=1) - radius - - initial_error = residual(np.asarray([*centre0, radius0], dtype=float)) - robust_scale = max( - 0.25, - 1.4826 - * float( - np.median( - np.abs(initial_error - float(np.median(initial_error))) - ) - ), - ) - result = least_squares( - residual, - np.asarray([*centre0, radius0], dtype=float), - loss="soft_l1", - f_scale=robust_scale, - max_nfev=1000, - ) - centre = np.asarray(result.x[:2], dtype=float) - radius = float(abs(result.x[2])) - if not result.success or not np.all(np.isfinite(centre)): - raise ValueError("trajectory circle optimization failed") - if not math.isfinite(radius) or radius <= 0.0: - raise ValueError("trajectory circle radius is invalid") - return centre, radius -def _trajectory_arc_rad(points_xy: np.ndarray, centre_xy: np.ndarray) -> float: - angles = np.mod( - np.arctan2( - points_xy[:, 1] - centre_xy[1], - points_xy[:, 0] - centre_xy[0], - ), - 2.0 * math.pi, - ) - if len(angles) < 2: - return 0.0 - angles = np.sort(angles) - gaps = np.diff(np.r_[angles, angles[0] + 2.0 * math.pi]) - return float(2.0 * math.pi - np.max(gaps)) def fit_image_circle_trajectory( diff --git a/src/linkerhand_calibration/linkerhand_calibration/zero_node.py b/src/linkerhand_calibration/linkerhand_calibration/zero_node.py deleted file mode 100644 index 07bdfd1..0000000 --- a/src/linkerhand_calibration/linkerhand_calibration/zero_node.py +++ /dev/null @@ -1,1608 +0,0 @@ -"""ROS 2 node for trajectory-circle G20 CMC zero/travel measurement.""" - -from __future__ import annotations - -from collections import deque -import json -import math -from pathlib import Path -import re -import time -from typing import Any - -from apriltag_msgs.msg import AprilTagDetectionArray -import cv2 -from cv_bridge import CvBridge -import numpy as np -import rclpy -from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data -from sensor_msgs.msg import Image, JointState -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from .compat.legacy.thumb_core import ( - BASELINE_COMMAND, - COMMAND_NAMES, - build_command, -) -from .storage import atomic_write_json -from .zero_calibration import ( - build_trajectory_zero_angle_payload, - build_trajectory_zero_travel_payload, - detect_reference_alignment_line, - fit_image_circle_trajectory, - measure_zero_from_circle, - summarize_zero_frames, - validate_zero_angle_payload, - validate_zero_travel_payload, -) - - -STATE_PREFLIGHT = "PREFLIGHT" -STATE_WAIT_START = "WAIT_START" -STATE_MOVING = "MOVING" -STATE_CAPTURING = "CAPTURING" -STATE_SWEEPING = "SWEEPING" -STATE_PAUSED = "PAUSED" -STATE_ABORTED = "ABORTED" -STATE_COMPLETE = "COMPLETE" - -STATE_ZH = { - STATE_PREFLIGHT: "标定前检查", - STATE_WAIT_START: "等待开始", - STATE_MOVING: "机械手运动中", - STATE_CAPTURING: "采集零位角", - STATE_SWEEPING: "采集圆心轨迹", - STATE_PAUSED: "标定已暂停", - STATE_ABORTED: "标定已终止", - STATE_COMPLETE: "标定完成", -} - -STAGE_ZH = { - "zero_before": "255零位（运动前）", - "sweep_out": "圆心轨迹（255到轨迹端点）", - "travel_endpoint": "行程端点", - "sweep_return": "圆心轨迹（返回255）", - "zero_after": "255零位（返回后）", -} - - -def _safe_name(value: str) -> str: - safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) - return safe or "UNSET" - - -class G20ThumbCmcTrajectoryNode(Node): - """Measure a CMC zero and optional travel from the T3-centre circle.""" - - def __init__(self) -> None: - super().__init__("g20_thumb_cmc_trajectory_calibration") - self._declare_parameters() - self._load_parameters() - - self.session_dir.mkdir(parents=True, exist_ok=True) - result_kind = "zero_travel" if self.measure_travel else "zero" - self.result_path = self.session_dir / ( - f"g20_left_{_safe_name(self.serial_number)}" - f"_{self.joint_name}_{result_kind}.json" - ) - - self.state = STATE_PREFLIGHT - self.reason = "waiting_for_t0_t3_and_hand_state" - self.latest_state_u8: tuple[float, ...] = () - self.latest_corners: dict[str, np.ndarray] = {} - self.latest_quality: dict[str, dict[str, Any]] = {} - self.latest_angles: dict[str, float] = {} - self.preflight_flags: deque[bool] = deque(maxlen=self.preflight_frames) - self.camera_alignment_measurements: deque[dict[str, Any] | None] = deque( - maxlen=self.camera_alignment_required_frames - ) - self.latest_camera_alignment: dict[str, Any] = {} - self.last_camera_alignment_update = 0.0 - self.session_detection_flags: list[bool] = [] - - self.round_index = 0 - self.rounds: list[dict[str, Any]] = [] - self.current_round: dict[str, Any] = {} - self.active_stage: str | None = None - self.active_target_u8: int | None = None - self.required_frames = 0 - self.stage_started_at = 0.0 - self.target_reached_at: float | None = None - self.capture_started_at = 0.0 - self.capture_frames: deque[dict[str, float]] = deque() - self.trajectory_frames: list[dict[str, float]] = [] - self.latest_trajectory_frames: list[dict[str, float]] = [] - self.latest_circle: dict[str, Any] = {} - self.latest_travel: dict[str, float] = {} - self.final_payload: dict[str, Any] | None = None - self.final_report: dict[str, Any] = {} - - self.command_publisher = self.create_publisher( - JointState, self.command_topic, 1 - ) - self.status_publisher = self.create_publisher(String, "~/status", 10) - self.status_text_publisher = self.create_publisher( - String, "~/status_text", 10 - ) - self.debug_publisher = None - self.bridge = CvBridge() - self.last_debug_publish = 0.0 - if self.publish_debug_image: - self.debug_publisher = self.create_publisher( - Image, "~/debug_image", qos_profile_sensor_data - ) - - self.create_subscription( - JointState, self.state_topic, self._state_callback, 10 - ) - self.create_subscription( - AprilTagDetectionArray, - self.detections_topic, - self._detections_callback, - qos_profile_sensor_data, - ) - if self.publish_debug_image or self.camera_alignment_enabled: - self.create_subscription( - Image, - self.image_topic, - self._image_callback, - qos_profile_sensor_data, - ) - - self.create_service(Trigger, "~/start", self._start_callback) - self.create_service(Trigger, "~/abort", self._abort_callback) - self.timer = self.create_timer(0.05, self._timer_callback) - self.status_timer = self.create_timer(0.5, self._publish_status) - self.get_logger().info( - f"{self.joint_name} calibration session: " - f"{self.session_dir}; result={self.result_path}" - ) - - def _declare_parameters(self) -> None: - self.declare_parameter("serial_number", "UNSET") - self.declare_parameter("session_dir", "calibration_output/session") - self.declare_parameter("commands_enabled", True) - self.declare_parameter( - "command_topic", "/g20/cb_left_hand_control_cmd" - ) - self.declare_parameter("state_topic", "/g20/cb_left_hand_state") - self.declare_parameter( - "detections_topic", "/apriltag/detections" - ) - self.declare_parameter( - "image_topic", "/camera/camera/color/image_rect" - ) - self.declare_parameter("publish_debug_image", True) - self.declare_parameter("debug_max_rate_hz", 10.0) - self.declare_parameter("debug_scale", 0.75) - self.declare_parameter("camera_alignment_enabled", False) - self.declare_parameter("camera_alignment_reference_y_ratio", 0.90) - self.declare_parameter("camera_alignment_roi_y_min_ratio", 0.55) - self.declare_parameter("camera_alignment_roi_y_max_ratio", 0.98) - self.declare_parameter("camera_alignment_minimum_line_length_ratio", 0.30) - self.declare_parameter("camera_alignment_max_candidate_angle_deg", 15.0) - self.declare_parameter("camera_alignment_max_angle_deg", 0.5) - self.declare_parameter("camera_alignment_max_vertical_offset_px", 12.0) - self.declare_parameter("camera_alignment_required_frames", 10) - self.declare_parameter("camera_alignment_minimum_detection_rate", 0.8) - self.declare_parameter("camera_alignment_max_age_seconds", 1.0) - self.declare_parameter("t0_id", 0) - self.declare_parameter("t3_id", 1) - self.declare_parameter("joint_name", "thumb_cmc_pitch") - self.declare_parameter("motor_index", 0) - self.declare_parameter("zero_command_u8", 255) - self.declare_parameter("measure_travel", False) - self.declare_parameter( - "baseline_command_u8", list(BASELINE_COMMAND) - ) - self.declare_parameter("repetitions", 3) - self.declare_parameter("zero_capture_frames", 30) - self.declare_parameter("trajectory_command_u8", 64) - self.declare_parameter("trajectory_bin_size_u8", 8.0) - self.declare_parameter("trajectory_minimum_frames", 45) - self.declare_parameter("trajectory_minimum_bins", 18) - self.declare_parameter("trajectory_minimum_state_span_u8", 160.0) - self.declare_parameter("trajectory_minimum_radius_px", 20.0) - self.declare_parameter("trajectory_minimum_arc_deg", 20.0) - self.declare_parameter("trajectory_maximum_radial_rms_px", 2.0) - self.declare_parameter( - "trajectory_maximum_p95_radial_error_px", 3.5 - ) - self.declare_parameter("trajectory_endpoint_settle_seconds", 0.3) - self.declare_parameter("trajectory_timeout_seconds", 30.0) - self.declare_parameter("settle_seconds", 0.5) - self.declare_parameter("move_timeout_seconds", 20.0) - self.declare_parameter("capture_timeout_seconds", 15.0) - self.declare_parameter("state_tolerance_u8", 2.0) - self.declare_parameter("preflight_frames", 60) - self.declare_parameter("minimum_detection_rate", 0.95) - self.declare_parameter("maximum_hamming", 0) - self.declare_parameter("minimum_decision_margin", 30.0) - self.declare_parameter("minimum_edge_pixels", 40.0) - self.declare_parameter("maximum_round_difference_deg", 1.0) - self.declare_parameter("maximum_travel_difference_deg", 1.0) - self.declare_parameter("minimum_travel_deg", 10.0) - self.declare_parameter("maximum_return_error_deg", 1.0) - self.declare_parameter("maximum_zero_radial_error_px", 4.0) - self.declare_parameter("maximum_static_position_rms_px", 1.5) - - def _load_parameters(self) -> None: - def value(name: str) -> Any: - return self.get_parameter(name).value - - self.serial_number = str(value("serial_number")) - if ( - not self.serial_number - or self.serial_number == "UNSET" - or re.fullmatch(r"[A-Za-z0-9_.-]+", self.serial_number) is None - ): - raise ValueError("serial_number is required and must be path-safe") - self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() - self.commands_enabled = bool(value("commands_enabled")) - self.command_topic = str(value("command_topic")) - self.state_topic = str(value("state_topic")) - self.detections_topic = str(value("detections_topic")) - self.image_topic = str(value("image_topic")) - self.publish_debug_image = bool(value("publish_debug_image")) - self.debug_max_rate_hz = float(value("debug_max_rate_hz")) - self.debug_scale = float(value("debug_scale")) - self.camera_alignment_enabled = bool( - value("camera_alignment_enabled") - ) - self.camera_alignment_reference_y_ratio = float( - value("camera_alignment_reference_y_ratio") - ) - self.camera_alignment_roi_y_min_ratio = float( - value("camera_alignment_roi_y_min_ratio") - ) - self.camera_alignment_roi_y_max_ratio = float( - value("camera_alignment_roi_y_max_ratio") - ) - self.camera_alignment_minimum_line_length_ratio = float( - value("camera_alignment_minimum_line_length_ratio") - ) - self.camera_alignment_max_candidate_angle_rad = math.radians( - float(value("camera_alignment_max_candidate_angle_deg")) - ) - self.camera_alignment_max_angle_rad = math.radians( - float(value("camera_alignment_max_angle_deg")) - ) - self.camera_alignment_max_vertical_offset_px = float( - value("camera_alignment_max_vertical_offset_px") - ) - self.camera_alignment_required_frames = int( - value("camera_alignment_required_frames") - ) - self.camera_alignment_minimum_detection_rate = float( - value("camera_alignment_minimum_detection_rate") - ) - self.camera_alignment_max_age_seconds = float( - value("camera_alignment_max_age_seconds") - ) - self.t0_id = int(value("t0_id")) - self.t3_id = int(value("t3_id")) - self.role_by_id = {self.t0_id: "t0", self.t3_id: "t3"} - if self.t0_id == self.t3_id: - raise ValueError("t0_id and t3_id must differ") - self.joint_name = str(value("joint_name")) - self.motor_index = int(value("motor_index")) - self.zero_command_u8 = int(value("zero_command_u8")) - self.measure_travel = bool(value("measure_travel")) - self.baseline_command = tuple( - int(item) for item in value("baseline_command_u8") - ) - self.repetitions = int(value("repetitions")) - self.zero_capture_frames = int(value("zero_capture_frames")) - self.trajectory_command_u8 = int(value("trajectory_command_u8")) - self.trajectory_bin_size_u8 = float(value("trajectory_bin_size_u8")) - self.trajectory_minimum_frames = int( - value("trajectory_minimum_frames") - ) - self.trajectory_minimum_bins = int(value("trajectory_minimum_bins")) - self.trajectory_minimum_state_span_u8 = float( - value("trajectory_minimum_state_span_u8") - ) - self.trajectory_minimum_radius_px = float( - value("trajectory_minimum_radius_px") - ) - self.trajectory_minimum_arc_rad = math.radians( - float(value("trajectory_minimum_arc_deg")) - ) - self.trajectory_maximum_radial_rms_px = float( - value("trajectory_maximum_radial_rms_px") - ) - self.trajectory_maximum_p95_radial_error_px = float( - value("trajectory_maximum_p95_radial_error_px") - ) - self.trajectory_endpoint_settle_seconds = float( - value("trajectory_endpoint_settle_seconds") - ) - self.trajectory_timeout_seconds = float( - value("trajectory_timeout_seconds") - ) - self.settle_seconds = float(value("settle_seconds")) - self.move_timeout_seconds = float(value("move_timeout_seconds")) - self.capture_timeout_seconds = float(value("capture_timeout_seconds")) - self.state_tolerance_u8 = float(value("state_tolerance_u8")) - self.preflight_frames = int(value("preflight_frames")) - self.minimum_detection_rate = float(value("minimum_detection_rate")) - self.maximum_hamming = int(value("maximum_hamming")) - self.minimum_decision_margin = float( - value("minimum_decision_margin") - ) - self.minimum_edge_pixels = float(value("minimum_edge_pixels")) - self.maximum_round_difference_rad = math.radians( - float(value("maximum_round_difference_deg")) - ) - self.maximum_travel_difference_rad = math.radians( - float(value("maximum_travel_difference_deg")) - ) - self.minimum_travel_rad = math.radians( - float(value("minimum_travel_deg")) - ) - self.maximum_return_error_rad = math.radians( - float(value("maximum_return_error_deg")) - ) - self.maximum_zero_radial_error_px = float( - value("maximum_zero_radial_error_px") - ) - self.maximum_static_position_rms_px = float( - value("maximum_static_position_rms_px") - ) - self._validate_parameters() - - def _validate_parameters(self) -> None: - expected_motor_indices = { - "thumb_cmc_pitch": 0, - "thumb_cmc_roll": 5, - } - if self.joint_name not in expected_motor_indices: - raise ValueError( - "joint_name must be thumb_cmc_pitch or thumb_cmc_roll" - ) - if self.motor_index != expected_motor_indices[self.joint_name]: - raise ValueError( - f"{self.joint_name} requires motor_index " - f"{expected_motor_indices[self.joint_name]}" - ) - if self.measure_travel and self.joint_name != "thumb_cmc_roll": - raise ValueError( - "measure_travel is currently supported only for thumb_cmc_roll" - ) - if not ( - 0.0 - <= self.camera_alignment_roi_y_min_ratio - < self.camera_alignment_reference_y_ratio - < self.camera_alignment_roi_y_max_ratio - <= 1.0 - ): - raise ValueError( - "camera alignment ratios must satisfy " - "0 <= roi_min < reference < roi_max <= 1" - ) - if not ( - 0.0 < self.camera_alignment_minimum_line_length_ratio <= 1.0 - ): - raise ValueError( - "camera_alignment_minimum_line_length_ratio must be in (0, 1]" - ) - if not ( - self.camera_alignment_max_angle_rad - < self.camera_alignment_max_candidate_angle_rad - < math.pi / 2.0 - ): - raise ValueError( - "camera alignment acceptance angle must be smaller than " - "the candidate angle" - ) - if self.camera_alignment_required_frames < 3: - raise ValueError( - "camera_alignment_required_frames must be at least 3" - ) - if not 0.0 < self.camera_alignment_minimum_detection_rate <= 1.0: - raise ValueError( - "camera_alignment_minimum_detection_rate must be in (0, 1]" - ) - if len(self.baseline_command) != 20: - raise ValueError("baseline_command_u8 must contain 20 values") - if any(value < 0 or value > 255 for value in self.baseline_command): - raise ValueError("baseline command values must be in [0, 255]") - if not 0 <= self.zero_command_u8 <= 255: - raise ValueError("zero_command_u8 must be in [0, 255]") - if self.baseline_command[self.motor_index] != self.zero_command_u8: - raise ValueError( - "baseline command for the calibrated motor must equal " - "zero_command_u8" - ) - if self.repetitions != 3: - raise ValueError("CMC zero calibration requires exactly 3 repetitions") - if self.zero_capture_frames != 30: - raise ValueError("zero_capture_frames must be 30") - if not 0 <= self.trajectory_command_u8 <= 255: - raise ValueError("trajectory_command_u8 must be in [0, 255]") - if self.trajectory_command_u8 == self.zero_command_u8: - raise ValueError( - "trajectory_command_u8 must differ from zero_command_u8" - ) - available_span = abs( - self.zero_command_u8 - self.trajectory_command_u8 - ) - if self.trajectory_minimum_state_span_u8 > available_span: - raise ValueError( - "trajectory_minimum_state_span_u8 exceeds commanded span" - ) - if self.trajectory_minimum_frames < 12: - raise ValueError("trajectory_minimum_frames must be at least 12") - if self.trajectory_minimum_bins < 6: - raise ValueError("trajectory_minimum_bins must be at least 6") - if min( - self.debug_max_rate_hz, - self.debug_scale, - self.camera_alignment_max_angle_rad, - self.camera_alignment_max_vertical_offset_px, - self.camera_alignment_max_age_seconds, - self.trajectory_bin_size_u8, - self.trajectory_minimum_state_span_u8, - self.trajectory_minimum_radius_px, - self.trajectory_minimum_arc_rad, - self.trajectory_maximum_radial_rms_px, - self.trajectory_maximum_p95_radial_error_px, - self.trajectory_endpoint_settle_seconds, - self.trajectory_timeout_seconds, - self.settle_seconds, - self.move_timeout_seconds, - self.capture_timeout_seconds, - self.state_tolerance_u8, - self.minimum_decision_margin, - self.minimum_edge_pixels, - self.maximum_round_difference_rad, - self.maximum_travel_difference_rad, - self.minimum_travel_rad, - self.maximum_return_error_rad, - self.maximum_zero_radial_error_px, - self.maximum_static_position_rms_px, - ) <= 0.0: - raise ValueError("timing and quality thresholds must be positive") - if self.preflight_frames < 10: - raise ValueError("preflight_frames must be at least 10") - if not 0.0 < self.minimum_detection_rate <= 1.0: - raise ValueError("minimum_detection_rate must be in (0, 1]") - - def _state_callback(self, message: JointState) -> None: - if len(message.position) != 20: - return - if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): - lookup = dict(zip(message.name, message.position)) - self.latest_state_u8 = tuple( - float(lookup[name]) for name in COMMAND_NAMES - ) - else: - self.latest_state_u8 = tuple( - float(item) for item in message.position - ) - - def _detections_callback( - self, message: AprilTagDetectionArray - ) -> None: - corners_by_role: dict[str, np.ndarray] = {} - quality_by_role: dict[str, dict[str, Any]] = {} - for detection in message.detections: - role = self.role_by_id.get(int(detection.id)) - if role is None: - continue - corners = np.asarray( - [ - [float(point.x), float(point.y)] - for point in detection.corners - ], - dtype=float, - ) - if corners.shape != (4, 2): - continue - edges = np.linalg.norm( - corners - np.roll(corners, -1, axis=0), - axis=1, - ) - quality_by_role[role] = { - "hamming": int(detection.hamming), - "decision_margin": float(detection.decision_margin), - "edge_pixels": float(np.mean(edges)), - } - corners_by_role[role] = corners - - good = set(corners_by_role) == {"t0", "t3"} and all( - self._quality_valid(quality_by_role[role]) - for role in ("t0", "t3") - ) - self.preflight_flags.append(good) - if self.state not in { - STATE_PREFLIGHT, - STATE_WAIT_START, - STATE_COMPLETE, - STATE_ABORTED, - }: - self.session_detection_flags.append(good) - self.latest_corners = corners_by_role - self.latest_quality = quality_by_role - if not good: - return - - t0_centre = np.mean(corners_by_role["t0"], axis=0) - t3_centre = np.mean(corners_by_role["t3"], axis=0) - frame = { - "t0_x_px": float(t0_centre[0]), - "t0_y_px": float(t0_centre[1]), - "t3_x_px": float(t3_centre[0]), - "t3_y_px": float(t3_centre[1]), - "state_u8": ( - float(self.latest_state_u8[self.motor_index]) - if len(self.latest_state_u8) == 20 - else float("nan") - ), - } - if self.latest_circle: - try: - live_summary = summarize_zero_frames([frame]) - self.latest_angles = measure_zero_from_circle( - self.latest_circle, live_summary - ) - except ValueError: - self.latest_angles = {} - - if self.state == STATE_SWEEPING: - if len(self.latest_state_u8) == 20: - self.trajectory_frames.append(frame) - return - if self.state != STATE_CAPTURING: - return - if not self._motor_at_target(): - return - self.capture_frames.append(frame) - while len(self.capture_frames) > self.required_frames: - self.capture_frames.popleft() - if len(self.capture_frames) < self.required_frames: - return - summary = summarize_zero_frames(list(self.capture_frames)) - if ( - summary["relative_position_rms_px"] - > self.maximum_static_position_rms_px - ): - self.reason = ( - f"{self.active_stage}_position_not_stable:" - f"{summary['relative_position_rms_px']:.2f}px" - ) - return - self._finish_capture(summary) - - def _quality_valid(self, quality: dict[str, Any]) -> bool: - return bool( - int(quality["hamming"]) <= self.maximum_hamming - and float(quality["decision_margin"]) - >= self.minimum_decision_margin - and float(quality["edge_pixels"]) >= self.minimum_edge_pixels - ) - - def _update_camera_alignment( - self, image: np.ndarray, now: float - ) -> None: - if not self.camera_alignment_enabled: - return - height = int(image.shape[0]) - reference_y = self.camera_alignment_reference_y_ratio * float( - height - 1 - ) - detected = detect_reference_alignment_line( - image, - reference_y_px=reference_y, - roi_y_min_ratio=self.camera_alignment_roi_y_min_ratio, - roi_y_max_ratio=self.camera_alignment_roi_y_max_ratio, - minimum_length_ratio=( - self.camera_alignment_minimum_line_length_ratio - ), - maximum_candidate_angle_rad=( - self.camera_alignment_max_candidate_angle_rad - ), - ) - self.last_camera_alignment_update = now - self.camera_alignment_measurements.append(detected) - valid_measurements = [ - measurement - for measurement in self.camera_alignment_measurements - if measurement is not None - ] - window_frames = len(self.camera_alignment_measurements) - detected_frames = len(valid_measurements) - detection_rate = ( - float(detected_frames) / float(window_frames) - if window_frames - else 0.0 - ) - if not valid_measurements: - self.latest_camera_alignment = { - "enabled": True, - "detected": False, - "sample_passed": False, - "window_frames": window_frames, - "detected_frames": 0, - "detection_rate": 0.0, - } - else: - latest_valid = valid_measurements[-1] - line_xyxy = np.median( - np.asarray( - [ - measurement["line_xyxy_px"] - for measurement in valid_measurements - ], - dtype=float, - ), - axis=0, - ) - angle_rad = float( - np.median( - [ - float(measurement["angle_rad"]) - for measurement in valid_measurements - ] - ) - ) - vertical_offset_px = float( - np.median( - [ - float(measurement["vertical_offset_px"]) - for measurement in valid_measurements - ] - ) - ) - sample_passed = bool( - abs(angle_rad) <= self.camera_alignment_max_angle_rad - and abs(vertical_offset_px) - <= self.camera_alignment_max_vertical_offset_px - ) - self.latest_camera_alignment = { - "enabled": True, - "detected": True, - "line_xyxy_px": [float(value) for value in line_xyxy], - "angle_rad": angle_rad, - "vertical_offset_px": vertical_offset_px, - "reference_y_px": float(latest_valid["reference_y_px"]), - "roi_y_px": list(latest_valid["roi_y_px"]), - "sample_passed": sample_passed, - "window_frames": window_frames, - "detected_frames": detected_frames, - "detection_rate": detection_rate, - } - self.latest_camera_alignment.update( - { - "required_frames": self.camera_alignment_required_frames, - "minimum_detection_rate": ( - self.camera_alignment_minimum_detection_rate - ), - "maximum_angle_rad": self.camera_alignment_max_angle_rad, - "maximum_vertical_offset_px": ( - self.camera_alignment_max_vertical_offset_px - ), - "ready": self._camera_alignment_ready(now), - } - ) - - def _camera_alignment_ready(self, now: float | None = None) -> bool: - if not self.camera_alignment_enabled: - return True - current_time = time.monotonic() if now is None else float(now) - return bool( - self.latest_camera_alignment.get("detected", False) - and current_time - self.last_camera_alignment_update - <= self.camera_alignment_max_age_seconds - and int(self.latest_camera_alignment.get("window_frames", 0)) - == self.camera_alignment_required_frames - and float(self.latest_camera_alignment.get("detection_rate", 0.0)) - >= self.camera_alignment_minimum_detection_rate - and bool( - self.latest_camera_alignment.get("sample_passed", False) - ) - ) - - def _start_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state not in {STATE_WAIT_START, STATE_PAUSED}: - response.success = False - response.message = f"current state is {self.state}" - return response - if not self.commands_enabled: - response.success = False - response.message = "commands_enabled is false" - return response - if len(self.latest_state_u8) != 20: - response.success = False - response.message = "hand state is not available" - return response - if self.command_publisher.get_subscription_count() < 1: - response.success = False - response.message = "hand SDK is not subscribed to command topic" - return response - if self.count_publishers(self.command_topic) > 1: - response.success = False - response.message = "another node is publishing hand commands" - return response - if self._preflight_rate() < self.minimum_detection_rate: - response.success = False - response.message = "T0/T3 detection rate is below threshold" - return response - self._reset_run() - self._begin_stage("zero_before") - response.success = True - response.message = f"{self.joint_name} calibration started" - return response - - def _abort_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state in {STATE_COMPLETE, STATE_ABORTED}: - response.success = False - response.message = f"current state is {self.state}" - return response - self.state = STATE_ABORTED - self.reason = "aborted_by_operator_no_motion_command_sent" - self.capture_frames.clear() - response.success = True - response.message = "aborted; no automatic hand motion was sent" - return response - - def _reset_run(self) -> None: - self.round_index = 0 - self.rounds = [] - self.current_round = {} - self.active_stage = None - self.active_target_u8 = None - self.capture_frames.clear() - self.trajectory_frames = [] - self.latest_trajectory_frames = [] - self.latest_circle = {} - self.latest_angles = {} - self.latest_travel = {} - self.session_detection_flags = [] - self.final_payload = None - self.final_report = {} - - def _begin_stage(self, stage: str) -> None: - allowed_stages = {"zero_before", "zero_after"} - if self.measure_travel: - allowed_stages.add("travel_endpoint") - if stage not in allowed_stages: - raise ValueError(f"unknown stage {stage}") - self.active_stage = stage - self.active_target_u8 = ( - self.trajectory_command_u8 - if stage == "travel_endpoint" - else self.zero_command_u8 - ) - self.required_frames = self.zero_capture_frames - self.capture_frames = deque(maxlen=self.required_frames) - self.stage_started_at = time.monotonic() - self.target_reached_at = None - self.capture_started_at = 0.0 - self.state = STATE_MOVING - self.reason = ( - f"round_{self.round_index + 1}:{stage}:" - f"target={self.active_target_u8}" - ) - self._publish_hand_command( - build_command( - self.motor_index, - self.active_target_u8, - self.baseline_command, - ) - ) - - def _begin_sweep(self, stage: str) -> None: - if stage not in {"sweep_out", "sweep_return"}: - raise ValueError(f"unknown sweep stage {stage}") - if stage == "sweep_out": - self.trajectory_frames = [] - target = self.trajectory_command_u8 - else: - target = self.zero_command_u8 - self.active_stage = stage - self.active_target_u8 = target - self.required_frames = self.trajectory_minimum_frames - self.stage_started_at = time.monotonic() - self.target_reached_at = None - self.state = STATE_SWEEPING - self.reason = ( - f"round_{self.round_index + 1}:{stage}:" - f"target={target}:collecting_trajectory" - ) - self._publish_hand_command( - build_command(self.motor_index, target, self.baseline_command) - ) - - def _fit_trajectory( - self, observations: list[dict[str, float]] - ) -> dict[str, Any]: - return fit_image_circle_trajectory( - observations, - bin_size_u8=self.trajectory_bin_size_u8, - minimum_frames=self.trajectory_minimum_frames, - minimum_bins=self.trajectory_minimum_bins, - minimum_state_span_u8=self.trajectory_minimum_state_span_u8, - minimum_radius_px=self.trajectory_minimum_radius_px, - minimum_arc_rad=self.trajectory_minimum_arc_rad, - maximum_radial_rms_px=self.trajectory_maximum_radial_rms_px, - maximum_p95_radial_error_px=( - self.trajectory_maximum_p95_radial_error_px - ), - ) - - def _publish_hand_command(self, values: list[int]) -> None: - message = JointState() - message.header.stamp = self.get_clock().now().to_msg() - message.name = list(COMMAND_NAMES) - message.position = [float(value) for value in values] - self.command_publisher.publish(message) - - def _motor_at_target(self) -> bool: - return bool( - self.active_target_u8 is not None - and len(self.latest_state_u8) == 20 - and abs( - float(self.latest_state_u8[self.motor_index]) - - float(self.active_target_u8) - ) - <= self.state_tolerance_u8 - ) - - def _timer_callback(self) -> None: - now = time.monotonic() - if self.state == STATE_PREFLIGHT: - if ( - len(self.preflight_flags) == self.preflight_frames - and len(self.latest_state_u8) == 20 - and self._preflight_rate() >= self.minimum_detection_rate - ): - self.state = STATE_WAIT_START - self.reason = "call_start_service" - elif ( - len(self.preflight_flags) == self.preflight_frames - and self._preflight_rate() < self.minimum_detection_rate - ): - self.reason = ( - "T0/T3 detection rate too low:" - f"{self._preflight_rate():.3f}" - ) - return - if self.state == STATE_MOVING: - if now - self.stage_started_at > self.move_timeout_seconds: - self._pause( - f"move_timeout:target={self.active_target_u8}:" - f"actual={self._motor_value_text()}" - ) - return - if not self._motor_at_target(): - self.target_reached_at = None - return - if self.target_reached_at is None: - self.target_reached_at = now - self.reason = ( - f"{self.active_stage}:settling:" - f"{self.settle_seconds:.1f}s" - ) - return - if now - self.target_reached_at >= self.settle_seconds: - self.state = STATE_CAPTURING - self.capture_started_at = now - self.capture_frames.clear() - self.reason = ( - f"{self.active_stage}:collecting:" - f"0/{self.required_frames}" - ) - return - if self.state == STATE_SWEEPING: - self.reason = ( - f"{self.active_stage}:trajectory_frames=" - f"{len(self.trajectory_frames)}:" - f"actual={self._motor_value_text()}:" - f"target={self.active_target_u8}" - ) - if now - self.stage_started_at > self.trajectory_timeout_seconds: - self._pause( - f"trajectory_timeout:{self.active_stage}:" - f"target={self.active_target_u8}:" - f"actual={self._motor_value_text()}:" - f"frames={len(self.trajectory_frames)}" - ) - return - if not self._motor_at_target(): - self.target_reached_at = None - return - if self.target_reached_at is None: - self.target_reached_at = now - return - if ( - now - self.target_reached_at - >= self.trajectory_endpoint_settle_seconds - ): - self._finish_sweep() - return - if self.state == STATE_CAPTURING: - self.reason = ( - f"{self.active_stage}:collecting:" - f"{len(self.capture_frames)}/{self.required_frames}" - ) - if not self._motor_at_target(): - self._pause( - f"motor_left_target_during_capture:" - f"target={self.active_target_u8}:" - f"actual={self._motor_value_text()}" - ) - return - if now - self.capture_started_at > self.capture_timeout_seconds: - self._pause( - f"capture_timeout:{self.active_stage}:" - f"{len(self.capture_frames)}/{self.required_frames}" - ) - - def _finish_capture(self, summary: dict[str, float]) -> None: - if self.active_stage is None: - return - stage = self.active_stage - self.current_round[stage] = summary - self.capture_frames.clear() - if stage == "zero_before": - self._begin_sweep("sweep_out") - return - if stage == "travel_endpoint": - self._begin_sweep("sweep_return") - return - - self.rounds.append(dict(self.current_round)) - self.current_round = {} - self.round_index += 1 - if self.round_index < self.repetitions: - self._begin_stage("zero_before") - return - self._complete_run() - - def _finish_sweep(self) -> None: - stage = self.active_stage - if stage == "sweep_out": - if self.measure_travel: - self._begin_stage("travel_endpoint") - else: - self._begin_sweep("sweep_return") - return - if stage != "sweep_return": - self._pause(f"unexpected_sweep_stage:{stage}") - return - try: - circle = self._fit_trajectory(self.trajectory_frames) - except ValueError as error: - self._pause(str(error)) - return - self.current_round["trajectory_circle"] = circle - self.current_round["trajectory_frames"] = list(self.trajectory_frames) - self.latest_trajectory_frames = list(self.trajectory_frames) - self.latest_circle = circle - self._begin_stage("zero_after") - - def _complete_run(self) -> None: - detection_rate = ( - float(np.mean(self.session_detection_flags)) - if self.session_detection_flags - else 0.0 - ) - all_trajectory_frames = [ - frame - for round_value in self.rounds - for frame in round_value["trajectory_frames"] - ] - try: - circle = self._fit_trajectory(all_trajectory_frames) - measured_rounds = [ - { - "zero_before": measure_zero_from_circle( - circle, round_value["zero_before"] - ), - **( - { - "travel_endpoint": measure_zero_from_circle( - circle, round_value["travel_endpoint"] - ) - } - if self.measure_travel - else {} - ), - "zero_after": measure_zero_from_circle( - circle, round_value["zero_after"] - ), - } - for round_value in self.rounds - ] - except ValueError as error: - self._pause(f"final_trajectory_fit_failed:{error}") - return - if self.measure_travel: - payload, report = build_trajectory_zero_travel_payload( - serial_number=self.serial_number, - joint_name=self.joint_name, - zero_command_u8=self.zero_command_u8, - travel_endpoint_command_u8=self.trajectory_command_u8, - rounds=measured_rounds, - trajectory_quality=circle, - maximum_round_difference_rad=( - self.maximum_round_difference_rad - ), - maximum_travel_difference_rad=( - self.maximum_travel_difference_rad - ), - minimum_travel_rad=self.minimum_travel_rad, - maximum_return_error_rad=self.maximum_return_error_rad, - maximum_zero_radial_error_px=( - self.maximum_zero_radial_error_px - ), - detection_rate=detection_rate, - minimum_detection_rate=self.minimum_detection_rate, - ) - validate_zero_travel_payload(payload) - else: - payload, report = build_trajectory_zero_angle_payload( - serial_number=self.serial_number, - rounds=measured_rounds, - trajectory_quality=circle, - maximum_round_difference_rad=( - self.maximum_round_difference_rad - ), - maximum_return_error_rad=self.maximum_return_error_rad, - maximum_zero_radial_error_px=( - self.maximum_zero_radial_error_px - ), - detection_rate=detection_rate, - minimum_detection_rate=self.minimum_detection_rate, - ) - validate_zero_angle_payload(payload) - atomic_write_json(self.result_path, payload) - self.final_payload = payload - self.final_report = report - self.latest_circle = circle - self.latest_trajectory_frames = all_trajectory_frames - self.latest_angles = { - "table_rad": payload["zero_angles"]["table_projected_zero_rad"], - } - self.latest_travel = ( - dict(payload["travel"]) if self.measure_travel else {} - ) - self.state = STATE_COMPLETE - self.reason = ( - "result_passed" - if payload["quality"]["passed"] - else "result_written_but_quality_failed" - ) - self.active_stage = None - self.active_target_u8 = self.zero_command_u8 - self.get_logger().info( - f"{self.joint_name} result written: {self.result_path}; " - f"passed={payload['quality']['passed']}" - ) - - def _pause(self, reason: str) -> None: - self.state = STATE_PAUSED - self.reason = reason - self.capture_frames.clear() - self.get_logger().error( - f"{self.joint_name} calibration paused: {reason}" - ) - - def _preflight_rate(self) -> float: - return ( - float(np.mean(self.preflight_flags)) - if self.preflight_flags - else 0.0 - ) - - def _motor_value_text(self) -> str: - if len(self.latest_state_u8) != 20: - return "unavailable" - return f"{self.latest_state_u8[self.motor_index]:.1f}" - - def _image_callback(self, message: Image) -> None: - if ( - not self.camera_alignment_enabled - and ( - self.debug_publisher is None - or self.debug_publisher.get_subscription_count() < 1 - ) - ): - return - now = time.monotonic() - if now - self.last_debug_publish < 1.0 / self.debug_max_rate_hz: - return - self.last_debug_publish = now - try: - image = self.bridge.imgmsg_to_cv2( - message, desired_encoding="bgr8" - ) - except Exception as error: - self.get_logger().warning(f"debug image conversion failed: {error}") - return - self._update_camera_alignment(image, now) - if ( - self.debug_publisher is None - or self.debug_publisher.get_subscription_count() < 1 - ): - return - if self.debug_scale != 1.0: - image = cv2.resize( - image, - None, - fx=self.debug_scale, - fy=self.debug_scale, - interpolation=cv2.INTER_AREA, - ) - - height, width = image.shape[:2] - reference_y = int( - round(self.camera_alignment_reference_y_ratio * (height - 1)) - ) - cv2.line( - image, - (20, reference_y), - (max(20, width - 20), reference_y), - (0, 0, 255), - 5, - ) - cv2.putText( - image, - "RED target / image +x", - (25, max(25, reference_y - 8)), - cv2.FONT_HERSHEY_SIMPLEX, - 0.55, - (0, 0, 255), - 2, - ) - if self.latest_camera_alignment.get("detected", False): - line = np.asarray( - self.latest_camera_alignment["line_xyxy_px"], dtype=float - ).reshape(2, 2) - line *= self.debug_scale - left = tuple(np.rint(line[0]).astype(int)) - right = tuple(np.rint(line[1]).astype(int)) - clipped, left, right = cv2.clipLine( - (0, 0, width, height), left, right - ) - if clipped: - cv2.line(image, left, right, (255, 0, 0), 2) - label_x = max(20, min(width - 260, left[0] + 10)) - label_y = max(25, min(height - 15, left[1] - 8)) - cv2.putText( - image, - "BLUE detected reference", - (label_x, label_y), - cv2.FONT_HERSHEY_SIMPLEX, - 0.55, - (255, 0, 0), - 2, - ) - colors = {"t0": (0, 210, 0), "t3": (0, 170, 255)} - for role in ("t0", "t3"): - corners = self.latest_corners.get(role) - if corners is None: - continue - points = np.rint(corners * self.debug_scale).astype(np.int32) - cv2.polylines(image, [points], True, colors[role], 2) - centre = np.mean(points, axis=0) - start = tuple(np.rint(centre).astype(int)) - cv2.putText( - image, - role.upper(), - (start[0] + 5, start[1] - 8), - cv2.FONT_HERSHEY_SIMPLEX, - 0.65, - colors[role], - 2, - ) - - if "t0" in self.latest_corners: - anchor = np.mean(self.latest_corners["t0"], axis=0) - trajectory = ( - self.trajectory_frames - if self.state == STATE_SWEEPING - else self.latest_trajectory_frames - ) - if trajectory: - stride = max(1, len(trajectory) // 300) - for frame in trajectory[::stride]: - relative = np.asarray( - [ - frame["t3_x_px"] - frame["t0_x_px"], - frame["t3_y_px"] - frame["t0_y_px"], - ], - dtype=float, - ) - point = tuple( - np.rint( - (anchor + relative) * self.debug_scale - ).astype(int) - ) - cv2.circle(image, point, 2, (255, 180, 0), -1) - if self.latest_circle: - centre_relative = np.asarray( - self.latest_circle["centre_relative_xy_px"], dtype=float - ) - centre = (anchor + centre_relative) * self.debug_scale - centre_point = tuple(np.rint(centre).astype(int)) - radius = max( - 1, - int( - round( - float(self.latest_circle["radius_px"]) - * self.debug_scale - ) - ), - ) - cv2.circle(image, centre_point, radius, (255, 0, 255), 2) - cv2.drawMarker( - image, - centre_point, - (255, 0, 255), - cv2.MARKER_CROSS, - 18, - 2, - ) - cv2.putText( - image, - "CMC circle centre", - (centre_point[0] + 8, centre_point[1] - 8), - cv2.FONT_HERSHEY_SIMPLEX, - 0.52, - (255, 0, 255), - 2, - ) - cv2.line( - image, - centre_point, - (centre_point[0] + 85, centre_point[1]), - (0, 0, 255), - 2, - ) - if "t3" in self.latest_corners: - t3_centre = ( - np.mean(self.latest_corners["t3"], axis=0) - * self.debug_scale - ) - cv2.line( - image, - centre_point, - tuple(np.rint(t3_centre).astype(int)), - (255, 0, 255), - 3, - ) - - cv2.putText( - image, - f"{self.state} round {min(self.round_index + 1, 3)}/3", - (20, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 0.65, - (0, 255, 0) - if self.state not in {STATE_PAUSED, STATE_ABORTED} - else (0, 0, 255), - 2, - ) - angle_text_y = 58 - if self.camera_alignment_enabled: - alignment_ready = self._camera_alignment_ready(now) - if self.latest_camera_alignment.get("detected", False): - angle_deg = math.degrees( - float(self.latest_camera_alignment["angle_rad"]) - ) - offset_px = float( - self.latest_camera_alignment["vertical_offset_px"] - ) - alignment_text = ( - f"red-blue angle {angle_deg:+.2f} deg " - f"dy {offset_px:+.1f}px " - f"hit {self.latest_camera_alignment.get('detected_frames', 0)}/" - f"{self.camera_alignment_required_frames} " - f"{'ALIGNED' if alignment_ready else 'ADJUST'}" - ) - else: - alignment_text = "BLUE reference line not detected" - cv2.putText( - image, - alignment_text, - (20, angle_text_y), - cv2.FONT_HERSHEY_SIMPLEX, - 0.60, - (0, 255, 0) if alignment_ready else (0, 165, 255), - 2, - ) - if self.latest_angles: - cv2.putText( - image, - "table zero: " - f"{math.degrees(self.latest_angles['table_rad']):.2f} deg", - (20, 86 if self.camera_alignment_enabled else 58), - cv2.FONT_HERSHEY_SIMPLEX, - 0.62, - (0, 255, 255), - 2, - ) - elif self.state in { - STATE_MOVING, - STATE_CAPTURING, - STATE_SWEEPING, - }: - cv2.putText( - image, - "zero angle: waiting for trajectory circle", - (20, 86 if self.camera_alignment_enabled else 58), - cv2.FONT_HERSHEY_SIMPLEX, - 0.58, - (0, 255, 255), - 2, - ) - output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") - output.header = message.header - self.debug_publisher.publish(output) - - def _publish_status(self) -> None: - tag_status: dict[str, dict[str, Any]] = {} - for role in ("t0", "t3"): - quality = self.latest_quality.get(role) - if quality is None: - tag_status[role] = {"valid": False, "reason": "未检测到"} - else: - tag_status[role] = { - **quality, - "valid": self._quality_valid(quality), - } - camera_alignment = { - "enabled": self.camera_alignment_enabled, - **self.latest_camera_alignment, - "ready": self._camera_alignment_ready(), - } - payload = { - "state": self.state, - "state_zh": STATE_ZH[self.state], - "reason": self.reason, - "measurement_method": ( - "t3_center_to_circle_centre_image_trajectory" - ), - "joint": self.joint_name, - "motor_index": self.motor_index, - "zero_command_u8": self.zero_command_u8, - "measure_travel": self.measure_travel, - "cycle": min(self.round_index + 1, self.repetitions), - "cycles_total": self.repetitions, - "stage": self.active_stage, - "stage_zh": ( - None - if self.active_stage is None - else self._stage_zh(self.active_stage) - ), - "active_command_u8": self.active_target_u8, - "actual_command_u8": ( - None - if len(self.latest_state_u8) != 20 - else self.latest_state_u8[self.motor_index] - ), - "capture_frames_seen": len(self.capture_frames), - "capture_frames_required": self.required_frames, - "trajectory_frames_seen": len(self.trajectory_frames), - "trajectory_command_u8": self.trajectory_command_u8, - "trajectory_circle": self.latest_circle, - "preflight_detection_rate": self._preflight_rate(), - "tag_quality": tag_status, - "camera_alignment": camera_alignment, - "latest_angles_rad": self.latest_angles, - "latest_travel_rad": self.latest_travel, - "result_path": str(self.result_path), - "result": self.final_payload, - "quality_report": self.final_report, - } - message = String() - message.data = json.dumps(payload, ensure_ascii=False) - self.status_publisher.publish(message) - text = String() - text.data = self._status_text(payload) - self.status_text_publisher.publish(text) - - def _stage_zh(self, stage: str) -> str: - if stage == "zero_before": - return f"{self.zero_command_u8}零位（运动前）" - if stage == "sweep_out": - return ( - f"圆心轨迹（{self.zero_command_u8}到" - f"{self.trajectory_command_u8}）" - ) - if stage == "travel_endpoint": - return f"{self.trajectory_command_u8}行程端点" - if stage == "sweep_return": - return f"圆心轨迹（返回{self.zero_command_u8}）" - if stage == "zero_after": - return f"{self.zero_command_u8}零位（返回后）" - return STAGE_ZH.get(stage, stage) - - def _status_text(self, payload: dict[str, Any]) -> str: - reason_zh = self._reason_zh() - if self.state == STATE_CAPTURING: - frame_text = ( - f"帧数：{len(self.capture_frames)}/{self.required_frames}" - ) - elif self.state == STATE_SWEEPING: - frame_text = ( - f"轨迹帧数：{len(self.trajectory_frames)}/" - f"至少{self.trajectory_minimum_frames}" - ) - else: - frame_text = "帧数：当前未采集" - lines = [ - f"状态：{payload['state_zh']}", - f"原因：{reason_zh}", - ( - f"关节：{self.joint_name}，电机索引" - f"{self.motor_index}" - ), - ( - f"进度：第{payload['cycle']}/{self.repetitions}轮，" - f"{payload['stage_zh'] or '无活动阶段'}" - ), - frame_text, - ] - if reason_zh != self.reason: - lines.append(f"诊断代码：{self.reason}") - for role, label in (("t0", "掌心T0(ID 0)"), ("t3", "根部T3(ID 1)")): - status = payload["tag_quality"][role] - if "edge_pixels" not in status: - lines.append(f"标签：{label} 未检测到") - continue - lines.append( - f"标签：{label} " - f"{'正常' if status['valid'] else '不合格'}，" - f"边长{status['edge_pixels']:.1f}px，" - f"置信度{status['decision_margin']:.1f}，" - f"hamming={status['hamming']}" - ) - alignment = payload["camera_alignment"] - if alignment["enabled"]: - if alignment.get("detected", False): - angle_deg = math.degrees(float(alignment["angle_rad"])) - offset_px = float(alignment["vertical_offset_px"]) - lines.extend( - [ - "相机二维对齐：", - ( - f"- 红蓝线夹角 {angle_deg:+.3f}°，要求 " - "±" - f"{math.degrees(self.camera_alignment_max_angle_rad):.3f}°" - ), - ( - f"- 垂直偏差 {offset_px:+.1f}px，要求 " - f"±{self.camera_alignment_max_vertical_offset_px:.1f}px" - ), - ( - "- 状态 " - f"{'已对齐' if alignment['ready'] else '请调整相机'}" - "，" - f"窗口 {alignment.get('window_frames', 0)}/" - f"{self.camera_alignment_required_frames}，" - f"有效帧 {alignment.get('detected_frames', 0)}/" - f"{self.camera_alignment_required_frames}" - ), - ] - ) - else: - lines.append( - "相机二维对齐：未识别到下方物理参考直线" - ) - if self.latest_angles: - lines.extend( - [ - "内向径向零位角：", - ( - "- 桌面投影零位 " - f"{self.latest_angles['table_rad']:.6f} rad " - f"({math.degrees(self.latest_angles['table_rad']):.2f}°)" - ), - ] - ) - elif self.state in { - STATE_MOVING, - STATE_CAPTURING, - STATE_SWEEPING, - }: - lines.append("当前角度：等待完成T3中心轨迹圆拟合") - if self.latest_circle: - lines.extend( - [ - "轨迹圆质量：", - ( - f"- 半径 {self.latest_circle['radius_px']:.2f}px，" - f"圆弧 {math.degrees(self.latest_circle['arc_rad']):.2f}°" - ), - ( - f"- 径向RMS {self.latest_circle['radial_rms_px']:.2f}px，" - f"P95 {self.latest_circle['radial_p95_px']:.2f}px" - ), - ] - ) - if self.latest_travel: - lines.extend( - [ - "角行程：", - ( - f"- 有符号 {self.latest_travel['signed_rad']:.6f} rad " - f"({math.degrees(self.latest_travel['signed_rad']):.2f}°)" - ), - ( - f"- 行程大小 {self.latest_travel['range_rad']:.6f} rad " - f"({math.degrees(self.latest_travel['range_rad']):.2f}°)" - ), - ] - ) - if self.state == STATE_WAIT_START: - lines.append( - f"下一步：调用 /{self.get_name()}/start 开始标定" - ) - elif self.state == STATE_PAUSED: - lines.append("下一步：排除问题后再次调用 start，从第一轮重做") - elif self.state == STATE_COMPLETE: - lines.append(f"结果文件：{self.result_path}") - return "\n".join(lines) - - def _reason_zh(self) -> str: - reason = str(self.reason) - exact = { - "waiting_for_t0_t3_and_hand_state": "等待T0、T3和机械手状态", - "call_start_service": "预检通过，可以调用start开始", - "result_passed": "轨迹圆与三轮回零质量均通过", - "result_written_but_quality_failed": ( - "结果已保存，但三轮一致性或回零质量未通过" - ), - "aborted_by_operator_no_motion_command_sent": "已由用户终止", - } - if reason in exact: - return exact[reason] - prefixes = ( - ("trajectory_frames_too_few", "轨迹有效帧不足"), - ("trajectory_valid_frames_too_few", "轨迹有效数据不足"), - ("trajectory_state_span_too_small", "电机轨迹跨度不足"), - ("trajectory_bins_too_few", "轨迹覆盖的电机区间不足"), - ("trajectory_circle_quality_failed", "轨迹圆拟合质量不合格"), - ("trajectory_timeout", "轨迹运动或采集超时"), - ("final_trajectory_fit_failed", "三轮合并轨迹圆拟合失败"), - ("move_timeout", "机械手未在规定时间到达目标"), - ("capture_timeout", "静态零位帧采集超时"), - ("motor_left_target_during_capture", "静态采集时电机离开目标"), - ("unexpected_sweep_stage", "内部轨迹阶段异常"), - ) - for prefix, text in prefixes: - if reason.startswith(prefix): - return text - if "_position_not_stable" in reason: - return "T0与T3相对位置仍在抖动" - if "_not_stable" in reason: - return "标签方向仍在抖动" - if reason.startswith("round_"): - return "正在执行本轮零位或圆心轨迹阶段" - return reason - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - node: G20ThumbCmcTrajectoryNode | None = None - try: - node = G20ThumbCmcTrajectoryNode() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() diff --git a/src/linkerhand_calibration/setup.py b/src/linkerhand_calibration/setup.py index c1a9be1..aa41fa0 100644 --- a/src/linkerhand_calibration/setup.py +++ b/src/linkerhand_calibration/setup.py @@ -41,12 +41,8 @@ setup( ["resource/" + package_name], ), ("share/" + package_name, ["package.xml", "README.md"]), - ( - "share/" + package_name + "/config", - glob("config/*.yaml") + glob("config/*.xml"), - ), ("share/" + package_name + "/launch", glob("launch/*.launch.py")), - ] + package_data_tree("urdf"), + ] + package_data_tree("config") + package_data_tree("urdf"), install_requires=["setuptools", "numpy", "scipy", "PyYAML"], tests_require=["pytest"], zip_safe=True, @@ -61,18 +57,11 @@ setup( "hikrobot_camera_node = " "linkerhand_calibration.hikrobot_camera:main" ), - "calibration_node = linkerhand_calibration.node:main", - ( - "cmc_pitch_zero_node = " - "linkerhand_calibration.zero_node:main" - ), - ( - "cmc_roll_calibration_node = " - "linkerhand_calibration.zero_node:main" - ), + "calibration_node = linkerhand_calibration.runtime.ros.entrypoint:main", + "o12_sdk_bridge = linkerhand_calibration.runtime.adapters.o12_bridge:main", ( "three_camera_calibration_node = " - "linkerhand_calibration.runtime.nodes.calibration:main" + "linkerhand_calibration.runtime.ros.entrypoint:main" ), ( "three_camera_extrinsics_node = " @@ -84,7 +73,7 @@ setup( ), ( "validate_g20_goldens = " - "linkerhand_calibration.models.g20.golden_regression:main" + "linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.golden_regression:main" ), ( "camera_alignment_view = " diff --git a/src/linkerhand_calibration/test/engine_capture_fixture.py b/src/linkerhand_calibration/test/engine_capture_fixture.py new file mode 100644 index 0000000..5d4de0e --- /dev/null +++ b/src/linkerhand_calibration/test/engine_capture_fixture.py @@ -0,0 +1,108 @@ +"""Ideal camera/SDK transport for the REAL capture and session executor. + +Only the sensor's pose estimator is idealized; the state machine, observation +joins, unique-image quality, four-cycle selection and finalizer are production +code. Real pinhole/IPPE behavior is independently tested in observation_capture. +""" + +from collections import defaultdict +from types import SimpleNamespace + +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.extrinsics import transform_matrix +from linkerhand_calibration.core.geometry.pnp import SquareTagPose +from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.reference_lock import ReferenceLock +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase + + +def collect_with_engine(profile, observations): + # One fixed camera geometry, independent of the task/model names. + normal = np.ones(3)/np.sqrt(3) + x = np.cross([0, 1, 0], normal); x /= np.linalg.norm(x) + rotation = np.column_stack((x, np.cross(normal, x), normal)) + camera = np.eye(4); camera[:3, :3] = rotation; camera[:3, 3] = [0, 0, -.5] + camera_inverse = np.linalg.inv(camera) + fixed_roles = {view.name: next(t.role for t in view.tags if t.fixed_reference) for view in profile.vision.views} + fixed_poses = {} + frame_groups = defaultdict(lambda: defaultdict(list)) + for row in observations: + name = row.get("joint", row.get("observation_joint")) + spec_name = name if "joint" in row else profile.measurement.cross_view_sources[name] + spec = profile.measurement.measurements[spec_name] + if spec.parent_role == fixed_roles[spec.view]: + fixed_poses[spec.view] = row["parent_pose_common"] + key = (row["task_name"], row["cycle"], row["direction"], row.get("sample_phase", "sweep"), row.get("steady_index")) + frame_groups[key][row["sample_id"]].append((row, spec)) + poses = {view: {} for view in profile.vision.view_names} + class IdealPoseSensor: + maximum_reprojection_error_px = 1.5 + def __init__(self, view): self.view = view + def reset(self): pass + def estimate(self, role, corners, **kwargs): return poses[self.view][role], "" + def camera_pose(payload): + value = camera_inverse @ transform_matrix(payload["translation_xyz_m"], payload["quaternion_xyzw"]) + return SquareTagPose(tuple(Rotation.from_matrix(value[:3, :3]).as_quat()), tuple(value[:3, 3]), 0.) + lock = ReferenceLock({v.name: next(t.tag_id for t in v.tags if t.fixed_reference) for v in profile.vision.views}) + capture = ObservationCapture(profile, reference_lock=lock, + extrinsics=SimpleNamespace(transform=lambda _: camera), + trackers={view: IdealPoseSensor(view) for view in profile.vision.view_names}) + execution = SessionExecution(profile) + execution.session.device_ready() + execution.session.start() + lock.begin_session() + current = profile.command.baseline_values + retained, stamp, phases = [], 0, set() + square = np.array([[-20, 20], [20, 20], [20, -20], [-20, -20]]) + while execution.session.phase != Phase.FIT: + phase = execution.session.phase + phases.add(phase) + if phase == Phase.REFERENCE_LOCKING: + lock.start_locking() + for view, role in fixed_roles.items(): + poses[view] = {role: camera_pose(fixed_poses[view])} + for _ in range(10): + stamp += 1 + capture.consume(CaptureFrame(view, stamp, np.eye(3), {role: square}, current, current)) + assert lock.locked + execution.session.reference_locked() + continue + if phase == Phase.EVALUATE: + quality = execution.evaluate(retained) + assert quality.passed, quality.failures + continue + motion = execution.motion(current) + assert motion is not None, phase + if motion.recording: + key = (motion.task_key, motion.cycle, motion.direction, motion.phase, motion.steady_index) + for rows in frame_groups[key].values(): + first = rows[0][0]; view = first["view"] + poses[view] = {} + centers = {} + for row, spec in rows: + poses[view][spec.parent_role] = camera_pose(row["parent_pose_common"]) + poses[view][spec.child_role] = camera_pose(row["child_pose_common"]) + centers.setdefault(spec.parent_role, np.zeros(2)) + centers[spec.child_role] = centers[spec.parent_role] + row["image_relative_xy_px"] + # Fixed references are physically stationary in this fixture; + # the image-circle test signal belongs only to moving Tags. + corners = {role: square+center for role, center in centers.items()} + if fixed_roles[view] in corners: + corners[fixed_roles[view]] = square + stamp += 1 + unit = profile.command.unit + frame = CaptureFrame(view, stamp, np.eye(3), corners, + tuple(first[f"state_{unit}"]), tuple(first[f"command_vector_{unit}"])) + captured, moved = capture.consume(frame, motion) + assert moved is None + # The IPPE candidate branch is covered by real projected-corner + # tests, not these deliberately ideal pose measurements. + retained.extend(row for row in captured if row["kind"] != "pnp_candidate_frame") + current = motion.target + execution.motion_complete() + assert len(execution.completed_units) == 8*len(profile.motion.tasks) + assert Phase.RETURN_BASELINE in phases + return execution, retained diff --git a/src/linkerhand_calibration/test/o12_artifact_fixture.py b/src/linkerhand_calibration/test/o12_artifact_fixture.py new file mode 100644 index 0000000..911d986 --- /dev/null +++ b/src/linkerhand_calibration/test/o12_artifact_fixture.py @@ -0,0 +1,32 @@ +"""Synthetic serialization envelope, NOT a replacement/review of real CAD. + +The older fitting fixture intentionally has travel beyond vendor CAD limits. +Those observations may test fitting, but cannot certify that real source file. +Use a clearly virtual envelope for coordinate/serializer unit tests; separate +tests require the immutable vendor CAD to reject out-of-range corrections. +""" +import xml.etree.ElementTree as ET +from pathlib import Path + + +def virtual_o12_source(directory: Path, original: Path) -> Path: + tree = ET.parse(original) + root = tree.getroot() + root.set("name", "virtual_o12_serialization_fixture_not_hardware") + for node in root.findall("joint"): + if node.get("type") != "revolute": + continue + limit = node.find("limit") + if node.find("mimic") is None: + limit.set("lower", "-1") + limit.set("upper", "3") + else: + limit.set("lower", "-4") + limit.set("upper", "4") + for mesh in root.iter("mesh"): + filename = mesh.get("filename") + if not filename.startswith(("package://", "file://")): + mesh.set("filename", str((original.parent / filename).resolve())) + destination = directory / "virtual_o12_source.urdf" + tree.write(destination) + return destination diff --git a/src/linkerhand_calibration/test/runtime_host_fixture.py b/src/linkerhand_calibration/test/runtime_host_fixture.py new file mode 100644 index 0000000..aeecac6 --- /dev/null +++ b/src/linkerhand_calibration/test/runtime_host_fixture.py @@ -0,0 +1,50 @@ +"""A real coordinator with in-memory ports; never creates a ROS node or SDK device.""" + +from pathlib import Path +from types import SimpleNamespace + +import yaml + +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.runtime.adapters.ros_binding import SdkBindingPorts, bind_ros_sdk +from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator +from linkerhand_calibration.runtime.inputs import RuntimePorts +from linkerhand_calibration.runtime.ros.parameters import load_runtime_parameters, parameter_defaults +from linkerhand_calibration.runtime.runner_support import protected_inputs + + +def coordinator_fixture(tmp_path, *, model="o6", finalization=None): + package = Path(__file__).resolve().parents[1] + config = load_product_config(package / f"config/{model}_right_product.yaml", + workspace=package.parents[1], check_can=False) + profile = config.calibration_contract.typed_profile + declared = next(iter(yaml.safe_load(config.calibration_config.read_text()).values()))["ros__parameters"] + values = {**parameter_defaults(profile), **declared, + "serial_number": "OFFLINE_COORDINATOR", "session_dir": str(tmp_path / model), + "source_urdf_path": str(config.source_urdf), "camera_extrinsics_file": str(config.camera_extrinsics), + **{key.replace("_sha256", "_expected_sha256"): value for key, value in protected_inputs(config).items()}} + parameters = load_runtime_parameters(profile, values.__getitem__) + clock = SimpleNamespace(now=10., publishers=1, positions=[], settings=[], health_receiver=None) + ports = RuntimePorts(lambda: int(clock.now*1e9), lambda: clock.now, lambda: clock.publishers, + clock.positions.append, clock.settings.append) + + def subscribe_health(callback): + clock.health_receiver = callback + callback('{"position_mode": true, "active_faults": []}') + + def factory(publish, set_speed, fresh): + return bind_ros_sdk(profile, SdkBindingPorts(fresh, ports.monotonic, subscribe_health), + publish=publish, set_speed=set_speed) + + return CalibrationCoordinator(profile, parameters, ports, factory, finalization=finalization), clock + + +def ready(host, clock): + import numpy as np + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + for view in host.profile.vision.view_names: + host.cameras.matrices[view] = np.eye(3) + host.cameras.image_sizes[view] = (640, 480) + host.cameras.info_received_at[view] = clock.now + host.cameras.detections_received_at[view] = clock.now + host.tick() diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index 56bd721..8375f63 100644 --- a/src/linkerhand_calibration/test/test_architecture.py +++ b/src/linkerhand_calibration/test/test_architecture.py @@ -1,5 +1,6 @@ from pathlib import Path from types import SimpleNamespace +import ast from linkerhand_calibration.compat import product_profile_key from linkerhand_calibration.core import ( @@ -11,7 +12,6 @@ from linkerhand_calibration.core import ( MotionPolicy, ProfileKey, QualityPolicy, - SampleRecord, ScopePolicy, TagSpec, TaskSpec, @@ -20,23 +20,19 @@ from linkerhand_calibration.core import ( ZeroSolvePolicy, validate_profile, ) -from linkerhand_calibration.core.solver import ( - SessionSolution, - TaskEvaluation, -) from linkerhand_calibration.core.urdf import UrdfCorrectionPlan from linkerhand_calibration.extrinsics import ( validate_camera_extrinsics_payload, ) -from linkerhand_calibration.models import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models import ( EngineBindings, ProfileRegistry, RegisteredProfile, get_default_registry, ) from linkerhand_calibration.product import get_product_calibration_contract -from linkerhand_calibration.runtime import SessionController, SessionState -from linkerhand_calibration.runtime.nodes import calibration as calibration_node +from linkerhand_calibration.runtime import CalibrationPhase, CalibrationSession +from linkerhand_calibration.runtime.ros import entrypoint as calibration_node PACKAGE = Path(__file__).resolve().parents[1] / "linkerhand_calibration" @@ -64,9 +60,9 @@ def _small_profile() -> CalibrationProfile: views=( ViewSpec( "oblique", - (TagSpec("base_a", 40, fixed_reference=True),), + (TagSpec("base_a", 40, fixed_reference=True), TagSpec("moving_a", 42)), ), - ViewSpec("wrist", (TagSpec("moving_b", 41),)), + ViewSpec("wrist", (TagSpec("base_b", 43, fixed_reference=True), TagSpec("moving_b", 41))), ), common_frame="fixture", extrinsic_reference_view="oblique", @@ -80,10 +76,10 @@ def _small_profile() -> CalibrationProfile: measurement=MeasurementPolicy( measurements={ "axis_a": MeasurementSpec( - "axis_a", "rotation", "oblique", "base_a", "base_a" + "axis_a", "rotation", "oblique", "base_a", "moving_a" ), "axis_b": MeasurementSpec( - "axis_b", "circle", "wrist", "moving_b", "moving_b" + "axis_b", "circle", "wrist", "base_b", "moving_b" ), } ), @@ -98,8 +94,8 @@ def _small_profile() -> CalibrationProfile: cad_frozen_joints=frozenset(), ), quality=QualityPolicy( - training_cycles=(0, 1), - holdout_cycle=2, + training_cycles=(0, 1, 2), + holdout_cycle=3, hard_threshold_keys=frozenset({"maximum_error"}), ), scope=ScopePolicy( @@ -132,15 +128,39 @@ def _bindings() -> EngineBindings: def test_core_has_no_ros_or_model_dependency() -> None: - text = _source_text(PACKAGE / "core") - for forbidden in ("rclpy", "cv_bridge", " G20", " L6", "..models"): - assert forbidden not in text + # Historical provenance in a comment is not a dependency. Check imports + # and executable product-identity branches instead of matching prose. + for path in (PACKAGE / "core").rglob("*.py"): + tree = ast.parse(path.read_text()) + for node in ast.walk(tree): + if isinstance(node, ast.ImportFrom): + assert not {"models", "rclpy", "cv_bridge"} & set((node.module or "").split(".")), path + if isinstance(node, ast.Import): + assert not any({"models", "rclpy", "cv_bridge"} & set(alias.name.split(".")) for alias in node.names), path + if isinstance(node, (ast.If, ast.IfExp)): + assert not any(isinstance(item, ast.Constant) and isinstance(item.value, str) + and item.value.upper() in {"G20", "L6", "O6", "O12"} + for item in ast.walk(node.test)), path def test_runtime_has_no_concrete_model_or_view_assumption() -> None: - text = _source_text(PACKAGE / "runtime") - for forbidden in ("G20", "L6", "RIGHT_19", '"front"', '"side"', '"top"'): - assert forbidden not in text + # A JSON "side" field describes handedness, not a hardcoded camera. + # Inspect executable selection rather than rejecting schema keys/comments. + forbidden = {"G20", "L6", "O6", "O12", "RIGHT_19", "front", "side", "top"} + for path in (PACKAGE / "runtime").rglob("*.py"): + tree = ast.parse(path.read_text()) + for node in ast.walk(tree): + if isinstance(node, (ast.If, ast.IfExp)): + # A payload.get("side") field lookup is not selection by + # camera identity. Inspect compared literal values instead. + assert not any(isinstance(item, ast.Constant) and isinstance(item.value, str) and item.value in forbidden + for comparison in ast.walk(node.test) if isinstance(comparison, ast.Compare) + for operand in comparison.comparators for item in ast.walk(operand)), path + if isinstance(node, ast.Subscript) and isinstance(node.slice, ast.Constant): + if node.slice.value in {"front", "side", "top"}: + assert not any(isinstance(item, ast.Attribute) and item.attr in { + "views", "cameras", "camera_matrices", "trackers"} + for item in ast.walk(node.value)), path def test_every_registered_profile_passes_static_integrity_checks() -> None: @@ -204,10 +224,14 @@ def test_generic_extrinsics_validator_uses_profile_view_names() -> None: ) -def test_ros_node_dispatcher_uses_profile_registry_without_model_branch( - monkeypatch, +def test_legacy_node_binding_handoff_has_no_model_identity_branch( + monkeypatch, tmp_path, ) -> None: + import hashlib profile = _small_profile() + from linkerhand_calibration import profiles + path = tmp_path/"profile.yaml" + path.write_text(profiles.dump_hand_profile(profile)) forwarded: list[list[str] | None] = [] bindings = _bindings() bindings = EngineBindings( @@ -221,14 +245,14 @@ def test_ros_node_dispatcher_uses_profile_registry_without_model_branch( ) registry = ProfileRegistry() registry.register(RegisteredProfile(profile, bindings)) - monkeypatch.setattr( - calibration_node, "get_default_registry", lambda: registry - ) + monkeypatch.setattr(calibration_node, "run_profile_node", lambda declared, args: forwarded.append(args)) calibration_node.main( [ "--profile-id", profile.key.profile_id, + "--profile-config", str(path), + "--profile-sha256", hashlib.sha256(path.read_bytes()).hexdigest(), "--ros-args", "-p", "sample_rate:=30", @@ -255,44 +279,15 @@ def test_retired_layout_alias_is_confined_to_compatibility_resolution() -> None: assert contract.typed_profile.key.layout == "g20_right_19" -def test_controller_uses_one_evaluator_and_solver_contract() -> None: - profile = _small_profile() - - class Evaluator: - def evaluate_task(self, selected, task, samples): - assert selected is profile - return TaskEvaluation(accepted=True) - - class Solver: - def solve_session(self, selected, samples): - assert selected is profile - assert {row.task_key for row in samples} == {"scan_a", "scan_b"} - return SessionSolution(True, {"ok": True}, {"axis_a": 0.0}) - - controller = SessionController(profile, Evaluator(), Solver()) - controller.start() - controller.finish_preflight(passed=True) - for index, task in enumerate(profile.motion.tasks): - controller.task_pose_ready() - controller.submit_task_samples( - ( - SampleRecord( - task.key, - task.joints[0], - task.view, - 0, - "decreasing", - 255, - index, - {}, - ), - ) - ) - assert controller.state == SessionState.SOLVING - assert controller.solve().passed - controller.finish_release_validation(passed=True) - controller.finish_publication() - assert controller.state == SessionState.COMPLETE +def test_public_runtime_exports_the_shared_session_kernel() -> None: + session = CalibrationSession(_small_profile()) + assert session.phase == CalibrationPhase.WAIT_DEVICE + session.device_ready() + session.start() + session.baseline_complete() + session.reference_locked() + session.preparation_complete() + assert session.phase == CalibrationPhase.SWEEP def test_urdf_writer_and_validator_can_share_one_edit_plan() -> None: diff --git a/src/linkerhand_calibration/test/test_artifact_publication.py b/src/linkerhand_calibration/test/test_artifact_publication.py new file mode 100644 index 0000000..c1a733f --- /dev/null +++ b/src/linkerhand_calibration/test/test_artifact_publication.py @@ -0,0 +1,68 @@ +from __future__ import annotations + +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.artifacts.storage import atomic_write_json +from linkerhand_calibration.runtime.artifacts import ArtifactPublisher + + +def test_pointer_changes_only_after_both_artifacts_validate(tmp_path: Path) -> None: + session = tmp_path / "serial" / "session" + session.mkdir(parents=True) + calibration = session / "calibration.json" + urdf = session / "corrected.urdf" + atomic_write_json(calibration, {"schema_version": 1, "quality": {"passed": True}}) + urdf.write_text('', encoding="utf-8") + publisher = ArtifactPublisher(session.parent, "latest_passed") + release = publisher.publish( + session_directory=session, + calibration_json=calibration, + corrected_urdf=urdf, + validate=lambda _json, _urdf: None, + ) + assert release.pointer.resolve() == session.resolve() + assert len(release.sha256["corrected_urdf_sha256"]) == 64 + + +def test_failed_pair_validation_never_updates_pointer(tmp_path: Path) -> None: + session = tmp_path / "serial" / "session" + session.mkdir(parents=True) + calibration = session / "calibration.json" + urdf = session / "corrected.urdf" + atomic_write_json(calibration, {"schema_version": 1}) + urdf.write_text('', encoding="utf-8") + publisher = ArtifactPublisher(session.parent, "latest_passed") + + def reject(_json: Path, _urdf: Path) -> None: + raise ValueError("holdout failed") + + with pytest.raises(ValueError, match="holdout"): + publisher.publish( + session_directory=session, + calibration_json=calibration, + corrected_urdf=urdf, + validate=reject, + ) + assert not (session.parent / "latest_passed").exists() + + +def test_staging_does_not_publish_and_changed_or_cancelled_stage_is_rejected(tmp_path): + session = tmp_path / "serial" / "session" + session.mkdir(parents=True) + calibration, urdf = session / "calibration.json", session / "corrected.urdf" + atomic_write_json(calibration, {"schema_version": 1}) + urdf.write_text('') + publisher = ArtifactPublisher(session.parent, "latest_passed") + staged = publisher.prepare(session_directory=session, calibration_json=calibration, + corrected_urdf=urdf, validate=lambda *_: {"passed": True}) + pointer = session.parent / "latest_passed" + assert not pointer.exists() + with pytest.raises(ValueError, match="abort|cancel"): + publisher.commit(staged, cancelled=lambda: True) + assert not pointer.exists() + atomic_write_json(calibration, {"schema_version": 2}) + with pytest.raises(ValueError, match="changed|hash|mutat"): + publisher.commit(staged) + assert not pointer.exists() diff --git a/src/linkerhand_calibration/test/test_calibration_coordinator.py b/src/linkerhand_calibration/test/test_calibration_coordinator.py new file mode 100644 index 0000000..c135cde --- /dev/null +++ b/src/linkerhand_calibration/test/test_calibration_coordinator.py @@ -0,0 +1,223 @@ +"""Session/observation/publication races exercised through real coordinator ports.""" + +from concurrent.futures import ThreadPoolExecutor +from dataclasses import replace +from threading import Event +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.runtime.artifacts.controller import FinalizationController +from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher +from linkerhand_calibration.runtime.inputs import DetectionInput +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from runtime_host_fixture import coordinator_fixture, ready + + +@pytest.mark.parametrize("change", ["start", "pause", "abort", "motion", "steady"]) +def test_late_pose_result_cannot_cross_session_or_motion_boundary(tmp_path, monkeypatch, change): + host, clock = coordinator_fixture(tmp_path) + entered, release = Event(), Event() + try: + ready(host, clock) + if change != "start": + assert host.start().success + host.execution.session.phase = Phase.SWEEP + host._motion = MotionCommand("steady" if change == "steady" else "sweep", + host.profile.command.baseline_values, 1., task_key=host.profile.motion.tasks[0].key, + command_index=0, cycle=0, direction="decreasing") if change != "start" else None + host._steady_capture_after_ns = host.ports.clock_ns() + + def consume(*args, **kwargs): + entered.set() + assert release.wait(2) + return [{"kind": "late_sample"}], None + + monkeypatch.setattr(host.capture, "consume", consume) + with ThreadPoolExecutor(1) as pool: + future = pool.submit(host.receive_detections, DetectionInput( + host.profile.vision.view_names[0], host.ports.clock_ns(), ())) + assert entered.wait(2) + # These calls must complete while the expensive pose solve is blocked. + if change == "start": + assert host.start().success + elif change == "pause": + host._pause("test_visual_fault") + elif change == "abort": + assert host.abort().success + elif change == "motion": + with host.step_data_lock: + host._motion = replace(host._motion, cycle=1) + else: + with host.step_data_lock: + host._steady_capture_after_ns = None + release.set() + future.result(timeout=2) + assert host.raw_records == [] + assert host._unit_rows == [] + finally: + release.set() + host.close() + + +def begin_fit(host, clock): + ready(host, clock) + assert host.start().success + clock.now += .01 + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + host.execution.session.phase = Phase.FIT + host.last_command = tuple(host.profile.command.baseline_values) + host.tick() + assert host.finalization.started + + +def completed_fit(**kwargs): + assert kwargs["publish"] is False + for event in ("fit_complete", "holdout_complete", "artifacts_built", "urdf_validated"): + kwargs["phase_changed"](event) + return ({"format": "unified_calibration_v2", "joints": {}}, None, + SimpleNamespace(path=kwargs["session_dir"]/"corrected.urdf", staged_release="staged")) + + +def replace_finalizer(host, finalizer): + host.finalization.close() + host.finalization = FinalizationController(host.profile, host.parameters.session_dir, finalizer=finalizer) + + +def test_feedback_and_status_continue_during_fit_failure_never_starts_motion(tmp_path): + host, clock = coordinator_fixture(tmp_path) + entered, release = Event(), Event() + def fit(**kwargs): + entered.set() + assert release.wait(2) + raise ValueError("synthetic_fit_failure") + try: + replace_finalizer(host, fit) + begin_fit(host, clock) + assert entered.wait(2) + for _ in range(10): + clock.now += .01 + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, host.ports.clock_ns()) + host.tick() + assert host.snapshot().calibration.phase == "FIT" + assert len(host.state_history) == 11 + release.set() + with pytest.raises(ValueError, match="synthetic_fit_failure"): + host.finalization.worker._future.result(timeout=2) + host.tick() + assert host.execution.session.phase == Phase.PAUSED + assert "fit_or_publication_failed:synthetic_fit_failure" == host.reason + count = len(clock.positions) + host.tick() + assert len(clock.positions) == count + assert all(command == tuple(host.profile.command.baseline_values) for command in clock.positions) + finally: + release.set() + host.close() + + +@pytest.mark.parametrize("abort_first", [True, False]) +def test_abort_and_artifact_commit_are_mutually_exclusive(tmp_path, monkeypatch, abort_first): + host, clock = coordinator_fixture(tmp_path) + committing, release, abort_entered, abort_done = Event(), Event(), Event(), Event() + commits = [] + def commit(publisher, staged): + committing.set() + assert release.wait(2) + commits.append(staged) + monkeypatch.setattr(ArtifactPublisher, "commit", commit) + try: + replace_finalizer(host, completed_fit) + begin_fit(host, clock) + host.finalization.worker._future.result(timeout=2) + if abort_first: + assert host.abort().success + host.tick() + assert not commits + assert host.execution.session.phase == Phase.ABORTED + else: + with ThreadPoolExecutor(2) as pool: + tick = pool.submit(host.tick) + assert committing.wait(2) + def abort(): + abort_entered.set() + result = host.abort() + abort_done.set() + return result + aborted = pool.submit(abort) + assert abort_entered.wait(2) + assert not abort_done.wait(.02) + release.set() + tick.result(timeout=2) + assert not aborted.result(timeout=2).success + assert host.execution.session.phase == Phase.COMPLETE + assert host.final_json.endswith("o6_right_OFFLINE_COORDINATOR_partial_calibration.json") + assert host.final_urdf.endswith("corrected.urdf") + host.tick() + assert commits == ["staged"] + finally: + release.set() + host.close() + + +def test_finalizer_last_events_are_drained_after_future_is_ready(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path) + commits = [] + try: + replace_finalizer(host, completed_fit) + begin_fit(host, clock) + host.finalization.worker._future.result(timeout=2) + events = tuple(host.finalization._events.get() for _ in range(4)) + original = host.finalization.worker.finish_if_ready + def finish(commit): + # Simulate worker completion between poll's first drain and done(). + for event in events: + host.finalization._events.put(event) + return original(commit) + monkeypatch.setattr(host.finalization.worker, "finish_if_ready", finish) + monkeypatch.setattr(ArtifactPublisher, "commit", lambda _, staged: commits.append(staged)) + host.tick() + assert host.execution.session.phase == Phase.COMPLETE + assert commits == ["staged"] + finally: + host.close() + + +def test_paused_session_does_not_issue_motion_when_timer_acquires_lock(tmp_path): + host, clock = coordinator_fixture(tmp_path) + tick_entered = Event() + try: + ready(host, clock) + assert host.start().success + with ThreadPoolExecutor(1) as pool: + with host.step_data_lock: + def tick(): + tick_entered.set() + host.tick() + future = pool.submit(tick) + assert tick_entered.wait(2) + host._pause("test_visual_fault") + count = len(clock.positions) + future.result(timeout=2) + assert host.execution.session.phase == Phase.PAUSED + assert len(clock.positions) == count + finally: + host.close() + + +def test_sdk_binding_uses_supplied_clock_for_transient_health_recovery(tmp_path): + host, clock = coordinator_fixture(tmp_path, model="o12") + try: + ready(host, clock) + assert host.sdk_adapter.health().safe + clock.health_receiver("malformed") + assert not host.sdk_adapter.health().active_faults + clock.now += 1.01 + assert host.sdk_adapter.health().active_faults == ("health_report_unparseable",) + clock.health_receiver('{"position_mode": true, "active_faults": []}') + host.receive_feedback(host.profile.command.names, host.profile.command.baseline_values, + host.ports.clock_ns()) + assert host.sdk_adapter.health().safe + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_capture_provenance.py b/src/linkerhand_calibration/test/test_capture_provenance.py new file mode 100644 index 0000000..152480d --- /dev/null +++ b/src/linkerhand_calibration/test/test_capture_provenance.py @@ -0,0 +1,77 @@ +"""Old/edited journals cannot silently acquire current configuration hashes.""" + +import copy +import hashlib +import json + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.artifacts.replay import validate_capture_provenance, validate_capture_units +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION, CalibrationEngine +from linkerhand_calibration.runtime.scan_quality import observation_streams + + +def journal(): + profile = load_bundled_hand_profile("l6_right_8") + hashes = {"source_urdf_sha256": "a"*64, "profile_config_sha256": "b"*64} + matrices = {view: [[100., 0., 50.], [0., 100., 50.], [0., 0., 1.]] for view in profile.vision.view_names} + header = {"kind": "session_start", "profile_id": profile.key.profile_id, "serial_number": "VIRTUAL", + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "curve_input_domain": profile.curve_input_domain, **hashes} + reference = {"kind": "fixed_base_reference_locked", "profile_id": profile.key.profile_id, + "acquisition_policy_version": ACQUISITION_POLICY_VERSION, + "protected_hashes": {**hashes, "intrinsics_sha256": hashlib.sha256( + json.dumps(dict(sorted(matrices.items())), sort_keys=True).encode()).hexdigest()}, + "fixed_corners_by_view": {view: [[0., 0.], [1., 0.], [1., 1.], [0., 1.]] for view in matrices}, + "fixed_poses": {view: {"rotation_xyzw": [0., 0., 0., 1.], "translation_xyz_m": [0., 0., 1.]} + for view in matrices}, "moving_tag_poses": {}} + rows = [header, *({"kind": "rectified_camera_model", "view": view, "camera_matrix": matrix, + "input_is_rectified": True, "matrix_source": "CameraInfo.P[:3,:3]"} for view, matrix in matrices.items()), reference] + return profile, hashes, rows + + +@pytest.mark.parametrize("mutation", ["old_policy", "changed_hash", "missing_reference", "projection", "input_domain"]) +def test_changed_or_missing_acquisition_provenance_cannot_publish(mutation): + profile, hashes, rows = journal() + validate_capture_provenance(profile, "VIRTUAL", hashes, rows) + if mutation == "old_policy": + rows[0]["acquisition_policy_version"] = "unified_engine_v2" + elif mutation == "changed_hash": + rows[0]["source_urdf_sha256"] = "c"*64 + elif mutation == "missing_reference": + rows.pop() + elif mutation == "projection": + rows[1]["camera_matrix"][0][0] = 101. + else: + rows[0]["curve_input_domain"] = "command_rad" + with pytest.raises(ValueError, match="capture_provenance"): + validate_capture_provenance(profile, "VIRTUAL", hashes, rows) + + +def test_offline_units_use_live_quality_and_require_completed_last_attempt(): + profile, _, rows = journal() + stamp = 0 + for unit in CalibrationEngine(profile).scan_units(): + task = next(task for task in profile.motion.tasks if task.key == unit.task_key) + common = {"task_name": unit.task_key, "cycle": unit.cycle, "direction": unit.direction, "attempt": 1} + for field, joint, spec in observation_streams(profile, task): + for i in range(129): + stamp += 1 + rows.append({**common, field: joint, "view": spec.view, "image_stamp_ns": stamp, + "feedback_u8": unit.start+(unit.end-unit.start)*i/128}) + from linkerhand_calibration.runtime.steady import steady_targets + for index, target in enumerate(steady_targets(profile, unit)): + for _ in range(3): + stamp += 1 + rows.append({**common, field: joint, "view": spec.view, "image_stamp_ns": stamp, + "sample_phase": "steady", "steady_index": index, "steady_target": target}) + rows.append({**common, "kind": "scan_unit_complete", "passed": True}) + validate_capture_units(profile, rows) + failed = copy.deepcopy(rows) + failed[-1]["passed"] = False + with pytest.raises(ValueError, match="capture_incomplete"): + validate_capture_units(profile, failed) + failed = rows+[dict(rows[-2], attempt=2)] + with pytest.raises(ValueError, match="capture_incomplete"): + validate_capture_units(profile, failed) diff --git a/src/linkerhand_calibration/test/test_compact_calibration_json.py b/src/linkerhand_calibration/test/test_compact_calibration_json.py new file mode 100644 index 0000000..74ae63a --- /dev/null +++ b/src/linkerhand_calibration/test/test_compact_calibration_json.py @@ -0,0 +1,182 @@ +"""Public lookup semantics, report integrity and actual compact-file replay.""" + +import json +from dataclasses import replace + +import numpy as np +import pytest + +from test_unified_artifact_pair import dual_fixture +from test_frozen_tag_replay import transform +from linkerhand_calibration.core.urdf.acceptance import SerializedJointMapping, validate_compact_urdf_tables +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher +from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper +from linkerhand_calibration.runtime.artifacts.serializers.compact_v2 import from_report, REPORT_FILENAME + + +def stage_compact(tmp_path): + session, path, urdf, report, validator = dual_fixture(tmp_path) + (session / REPORT_FILENAME).write_text(json.dumps(report)) + payload = from_report(report) + path.write_text(json.dumps(payload)) + return session, path, urdf, report, payload, validator + + +def test_rad_lookup_report_and_manifest_reader(tmp_path): + session, path, urdf, report, payload, validator = stage_compact(tmp_path) + assert "quality" not in payload and "command_unit" not in payload + assert set(payload["joints"]["drive"]) == {"sdk_channel", "input_values", "angle_rad"} + mapping = SerializedJointMapping(payload) + expected = .2 + .8*.5 - .07 + for direction in ("", "increasing", "decreasing"): + assert mapping.evaluate([1.1], direction, active_joints=["drive"])["drive"] == pytest.approx(expected) + with pytest.raises(ValueError, match="command-only"): + SerializedJointMapping(payload, input_kind="feedback") + release = ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, + calibration_json=path, corrected_urdf=urdf, validate=validator) + manifest = json.loads(release.manifest.read_text()) + assert manifest["calibration_report"] == REPORT_FILENAME + assert len(manifest["calibration_report_sha256"]) == 64 + mapper = load_unified_mapper(release.manifest) + from linkerhand_calibration.calibrated_joint_state_bridge import load_calibrated_command_mapper + assert load_calibrated_command_mapper(path).map_positions([1.1]) == pytest.approx(mapper.map_positions([1.1])) + assert mapper.map_positions([1.1])[0] == pytest.approx(expected) + assert mapper.map_positions([1.2])[0] == pytest.approx(expected+.04) + assert mapper.map_positions([1.1])[0] == pytest.approx(expected) + feedback = load_unified_mapper(path, input_kind="feedback") + assert feedback.map_positions([.5])[0] == pytest.approx(expected) + assert mapper.map_positions([1.1])[1] == pytest.approx(payload["joints"]["passive"]["angle_rad"][32]) + with pytest.raises(ValueError, match="outside"): + mapper.map_positions([3]) + (session / REPORT_FILENAME).write_text(json.dumps(report)+" ") + with pytest.raises(ValueError, match="SHA256"): + load_unified_mapper(path) + + +def test_byte_indices_are_exactly_sdk_0_to_255_and_passive_is_recursive(tmp_path): + _, _, _, report, _ = dual_fixture(tmp_path) + report["command_unit"] = "u8" + for kind in ("command", "feedback"): + row = report["joints"]["drive"][kind+"_to_rad"] + row.update(input_domain=kind+"_u8", input_unit="u8", knots=[0, 128, 255], + valid_input_range=[0, 255], angle_rad=[1., .5, 0.], + increasing_rad=[1.01, .51, .01], decreasing_rad=[.99, .49, -.01]) + report["joints"]["nested"] = {"urdf_joint": "nested", "passive": True, + "mimic": {"joint": "passive", "multiplier": -.5, "offset_rad": .2}} + payload = from_report(report) + assert all(set(row) == {"sdk_channel", "angle_rad"} for row in payload["joints"].values()) + values = payload["joints"]["drive"]["angle_rad"] + assert len(values) == 256 + assert [values[i] for i in (0, 128, 255)] == pytest.approx([1, .5, 0]) + mapping = SerializedJointMapping(payload) + for index in range(256): + assert mapping.evaluate([index], "", active_joints=["drive"])["drive"] == values[index] + with pytest.raises(ValueError, match="outside"): + mapping.evaluate([256], "", active_joints=["drive"]) + passive = np.asarray(payload["joints"]["passive"]["angle_rad"]) + assert payload["joints"]["nested"]["angle_rad"] == pytest.approx(.2-.5*passive) + # Missing endpoint evidence is not manufactured into a 256-entry export. + report["joints"]["drive"]["command_to_rad"].update(knots=[1, 128, 255], valid_input_range=[1, 255]) + with pytest.raises(ValueError, match="measured_full_command_domain"): + from_report(report) + + +@pytest.mark.parametrize("mutation", ["short_byte", "unknown_branch", "bad_channel", "unordered", "nan"]) +def test_malformed_public_lookup_is_rejected(tmp_path, mutation): + _, _, _, _, payload, _ = stage_compact(tmp_path) + row = payload["joints"]["drive"] + if mutation == "short_byte": + payload["input_unit"] = "u8" + for entry in payload["joints"].values(): + entry.pop("input_values") + elif mutation == "unknown_branch": + row["increasing_rad"] = row["angle_rad"] + elif mutation == "bad_channel": + row["sdk_channel"] = .5 + elif mutation == "unordered": + row["input_values"][1] = row["input_values"][0] + else: + row["angle_rad"][1] = float("nan") + with pytest.raises(ValueError): + SerializedJointMapping(payload) + + +def test_passive_table_must_match_actual_urdf(tmp_path): + _, _, urdf, _, payload, _ = stage_compact(tmp_path) + payload["joints"]["passive"]["angle_rad"] = [v+.01 for v in payload["joints"]["passive"]["angle_rad"]] + with pytest.raises(ValueError, match="mimic"): + validate_compact_urdf_tables(payload, UrdfKinematicModel(urdf)) + + +def test_average_table_itself_must_pass_independent_visual_holdout(tmp_path): + session, path, urdf, report, _, validator = stage_compact(tmp_path) + # Feedback remains perfect. Alter command training curves consistently, so + # the report/lookup correspondence check passes but independent replay fails. + mapping = report["joints"]["drive"]["command_to_rad"] + for key in ("angle_rad", "increasing_rad", "decreasing_rad"): + mapping[key] = [v+.06 for v in mapping[key]] + (session / REPORT_FILENAME).write_text(json.dumps(report)) + path.write_text(json.dumps(from_report(report))) + with pytest.raises(ValueError, match="compact_command_table_holdout_failed"): + ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, + calibration_json=path, corrected_urdf=urdf, validate=validator) + assert not (tmp_path / "latest").exists() + + +def test_directional_fit_cannot_hide_excessive_backlash_in_one_table(tmp_path): + session, path, urdf, report, _, validator = stage_compact(tmp_path) + mapping = report["joints"]["drive"]["command_to_rad"] + mean = np.asarray(mapping["angle_rad"]) + mapping["increasing_rad"] = (mean+.04).tolist() + mapping["decreasing_rad"] = (mean-.04).tolist() + # Independent analytic generator, using the known mounts rather than the + # validator's FK or its fitted curve as truth. + base = transform((.13, -.22, .19), (.1, .2, .3)) + mounts = {"a": transform((.3, .2, -.4), (.003, .012, .018)), + "b": transform((-.4, .3, .2), (-.013, .009, .016))} + observations = [] + for row in validator.command_observations: + x = (row.sdk_values[0]-.1)/2 + cad = .2+.8*x+(.04 if row.sdk_directions_by_joint["drive"] == "increasing" else -.04) + proximal = transform((.2, -.1, .3), (.02, -.01, .03)) @ transform((0, cad, 0)) + distal = proximal @ transform((-.1, .2, .1), (.01, 0, .04)) @ transform((.6*cad+.04, 0, 0)) + pose = base @ (proximal if row.role == "a" else distal) @ mounts[row.role] + observations.append(replace(row, common_from_tag=tuple(map(tuple, pose)))) + validator = replace(validator, command_observations=tuple(observations)) + # Both direction-specific curves pass with frozen mounts and actual files. + path.write_text(json.dumps(report)) + assert validator(path, urdf)["steady_command_tag_holdout"]["a"]["maximum_deg"] < 1e-6 + # The same evidence must reject a single direction-independent public table. + (session / REPORT_FILENAME).write_text(json.dumps(report)) + path.write_text(json.dumps(from_report(report))) + with pytest.raises(ValueError, match="compact_command_table_holdout_failed"): + ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, + calibration_json=path, corrected_urdf=urdf, validate=validator) + assert not (tmp_path / "latest").exists() + + +@pytest.mark.parametrize("reason", ["compact_command_table_holdout_failed", "compact_byte_requires_measured_full_command_domain"]) +def test_compact_failure_has_an_actionable_chinese_reason(reason): + from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh + code, message, suggestion = reason_zh({"reason": reason+":drive"}, model_name="VIRTUAL") + assert code.startswith("FIT-COMPACT") and message and suggestion + + +@pytest.mark.parametrize("phase", ["validation", "commit"]) +def test_report_is_protected_through_atomic_publication(tmp_path, phase): + session, path, urdf, report, _, validator = stage_compact(tmp_path) + publisher = ArtifactPublisher(tmp_path, "latest") + def mutate(*args): + result = validator(*args) + (session / REPORT_FILENAME).write_text(json.dumps(report)+" ") + return result + if phase == "validation": + with pytest.raises(ValueError, match="report changed"): + publisher.prepare(session_directory=session, calibration_json=path, corrected_urdf=urdf, validate=mutate) + else: + staged = publisher.prepare(session_directory=session, calibration_json=path, corrected_urdf=urdf, validate=validator) + (session / REPORT_FILENAME).write_text(json.dumps(report)+" ") + with pytest.raises(ValueError, match="report changed"): + publisher.commit(staged) + assert not (tmp_path / "latest").exists() diff --git a/src/linkerhand_calibration/test/test_config.py b/src/linkerhand_calibration/test/test_config.py index c1f4572..040e06a 100644 --- a/src/linkerhand_calibration/test/test_config.py +++ b/src/linkerhand_calibration/test/test_config.py @@ -257,7 +257,7 @@ def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None: assert parameters["passive_ip_multiplier"] == 1.02 assert parameters["pnp_minimum_valid_rate"] == 0.95 assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5 - assert parameters["pnp_reprojection_tie_px"] == 1.5 + assert "pnp_reprojection_tie_px" not in parameters # Shared runtime policy. assert parameters["pnp_tracker_reset_seconds"] == 5.0 assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0 assert parameters["pnp_group_relative_translation_scale_m"] == 0.01 diff --git a/src/linkerhand_calibration/test/test_declared_byte_direction.py b/src/linkerhand_calibration/test/test_declared_byte_direction.py new file mode 100644 index 0000000..59da0b5 --- /dev/null +++ b/src/linkerhand_calibration/test/test_declared_byte_direction.py @@ -0,0 +1,25 @@ +import numpy as np +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting.observed_motion import fit_byte_observed_motion + + +def test_positive_sdk_direction_reorients_observation_axis_and_table_together(): + rows = [] + mount = Rotation.from_euler("xyz", [.23, -.17, .31]) + for cycle in range(4): + for direction in ("increasing", "decreasing"): + for index, value in enumerate(np.linspace(0, 255, 86).astype(int)): + angle = value/255*.8 + q = Rotation.from_euler("z", angle) * mount + rows.append({"sample_id": f"{cycle}/{direction}/{index}", "cycle": cycle, + "direction": direction, "command_u8": int(value), + "relative_quaternion_xyzw": q.as_quat().tolist()}) + curves, errors, evidence = fit_byte_observed_motion({"arbitrary_joint": rows}, source_model=None, + mimic_sources={}, baseline_by_joint={"arbitrary_joint": 0}, direction_by_joint={"arbitrary_joint": 1}, + input_domain="command_u8") + curve = curves["arbitrary_joint"] + assert curve.angle_rad[-1] > curve.angle_rad[0] + assert abs(curve.angle_rad[-1]-.8) < 1e-6 + assert max(abs(e) for e in errors["arbitrary_joint"]) < 1e-6 + assert not evidence diff --git a/src/linkerhand_calibration/test/test_dual_mapping.py b/src/linkerhand_calibration/test/test_dual_mapping.py new file mode 100644 index 0000000..495835a --- /dev/null +++ b/src/linkerhand_calibration/test/test_dual_mapping.py @@ -0,0 +1,123 @@ +"""Command calibration uses visual truth, never feedback relabelled as command.""" + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.domain.measurement import JointCurveFit +from linkerhand_calibration.core.domain.result import CalibrationResult, JointMapping +from linkerhand_calibration.core.fitting.command_mapping import fit_command_mappings +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.engine import CalibrationEngine +from linkerhand_calibration.runtime.steady import steady_targets +from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution + + +def independent_steady_fixture(layout): + profile = load_bundled_hand_profile(layout) + task = profile.motion.tasks[0] + name = task.joints[0] + channel = task.command_index + lo, hi = sorted((task.start_value, task.end_value)) + native = profile.command.unit + # Deliberately different coordinate systems. The synthetic camera observes + # actual q(command); the SDK returns feedback = 0.8*command+offset. + offset = 10.0 if native == "u8" else .02 + scale = -.002 if native == "u8" else .7*profile.command.joint_directions[channel] + baseline = profile.command.baseline_values[channel] + q = lambda u: scale*(u-baseline) + knots = (.8*lo+offset, .8*hi+offset) + angles = (q(lo), q(hi)) + curve = JointCurveFit(angles, angles, angles, + {"space": "relative_rotation_3d", "reference_quaternion_xyzw": [0, 0, 0, 1], + "axis_xyz": [0, 0, 1], "input_knots": knots}, 0., 0., {}) + feedback = JointMapping(name, channel, f"feedback_{native}", knots, angles, angles, angles) + fit = CalibrationResult({name: curve}, {name: feedback}, {name: 0.}, {}, {}, {}, None, + reference_inputs={name: .8*baseline+offset}) + rows = [] + for unit in CalibrationEngine(profile).scan_units(): + if unit.task_key != task.key: + continue + for index, command in enumerate(steady_targets(profile, unit)): + for frame in range(3): + vector = list(profile.command.baseline_values) + for i, v in task.auxiliary_commands: + vector[i] = v + vector[channel] = command + rows.append({"joint": name, "task_name": task.key, "view": task.view, + "sample_id": f"{unit.cycle}:{unit.direction}:{index}:{frame}", + "sample_phase": "steady", "steady_target": command, "steady_index": index, + "cycle": unit.cycle, "direction": unit.direction, + f"command_{native}": command, f"feedback_{native}": .8*command+offset, + f"command_vector_{native}": vector, + "relative_quaternion_xyzw": Rotation.from_rotvec([0, 0, q(command)]).as_quat()}) + return profile, fit, name, rows, q + + +@pytest.mark.parametrize("layout", ["l6_right_8", "o12_right_16"]) +def test_command_map_is_measured_not_feedback_relabelled(layout): + profile, fit, name, rows, q = independent_steady_fixture(layout) + result = fit_command_mappings(profile, fit, {name: rows}) + mapping = result.command_mappings[name] + for command in np.linspace(mapping.knots[0], mapping.knots[-1], 30): + if profile.command.unit == "u8": + command = round(command) + assert mapping.evaluate(command, "increasing") == pytest.approx(q(command), abs=1e-9) + assert result.output_mappings[name] is fit.output_mappings[name] + assert result.command_mappings[name].knots != result.output_mappings[name].knots + + +def test_holdout_cannot_update_a_command_fit_or_tag_mount(): + profile, fit, name, rows, _ = independent_steady_fixture("o12_right_16") + for row in rows: + if row["cycle"] == 3: + row["relative_quaternion_xyzw"] = (Rotation.from_rotvec([0, 0, .09]) + * Rotation.from_quat(row["relative_quaternion_xyzw"])).as_quat() + with pytest.raises(ValueError, match="steady_command_holdout_failed"): + fit_command_mappings(profile, fit, {name: rows}) + assert not fit.command_mappings + + +def test_missing_steady_nodes_and_changed_auxiliary_are_not_silently_interpolated(): + profile, fit, name, rows, _ = independent_steady_fixture("o12_right_16") + with pytest.raises(ValueError, match="steady_training_incomplete"): + fit_command_mappings(profile, fit, {name: [r for r in rows if r["cycle"] != 1]}) + rows[0]["command_vector_rad"][1] = -.1 + with pytest.raises(ValueError, match="scan_changed_multiple"): + fit_command_mappings(profile, fit, {name: rows}) + + +def test_steady_motion_is_bounded_and_stability_is_not_tracking_equality(): + profile = load_bundled_hand_profile("o12_right_16") + initial = profile.command.baseline_values + target = list(initial); target[0] = .1 + segment = MotionExecution(profile, MotionCommand("steady", tuple(target), .1, + command_index=0, direction="increasing", steady_index=1), initial_command=initial, + initial_feedback=initial, now=0, identity="steady") + ready = False + for index in range(500): + now = index*.02 + command = segment.sample(now) + feedback = list(command); feedback[0] -= .02 + segment.observe(feedback, stamp=index, now=now) + ready |= segment.steady_ready(now) + if segment.arrived(now): + break + assert ready and segment.arrived(now) + assert now-segment.finished_at < 2.03 + + +def test_scan_and_steady_images_have_separate_input_semantics(): + from linkerhand_calibration.runtime.acquisition import accepted_joint_records + profile = load_bundled_hand_profile("l6_right_8") + task = profile.motion.tasks[0] + row = {"joint": task.joints[0], "task_name": task.key, "cycle": 0, "direction": "increasing", + "view": task.view, "image_stamp_ns": 1, "relative_quaternion_xyzw": [0, 0, 0, 1], + "command_u8": 10., "feedback_u8": 1.2} + original = dict(row) + result = accepted_joint_records(profile, [row])[task.joints[0]][0] + assert result["command_u8"] == 10. and result["feedback_u8"] == 1.2 and row == original + # A partially captured new retry must not borrow the old steady attempt. + steady = dict(row, sample_phase="steady", attempt=1) + retry = dict(row, attempt=2, image_stamp_ns=2) + assert not accepted_joint_records(profile, [steady, retry], sample_phase="steady")[task.joints[0]] diff --git a/src/linkerhand_calibration/test/test_finalization_worker.py b/src/linkerhand_calibration/test/test_finalization_worker.py new file mode 100644 index 0000000..ff861ba --- /dev/null +++ b/src/linkerhand_calibration/test/test_finalization_worker.py @@ -0,0 +1,58 @@ +from threading import Event + +import pytest + +from linkerhand_calibration.runtime.artifacts.worker import FinalizationWorker + + +def test_fit_does_not_block_control_and_abort_cannot_publish(): + entered, release, finished = Event(), Event(), Event() + def fit(*, publish, cancelled): + assert not publish + entered.set() + assert release.wait(2) + assert cancelled() + finished.set() + return "not publishable" + worker = FinalizationWorker() + commits = [] + try: + worker.start(fit) + assert entered.wait(2) + # A deterministic blocked fit must not block any controller poll. + for _ in range(100): + assert worker.finish_if_ready(commits.append) is None + worker.cancel() + release.set() + assert finished.wait(2) + assert worker.finish_if_ready(commits.append) is None + assert commits == [] + finally: + release.set() + worker.close() + + +def test_success_commits_once_and_failure_never_retries(): + for fail in (False, True): + worker = FinalizationWorker() + commits = [] + try: + def fit(**kwargs): + assert kwargs["publish"] is False + return ("json", "fit", "staged") + worker.start(fit) + worker._future.result(timeout=2) + def commit(value): + commits.append(value) + if fail: + raise ValueError("invalid staged hash") + if fail: + with pytest.raises(ValueError, match="hash"): + worker.finish_if_ready(commit) + else: + assert worker.finish_if_ready(commit) == ("json", "fit", "staged") + assert worker.finish_if_ready(commit) is None + assert len(commits) == 1 + finally: + worker.close() + diff --git a/src/linkerhand_calibration/test/test_frozen_rotation_reference.py b/src/linkerhand_calibration/test/test_frozen_rotation_reference.py new file mode 100644 index 0000000..601d6fa --- /dev/null +++ b/src/linkerhand_calibration/test/test_frozen_rotation_reference.py @@ -0,0 +1,47 @@ +import math + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting.spatial import ( + fit_rotation_joint_curve, rotation_curve_holdout_errors, +) + + +def rows(cycles, *, slip=0.0, transverse=False): + mounting = Rotation.from_euler("xyz", (0.2, -0.3, 0.1)) + result = [] + for cycle in cycles: + for direction in ("decreasing", "increasing"): + for command in range(256): + angle = (255 - command)/255 + (0.02 if direction == "increasing" else 0) + rotation = mounting * Rotation.from_euler("z", angle) + if slip: + rotation = rotation * Rotation.from_euler("x" if transverse else "z", slip) + result.append({"cycle": cycle, "direction": direction, "command_u8": command, + "relative_quaternion_xyzw": rotation.as_quat().tolist()}) + return result + + +@pytest.mark.parametrize("transverse", [False, True]) +def test_live_shared_fitter_does_not_rezero_a_slipped_holdout_tag(transverse): + fit = fit_rotation_joint_curve(rows((0, 1, 2)), zero_command_u8=255, + canonical_zero_direction="decreasing") + clean = rotation_curve_holdout_errors(fit, rows((3,)), zero_command_u8=255) + assert np.max(np.abs(clean)) < 1e-7 + slipped = rotation_curve_holdout_errors(fit, + rows((3,), slip=math.radians(5), transverse=transverse), zero_command_u8=255) + assert np.min(np.abs(slipped)) > math.radians(4.9) + + +def test_live_shared_fitter_rejects_training_as_holdout(): + fit = fit_rotation_joint_curve(rows((0, 1, 2)), zero_command_u8=255) + with pytest.raises(ValueError, match="training cycles"): + rotation_curve_holdout_errors(fit, rows((2,)), zero_command_u8=255) + + +def test_two_directions_keep_the_same_training_reference(): + fit = fit_rotation_joint_curve(rows((0, 1, 2)), zero_command_u8=255) + assert fit.increasing_rad[-1] - fit.decreasing_rad[-1] == pytest.approx(0.02, abs=1e-7) + assert max(abs(v) for v in rotation_curve_holdout_errors(fit, rows((3,)), zero_command_u8=255)) < 1e-7 diff --git a/src/linkerhand_calibration/test/test_frozen_tag_replay.py b/src/linkerhand_calibration/test/test_frozen_tag_replay.py new file mode 100644 index 0000000..0661248 --- /dev/null +++ b/src/linkerhand_calibration/test/test_frozen_tag_replay.py @@ -0,0 +1,135 @@ +"""Independent rigid-body generator: never uses the fitted FK to make truth.""" + +from dataclasses import replace +import hashlib +import json + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting.tag_installation import ( + TagTrainingPose, fit_tag_installations, matrix_tuple, +) +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.core.urdf.plan import build_standard_correction_plan +from linkerhand_calibration.core.urdf.tag_acceptance import TagHoldout, validate_serialized_tag_holdout +from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher, FrozenTagArtifactValidator + + +def transform(rpy=(0, 0, 0), xyz=(0, 0, 0)): + value = np.eye(4) + value[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + value[:3, 3] = xyz + return value + + +def fixture(tmp_path, unit="rad"): + source = tmp_path / "raw.urdf" + source.write_text(''' + + + + + + + + ''') + zeros = {"drive": 0.07, "passive": -0.03} + rights = {"drive": ("origin.rpy", "limit.lower", "limit.upper"), + "passive": ("origin.rpy", "limit.lower", "limit.upper", "mimic.offset")} + digest = hashlib.sha256(source.read_bytes()).hexdigest() + plan = build_standard_correction_plan(source_urdf=source, source_sha256=digest, + zero_offsets_rad=zeros, authorized_fields=rights) + base = transform((0.13, -0.22, 0.19), (0.1, 0.2, 0.3)) + mounts = {"a": transform((0.3, 0.2, -0.4), (0.003, 0.012, 0.018)), + "b": transform((-0.4, 0.3, 0.2), (-0.013, 0.009, 0.016))} + training, holdout = [], [] + for cycle in range(4): + for direction in ("increasing", "decreasing"): + for i, phase in enumerate(np.linspace(0, 1, 80)): + sdk = phase if unit == "rad" else round(phase*255) + x = sdk if unit == "rad" else sdk/255 + # Known independent coordinates including direction-dependent + # backlash. Neither source-model FK nor the fitted curve makes truth. + cad = 0.2 + 0.8*x + (0.006 if direction == "increasing" else -0.006) + child = 0.6*cad + 0.04 + proximal = transform((0.2, -0.1, 0.3), (0.02, -0.01, 0.03)) @ transform((0, cad, 0)) + distal = proximal @ transform((-0.1, 0.2, 0.1), (0.01, 0, 0.04)) @ transform((child, 0, 0)) + for role, pose in (("a", proximal), ("b", distal)): + image = f"camera:{cycle}:{direction}:{i}" + observed = matrix_tuple(base @ pose @ mounts[role]) + if cycle < 3: + training.append(TagTrainingPose(image, role, cycle, observed, {"drive": cad, "passive": child})) + else: + holdout.append(TagHoldout(image, role, cycle, (sdk,), {"drive": direction}, observed)) + frozen = fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, + link_by_role={"a": "proximal", "b": "distal"}, observations=training) + knots = np.linspace(0, 1 if unit == "rad" else 255, 65 if unit == "rad" else 256) + angles = 0.2 + 0.8*knots/(1 if unit == "rad" else 255) - zeros["drive"] + row = {"urdf_joint": "drive", "motor_index": 0, "angle_rad": angles.tolist(), + "increasing_rad": (angles+0.006).tolist(), "decreasing_rad": (angles-0.006).tolist()} + if unit == "rad": + row["curve_input_knots_rad"] = knots.tolist() + payload = {"profile_id": "VIRTUAL/right/new/v1", "schema_version": 7 if unit == "rad" else 6, + "curve_input_domain": "feedback_" + unit, "joints": {"drive": row}} + return source, plan, base, training, frozen, holdout, payload + + +@pytest.mark.parametrize("unit", ["rad", "u8"]) +def test_written_standard_urdf_replays_independent_poses_and_publishes(tmp_path, unit): + source, plan, base, _, mounts, holdout, payload = fixture(tmp_path, unit) + session = tmp_path / "session" + session.mkdir() + urdf = plan.write(source, session / "corrected.urdf") + calibration = session / "calibration.json" + calibration.write_text(json.dumps(payload)) + loaded = [] + validator = FrozenTagArtifactValidator(payload["profile_id"], source, plan.source_sha256, + plan.authorized_fields, plan.zero_offsets_rad, matrix_tuple(base), mounts, + tuple(holdout), ("a", "b"), lambda path: loaded.append(path)) + release = ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, + calibration_json=calibration, corrected_urdf=urdf, validate=validator) + assert release.pointer.resolve() == session + evidence = json.loads(release.manifest.read_text())["validation"] + assert evidence["final_file_spatial_replay"] == "passed" + assert evidence["tag_holdout"]["b"]["maximum_deg"] < 1e-6 + assert loaded == [urdf] + + +@pytest.mark.parametrize("corruption", ["mimic", "origin", "json_zero", "tag_slip"]) +def test_final_file_or_tag_errors_cannot_be_absorbed_into_new_mounts(tmp_path, corruption): + source, plan, base, _, mounts, holdout, payload = fixture(tmp_path) + corrected = plan.write(source, tmp_path / "corrected.urdf") + if corruption == "mimic": + corrected.write_text(corrected.read_text().replace('multiplier="0.6"', 'multiplier="0.8"')) + elif corruption == "origin": + import xml.etree.ElementTree as ET + tree = ET.parse(corrected) + tree.find("joint/origin").set("rpy", "0.4 0.1 0.3") + tree.write(corrected) + elif corruption == "json_zero": + for name in ("angle_rad", "increasing_rad", "decreasing_rad"): + payload["joints"]["drive"][name] = [v + 0.1 for v in payload["joints"]["drive"][name]] + else: + holdout = [replace(row, common_from_tag=matrix_tuple( + np.asarray(row.common_from_tag) @ transform((0.1, 0, 0)))) for row in holdout] + with pytest.raises(ValueError, match="holdout_failed"): + validate_serialized_tag_holdout(corrected_urdf=corrected, payload=payload, + common_from_base=base, installations=mounts, observations=holdout, required_roles=("a", "b")) + + +def test_installation_is_unique_across_cycles_and_holdout_never_fits(tmp_path): + source, _, base, training, mounts, holdout, _ = fixture(tmp_path) + with pytest.raises(ValueError, match="training"): + fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, + link_by_role={"a": "proximal", "b": "distal"}, observations=[replace(training[0], cycle=3)]) + slipped = [replace(row, common_from_tag=matrix_tuple(np.asarray(row.common_from_tag) @ + transform((0.12, 0, 0)))) if row.cycle == 2 else row for row in training] + with pytest.raises(ValueError, match="not rigid"): + fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, + link_by_role={"a": "proximal", "b": "distal"}, observations=slipped) + # Same physical Tag observed through two tasks is deduplicated, not fitted twice. + duplicate = fit_tag_installations(source_model=UrdfKinematicModel(source), common_from_base=base, + link_by_role={"a": "proximal", "b": "distal"}, observations=training+training) + assert duplicate == mounts diff --git a/src/linkerhand_calibration/test/test_g20_right_product.py b/src/linkerhand_calibration/test/test_g20_right_product.py index e3d130e..1fe6fc0 100644 --- a/src/linkerhand_calibration/test/test_g20_right_product.py +++ b/src/linkerhand_calibration/test/test_g20_right_product.py @@ -1,23 +1,15 @@ from dataclasses import replace -from collections import deque import json import math from pathlib import Path from types import SimpleNamespace -from xml.etree import ElementTree as ET import numpy as np import pytest -from scipy.spatial.transform import Rotation import yaml -from linkerhand_calibration.core import ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, -) from linkerhand_calibration.full_hand import ( G20_COMBINATION_REQUIRED_TARGET_KEYS, - G20_REFERENCE_THUMB_CMC_JOINTS, G20_RIGHT_19_LAYOUT, JointCurveFit, build_compact_payload, @@ -29,7 +21,7 @@ from linkerhand_calibration.operator_report import ( classify_error, render_progress_zh, ) -from linkerhand_calibration.models.g20.runner import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.runner import ( _automatic_resume_candidate, _calibration_node_exited_before_status, _launch_command, @@ -41,40 +33,11 @@ from linkerhand_calibration.publication import ( ACTIVE_ZERO_JOINTS, PASSIVE_JOINTS, atomic_session_pointer, - build_mujoco_validation_commands, - finalize_session_artifacts, - session_artifact_paths, - standalone_thumb_offsets, - validate_runtime_curves_against_urdf_limits, - verify_corrected_urdf, - verify_partial_scope_preserves_certified_zeros, - verify_urdf_mesh_resources, ) from linkerhand_calibration.storage import atomic_write_json -from linkerhand_calibration.three_camera_node import ( - G20ThreeCameraCalibrationNode, - STEADY_COMMAND_CHECKPOINTS, - SweepItem, - _combination_joint_angles, - _combination_motor_directions, - _combination_validation_items, - _model_link_in_observer_base, - _partial_scope_frozen_zero_offsets, - _palm_axis_observer_schema, - _palm_axis_resume_policy, - build_standalone_thumb_payload, - recalibration_quality_joints, - recalibration_task_keys, - _steady_checkpoint_commands, - _unresolved_fit_failure_tasks, - combination_target_coverage, - resume_tasks_invalidated_by_tag_size_changes, - resumable_completed_task_prefix, -) from linkerhand_calibration.urdf_zero import ( RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, UrdfKinematicModel, - get_zero_calibration_profile, write_zero_corrected_urdf, ) from linkerhand_calibration.runtime import ACQUISITION_POLICY_VERSION @@ -251,81 +214,6 @@ def test_schema_v2_loads_the_same_registered_product(tmp_path) -> None: assert config.calibration_contract.typed_profile.key == config.profile_key -def test_first_round_uses_nine_bidirectional_steady_commands() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - spec = profile.sweep_specs[0] - decreasing = _steady_checkpoint_commands( - profile, SweepItem(spec, 0, "decreasing") - ) - increasing = _steady_checkpoint_commands( - profile, SweepItem(spec, 0, "increasing") - ) - assert (255, *decreasing) == STEADY_COMMAND_CHECKPOINTS - assert (0, *increasing) == tuple(reversed(STEADY_COMMAND_CHECKPOINTS)) - assert _steady_checkpoint_commands( - profile, SweepItem(spec, 1, "decreasing") - ) == () - - -def test_combination_coverage_requires_observation_and_validation_per_target() -> None: - assert G20_REFERENCE_THUMB_CMC_JOINTS == { - "thumb_cmc_pitch", - "thumb_cmc_roll", - "thumb_cmc_yaw", - } - assert not any( - "thumb" in key for key in G20_COMBINATION_REQUIRED_TARGET_KEYS - ) - incomplete = combination_target_coverage({}, {}) - assert incomplete["coverage_passed"] is False - assert set(incomplete["missing_validation_targets"]) == set( - G20_COMBINATION_REQUIRED_TARGET_KEYS - ) - - complete = combination_target_coverage( - {key: 2 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS}, - {key: 1 for key in G20_COMBINATION_REQUIRED_TARGET_KEYS}, - ) - assert complete["coverage_passed"] is True - - -def _complete_resume_task_rows(profile, spec) -> list[dict]: - rows: list[dict] = [] - for joint in spec.joints: - for cycle in range(4): - for direction in ("decreasing", "increasing"): - rows.extend( - { - "kind": "sample", - "attempt": 1, - "task_name": spec.key, - "joint": joint, - "cycle": cycle, - "direction": direction, - "requested_command_u8": ( - 0 if direction == "decreasing" else 255 - ), - "feedback_u8": command, - } - for command in (*range(32), 255) - ) - for direction in ("decreasing", "increasing"): - rows.extend( - { - "kind": "steady_command_sample", - "attempt": 1, - "task_name": spec.key, - "joint": joint, - "cycle": 0, - "direction": direction, - "requested_command_u8": command, - "feedback_u8": command, - } - for command in STEADY_COMMAND_CHECKPOINTS - ) - return rows - - _FIXED_BASE_CORNERS_BY_VIEW = { "front": ( (100.0, 100.0), @@ -372,590 +260,6 @@ def _resume_reference_views( } -def test_resume_reuses_only_a_fully_committed_task_prefix() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - first, second = profile.sweep_specs[:2] - rows = _complete_resume_task_rows(profile, first) - # A fragment of the following task must be discarded as a unit. - rows.append( - { - "kind": "sample", - "attempt": 1, - "task_name": second.key, - "joint": second.joints[0], - "cycle": 0, - "direction": "decreasing", - "feedback_u8": 255, - } - ) - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, - 255, 255, 255, 255, 255, 255, 255, 255, 255, 255], - rows, - ) - - assert completed == (first.key,) - assert reusable - assert {row["task_name"] for row in reusable} == {first.key} - - -def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - spec = next( - item for item in profile.sweep_specs if item.key == "pinky_pitch_side" - ) - rows = _complete_resume_task_rows(profile, spec) - rows.append( - { - "kind": "palm_axis_sample", - "attempt": 1, - "task_name": spec.key, - "source_joint": "pinky_mcp_pitch_front_axis", - "cycle": 0, - "direction": "decreasing", - "feedback_u8": 240, - } - ) - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - rows, - allow_sparse=True, - ) - - assert completed == (spec.key,) - assert sum( - row.get("kind") == "palm_axis_sample" for row in reusable - ) == 1 - - completed_without_side_channel, _ = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - _complete_resume_task_rows(profile, spec), - allow_sparse=True, - ) - assert completed_without_side_channel == (spec.key,) - - -def test_checkpoint_reacquires_new_thumb_yaw_side_channel_once() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - old_capabilities = set(profile.capabilities) - { - "palm_axis_relative_motion_v3" - } - - compatible, invalidated = _palm_axis_resume_policy( - profile, {"capabilities": sorted(old_capabilities)} - ) - - assert compatible is False - assert set(invalidated) == {spec.key for spec in profile.sweep_specs} - - compatible, invalidated = _palm_axis_resume_policy( - profile, - { - "capabilities": sorted(profile.capabilities), - "palm_axis_observers": _palm_axis_observer_schema(profile), - }, - ) - assert compatible is True - assert invalidated == () - - -def test_sparse_resume_keeps_complete_tasks_after_failed_task() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - first, failed, later = profile.sweep_specs[:3] - rows = [ - *(_complete_resume_task_rows(profile, first)), - *(_complete_resume_task_rows(profile, failed)), - *(_complete_resume_task_rows(profile, later)), - { - "kind": "fit_failure", - "task_name": failed.key, - "view": failed.view, - "motor_index": failed.motor_index, - "joints": list(failed.joints), - "attempt": 1, - "failures": [ - { - "joint": failed.joints[0], - "metric": "monotonic_correction_deg", - "actual": 2.1, - "limit": 2.0, - } - ], - }, - ] - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - rows, - allow_sparse=True, - ) - - assert completed == (first.key, later.key) - assert {row["task_name"] for row in reusable} == { - first.key, - later.key, - } - - -def test_corrected_distal_tag_sizes_invalidate_only_four_pip_tasks() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - previous = { - tag_id: (0.010 if tag_id in {7, 14, 16, 18} else 0.016) - for tag_id in range(19) - } - current = {tag_id: 0.016 for tag_id in range(19)} - - changed, invalidated = resume_tasks_invalidated_by_tag_size_changes( - profile, previous, current - ) - - assert changed == (7, 14, 16, 18) - assert invalidated == ( - "pinky_pip_side", - "ring_pip_side", - "middle_pip_side", - "index_pip_side", - ) - assert len(profile.sweep_specs) - len(invalidated) == 12 - - -def test_sparse_resume_revalidates_retired_product_dynamic_hysteresis() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - task = next( - spec for spec in profile.sweep_specs - if spec.key == "thumb_mcp_ip_front" - ) - rows = [ - *_complete_resume_task_rows(profile, task), - { - "kind": "fit_failure", - "task_name": task.key, - "view": task.view, - "motor_index": task.motor_index, - "joints": list(task.joints), - "attempt": 1, - "failures": [ - { - "joint": "thumb_ip", - "metric": "hysteresis_deg", - "actual": 2.2, - "limit": 2.0, - }, - { - "joint": "thumb_mcp", - "metric": "command_direction_gap_deg", - "actual": 2.4, - "limit": 2.0, - }, - ], - }, - ] - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - rows, - allow_sparse=True, - ) - - assert completed == (task.key,) - assert {row["task_name"] for row in reusable} == {task.key} - - -def test_resume_migrates_completed_legacy_split_roll_without_rescanning() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - merged = next( - spec for spec in profile.sweep_specs - if spec.key == "pinky_roll_multiview" - ) - rows = [ - row - for spec in profile.sweep_specs[:4] - for row in _complete_resume_task_rows(profile, spec) - ] - for source in _complete_resume_task_rows(profile, merged): - row = dict(source) - row["task_name"] = ( - "pinky_roll_side" - if row["joint"] == "pinky_mcp_roll_side" - else "pinky_roll_front" - ) - rows.append(row) - for joint in merged.joints: - old_task = ( - "pinky_roll_side" - if joint == "pinky_mcp_roll_side" - else "pinky_roll_front" - ) - for cycle in range(4): - for direction in ("decreasing", "increasing"): - rows.append( - { - "kind": "baseline_hold_sample", - "attempt": 1, - "task_name": old_task, - "joint": joint, - "cycle": cycle, - "direction": direction, - "feedback_u8": 127, - } - ) - - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - - completed, reusable = resumable_completed_task_prefix( - profile, 4, baseline, rows - ) - - assert completed == tuple(spec.key for spec in profile.sweep_specs[:5]) - migrated = [ - row for row in reusable - if row["task_name"] == "pinky_roll_multiview" - ] - assert {row["joint"] for row in migrated} == set(merged.joints) - assert {row["resume_source_task_name"] for row in migrated} == { - "pinky_roll_front", - "pinky_roll_side", - } - - -def test_resume_falls_back_from_an_interrupted_retry_to_complete_attempt() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - pip = next( - spec for spec in profile.sweep_specs - if spec.key == "pinky_pip_side" - ) - pip_index = profile.sweep_specs.index(pip) - rows = [ - row - for spec in profile.sweep_specs[: pip_index + 1] - for row in _complete_resume_task_rows(profile, spec) - ] - # The operator stopped the unnecessary second retry after only two of the - # nine checkpoints. Those partial rows must not hide attempt 1. - for joint in pip.joints: - for command in (255, 224): - rows.append( - { - "kind": "steady_command_sample", - "attempt": 2, - "task_name": pip.key, - "joint": joint, - "cycle": 0, - "direction": "decreasing", - "requested_command_u8": command, - "feedback_u8": command, - } - ) - rows.append( - { - "kind": "fit_failure", - "task_name": pip.key, - "view": pip.view, - "motor_index": pip.motor_index, - "joints": list(pip.joints), - "attempt": 1, - "failures": [ - { - "joint": "pinky_pip", - "metric": "rotation_orthogonal_rms_deg", - "actual": 8.176357, - "limit": 2.5, - } - ], - } - ) - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - rows, - ) - - assert completed == tuple( - spec.key for spec in profile.sweep_specs[: pip_index + 1] - ) - checkpoints = [ - row - for row in reusable - if row["kind"] == "steady_command_sample" - and row["direction"] == "decreasing" - ] - assert checkpoints - assert {int(row.get("attempt", 1)) for row in checkpoints} == {1} - - -def test_resume_retry_tombstone_discards_superseded_rows() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - task = next( - spec - for spec in profile.sweep_specs - if spec.key == "pinky_roll_multiview" - ) - old_checkpoints = [] - for joint in task.joints: - old_checkpoints.extend( - { - "kind": "steady_command_sample", - "attempt": 1, - "task_name": task.key, - "joint": joint, - "motor_index": task.motor_index, - "cycle": 0, - "direction": "decreasing", - "requested_command_u8": command, - "feedback_u8": command, - "resume_test_generation": "superseded", - } - for command in STEADY_COMMAND_CHECKPOINTS - ) - rows = [ - *old_checkpoints, - { - # Legacy retry events had no task_name or attempt. Motor, joint, - # cycle and direction must still act as a durable tombstone. - "kind": "automatic_sweep_retry", - "view": task.view, - "motor_index": task.motor_index, - "joints": list(task.joints), - "cycle": 0, - "direction": "decreasing", - "retry": 1, - }, - *_complete_resume_task_rows(profile, task), - ] - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - rows, - allow_sparse=True, - ) - - assert completed == (task.key,) - restored_checkpoints = [ - row - for row in reusable - if row["kind"] == "steady_command_sample" - and row["direction"] == "decreasing" - ] - assert len(restored_checkpoints) == 2 * len(STEADY_COMMAND_CHECKPOINTS) - assert all( - row.get("resume_test_generation") != "superseded" - for row in restored_checkpoints - ) - - -def test_interrupted_retry_does_not_resurrect_rejected_sweep() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - task = next( - spec - for spec in profile.sweep_specs - if spec.key == "pinky_roll_multiview" - ) - rows = [ - *_complete_resume_task_rows(profile, task), - { - "kind": "automatic_sweep_retry", - "task_name": task.key, - "attempt": 1, - "view": task.view, - "motor_index": task.motor_index, - "joints": list(task.joints), - "cycle": 0, - "direction": "decreasing", - "retry": 1, - }, - ] - - completed, reusable = resumable_completed_task_prefix( - profile, - 4, - [255] * 20, - rows, - allow_sparse=True, - ) - - assert completed == () - assert reusable == () - - -def test_resume_rejects_unresolved_fit_but_accepts_retired_model_metric() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - first = profile.sweep_specs[0] - unresolved_rows = _complete_resume_task_rows(profile, first) - unresolved_rows.append( - { - "kind": "fit_failure", - "task_name": first.key, - "view": first.view, - "motor_index": first.motor_index, - "joints": list(first.joints), - "attempt": 1, - "failures": [ - { - "joint": first.joints[0], - "metric": "arc_deg", - "actual": 10.0, - "limit": 15.0, - } - ], - } - ) - completed, _ = resumable_completed_task_prefix( - profile, 4, [255] * 20, unresolved_rows - ) - assert completed == () - - pitch = next( - spec for spec in profile.sweep_specs if spec.key == "pinky_pitch_side" - ) - retired_rows = [ - { - "kind": "sample", - "task_name": pitch.key, - "attempt": 3, - }, - { - "kind": "fit_failure", - "view": pitch.view, - "motor_index": pitch.motor_index, - "joints": list(pitch.joints), - "attempt": 3, - "failures": [ - { - "joint": "pinky_mcp_pitch", - "metric": "rotation_circle_axis_difference_deg", - "actual": 4.0, - "limit": 1.0, - } - ], - }, - ] - assert _unresolved_fit_failure_tasks(profile, retired_rows) == set() - - -def test_resume_revalidates_retired_passive_dip_position_metrics() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - pip = next( - spec for spec in profile.sweep_specs if spec.key == "pinky_pip_side" - ) - rows = [ - { - "kind": "sample", - "task_name": pip.key, - "attempt": 3, - }, - { - "kind": "fit_failure", - "task_name": pip.key, - "view": pip.view, - "motor_index": pip.motor_index, - "joints": list(pip.joints), - "attempt": 3, - "failures": [ - { - "joint": "pinky_dip", - "metric": "axis_pose_line_rms_mm", - "actual": 1.9, - "limit": 1.0, - }, - { - "joint": "pinky_dip", - "metric": "rotation_circle_axis_difference_deg", - "actual": 15.5, - "limit": 1.0, - }, - ], - }, - ] - - assert _unresolved_fit_failure_tasks(profile, rows) == set() - - -def test_resume_revalidates_thumb_ip_position_diagnostic() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - task = next( - spec - for spec in profile.sweep_specs - if spec.key == "thumb_mcp_ip_front" - ) - rows = [ - { - "kind": "sample", - "task_name": task.key, - "attempt": 3, - }, - { - "kind": "fit_failure", - "task_name": task.key, - "view": task.view, - "motor_index": task.motor_index, - "joints": list(task.joints), - "attempt": 3, - "failures": [ - { - "joint": "thumb_ip", - "metric": "axis_pose_line_rms_mm", - "actual": 1.4, - "limit": 1.0, - } - ], - }, - ] - - assert _unresolved_fit_failure_tasks(profile, rows) == set() - - -def test_resume_retires_validation_only_cross_view_curve_failure() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - spec = next( - item for item in profile.sweep_specs - if item.key == "ring_roll_multiview" - ) - rows = [ - { - "kind": "sample", - "task_name": spec.key, - "attempt": 3, - }, - { - "kind": "fit_failure", - "task_name": spec.key, - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "attempt": 3, - "failures": [ - { - "joint": "ring_mcp_roll", - "metric": "cross_view_roll_curve", - "reason": ( - "cross_view_roll_curve_difference_too_large:" - "angle_rad:1.297957deg" - ), - } - ], - }, - ] - - assert _unresolved_fit_failure_tasks(profile, rows) == set() - - def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None: config = _config(tmp_path) session = config.session_root / "20260819_150000" @@ -1016,7 +320,8 @@ def test_thumb_scope_resolves_passed_base_and_launches_partial_mode( }, }, ) - atomic_session_pointer(config.session_root, "latest_passed", session) + # Construct an archived fixture, not a new certified publication. + (config.session_root / "latest_passed").symlink_to(session, target_is_directory=True) resolved = _resolve_partial_base_session( config, config.session_root / "latest_passed" @@ -1033,24 +338,6 @@ def test_thumb_scope_resolves_passed_base_and_launches_partial_mode( assert f"resume_raw_samples_path:={session / 'raw_samples.jsonl'}" in command -def test_fingers_scope_reuses_thumb_and_recollects_twelve_tasks( - tmp_path: Path, -) -> None: - config = _config(tmp_path) - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - tasks = recalibration_task_keys(profile, "fingers") - command = _launch_command( - config, - config.session_root / "20260831_130000", - resume_from=config.session_root / "20260831_120000", - recalibration_scope="fingers", - ) - - assert len(tasks) == 12 - assert all("thumb_" not in name for name in tasks) - assert "recalibration_scope:=fingers" in command - - def test_standalone_thumb_launch_has_no_resume_dependency(tmp_path: Path) -> None: config = _config(tmp_path) command = _launch_command( @@ -1063,90 +350,6 @@ def test_standalone_thumb_launch_has_no_resume_dependency(tmp_path: Path) -> Non assert all("resume_raw_samples_path" not in value for value in command) -def test_partial_quality_gates_only_fresh_scope_joints() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - thumb = recalibration_quality_joints(profile, "thumb") - fingers = recalibration_quality_joints(profile, "fingers") - - assert thumb - assert fingers - assert all(name.startswith("thumb_") for name in thumb) - assert all(not name.startswith("thumb_") for name in fingers) - assert set(thumb) | set(fingers) == set(profile.measured_joints) - assert set(thumb).isdisjoint(fingers) - - -def test_standalone_thumb_payload_expands_non_thumb_to_cad_zero() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - curve = tuple(0.001 * (127 - value) for value in range(256)) - fits = { - name: JointCurveFit( - angle_rad=curve, - decreasing_rad=curve, - increasing_rad=curve, - circle={}, - maximum_monotonic_correction_rad=0.0, - maximum_hysteresis_rad=0.0, - quality={}, - ) - for name in recalibration_quality_joints(profile, "thumb") - } - thumb_offsets = { - "thumb_cmc_roll": 0.01, - "thumb_cmc_yaw": -0.02, - "thumb_cmc_pitch": 0.03, - "thumb_mcp": -0.01, - } - payload = build_standalone_thumb_payload( - profile=profile, - serial_number="G20_RIGHT_001", - measured_fits=fits, - thumb_offsets_rad=thumb_offsets, - validation_errors_rad=(0.001, -0.002), - baseline_command_u8=profile.baseline_command, - source_urdf_sha256="a" * 64, - camera_extrinsics_sha256="b" * 64, - corrected_urdf_sha256="c" * 64, - ) - - expanded = standalone_thumb_offsets(payload) - assert set(expanded) == set(ACTIVE_ZERO_JOINTS) - assert all(expanded[name] == value for name, value in thumb_offsets.items()) - assert all( - value == 0.0 - for name, value in expanded.items() - if not name.startswith("thumb_") - ) - - -def test_partial_scopes_freeze_exactly_the_non_target_zeros() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - offsets = { - name: 0.001 * (index + 1) - for index, name in enumerate(sorted(ACTIVE_ZERO_JOINTS)) - } - payload = _payload(zero_offsets=offsets) - - thumb_frozen = _partial_scope_frozen_zero_offsets( - profile, "thumb", payload - ) - finger_frozen = _partial_scope_frozen_zero_offsets( - profile, "fingers", payload - ) - - assert len(thumb_frozen) == 12 - assert all(not name.startswith("thumb_") for name in thumb_frozen) - assert len(finger_frozen) == 4 - assert all(name.startswith("thumb_") for name in finger_frozen) - expected = { - name: float( - payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"] - ) - for name in ACTIVE_ZERO_JOINTS - } - assert {**thumb_frozen, **finger_frozen} == expected - - def test_thumb_scope_rejects_base_with_different_geometry(tmp_path: Path) -> None: config = _config(tmp_path) session = config.session_root / "20260830_120000" @@ -1235,298 +438,6 @@ def test_automatic_resume_skips_newer_attempt_without_start_checkpoint( assert _automatic_resume_candidate(config) == usable -def test_node_restores_complete_prefix_into_new_self_contained_raw( - tmp_path: Path, monkeypatch -) -> None: - # The synthetic rows carry no pose trajectories, so the import-time hard - # gate revalidation (covered separately in test_three_camera_retry) is - # bypassed here to test the restore plumbing itself. - monkeypatch.setattr( - G20ThreeCameraCalibrationNode, - "_revalidate_imported_tasks", - lambda self, completed, **_kwargs: (tuple(completed), []), - ) - config = _config(tmp_path) - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - source_raw = tmp_path / "old" / "raw_samples.jsonl" - source_raw.parent.mkdir() - rows = [ - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "view_tags": { - view: dict(tags) for view, tags in profile.view_tags.items() - }, - "baseline_command_u8": baseline, - "source_urdf_sha256": config.source_urdf_sha256, - "capabilities": sorted(profile.capabilities), - "palm_axis_observers": _palm_axis_observer_schema(profile), - }, - *_fixed_base_reference_rows(), - *_complete_resume_task_rows(profile, profile.sweep_specs[0]), - ] - for row in rows: - if row.get("task_name") == profile.sweep_specs[0].key: - row["attempt"] = 2 - source_raw.write_text( - "".join(json.dumps(row) + "\n" for row in rows) - ) - current_raw = tmp_path / "new" / "raw_samples.jsonl" - current_raw.parent.mkdir() - current_raw.touch() - sweep_items = [] - for spec in profile.sweep_specs: - sweep_items.extend( - SweepItem(spec, -1, direction, precheck=True) - for direction in ("decreasing", "increasing") - ) - sweep_items.extend( - SweepItem(spec, cycle, direction) - for cycle in range(4) - for direction in ("decreasing", "increasing") - ) - node = SimpleNamespace( - resume_raw_samples_path=source_raw, - raw_path=current_raw, - hand_type="right", - profile=profile, - baseline_command=tuple(baseline), - source_urdf_path=config.source_urdf, - repetitions=4, - minimum_sweep_bins=32, - records_by_joint={name: [] for name in profile.record_joints}, - baseline_records_by_joint={name: [] for name in profile.record_joints}, - command_records_by_joint={name: [] for name in profile.record_joints}, - palm_axis_records_by_source={ - observer.source_name: [] - for observer in profile.palm_axis_observers - }, - sweep_index=0, - sweep_items=sweep_items, - resumed_task_keys=(), - resume_source_session="", - views=_resume_reference_views(), - fixed_base_maximum_corner_drift_px=2.0, - recalibration_scope="full", - ) - - count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( - node - ) - - assert count == 1 - assert node.sweep_index == 10 - assert node.resumed_task_keys == (profile.sweep_specs[0].key,) - assert node.sweep_attempts[profile.sweep_specs[0].key] == 2 - restored = [ - json.loads(line) for line in current_raw.read_text().splitlines() - ] - assert restored[0]["kind"] == "resume_checkpoint_import" - assert restored[0]["imported_attempt_floor_by_task"] == { - profile.sweep_specs[0].key: 2 - } - assert any(row["kind"] == "sample" for row in restored) - - -def test_full_resume_discards_all_old_tasks_after_start_position_change( - tmp_path: Path, monkeypatch -) -> None: - monkeypatch.setattr( - G20ThreeCameraCalibrationNode, - "_revalidate_imported_tasks", - lambda self, completed, **_kwargs: (tuple(completed), []), - ) - config = _config(tmp_path) - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - source_raw = tmp_path / "old_position" / "raw_samples.jsonl" - source_raw.parent.mkdir() - rows = [ - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "model": "G20", - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "view_tags": { - view: dict(tags) for view, tags in profile.view_tags.items() - }, - "baseline_command_u8": baseline, - "source_urdf_sha256": config.source_urdf_sha256, - "capabilities": sorted(profile.capabilities), - "palm_axis_observers": _palm_axis_observer_schema(profile), - }, - *_fixed_base_reference_rows(), - *_complete_resume_task_rows(profile, profile.sweep_specs[0]), - ] - source_raw.write_text( - "".join(json.dumps(row) + "\n" for row in rows), - encoding="utf-8", - ) - current_raw = tmp_path / "new_position" / "raw_samples.jsonl" - current_raw.parent.mkdir() - current_raw.touch() - moved_corners = { - view: tuple((x + 18.0, y - 7.0) for x, y in corners) - for view, corners in _FIXED_BASE_CORNERS_BY_VIEW.items() - } - sweep_items = [ - SweepItem(spec, cycle, direction) - for spec in profile.sweep_specs - for cycle in range(4) - for direction in ("decreasing", "increasing") - ] - node = SimpleNamespace( - resume_raw_samples_path=source_raw, - raw_path=current_raw, - model="G20", - hand_type="right", - profile=profile, - baseline_command=tuple(baseline), - source_urdf_path=config.source_urdf, - repetitions=4, - minimum_sweep_bins=32, - records_by_joint={name: [] for name in profile.record_joints}, - baseline_records_by_joint={name: [] for name in profile.record_joints}, - command_records_by_joint={name: [] for name in profile.record_joints}, - palm_axis_records_by_source={ - observer.source_name: [] - for observer in profile.palm_axis_observers - }, - sweep_index=0, - sweep_items=sweep_items, - resumed_task_keys=(), - resume_source_session="", - recalibration_scope="full", - recalibration_task_keys=(), - views=_resume_reference_views(moved_corners), - fixed_base_maximum_corner_drift_px=2.0, - ) - - count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( - node - ) - - assert count == 0 - assert node.resumed_task_keys == () - assert node.sweep_index == 0 - assert node.resume_position_policy == ( - "discard_all_tasks_for_new_start_pose" - ) - assert node.resume_position_changed_views == ("front", "side", "top") - assert all(not values for values in node.records_by_joint.values()) - imported = json.loads(current_raw.read_text(encoding="utf-8").splitlines()[0]) - assert imported["start_position_policy"] == ( - "discard_all_tasks_for_new_start_pose" - ) - assert imported["start_position_invalidated_task_keys"] == [ - spec.key for spec in profile.sweep_specs - ] - assert imported["imported_record_count"] == 0 - - -def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks( - tmp_path: Path, monkeypatch -) -> None: - monkeypatch.setattr( - G20ThreeCameraCalibrationNode, - "_revalidate_imported_tasks", - lambda self, completed, **_kwargs: (tuple(completed), []), - ) - config = _config(tmp_path) - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - thumb_tasks = recalibration_task_keys(profile, "thumb") - thumb_spec = next(spec for spec in profile.sweep_specs if spec.key in thumb_tasks) - finger_spec = next( - spec - for spec in profile.sweep_specs - if spec.key == "pinky_pitch_side" - ) - source_raw = tmp_path / "passed" / "raw_samples.jsonl" - source_raw.parent.mkdir() - rows = [ - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "model": "G20", - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "view_tags": { - view: dict(tags) for view, tags in profile.view_tags.items() - }, - "baseline_command_u8": baseline, - "source_urdf_sha256": config.source_urdf_sha256, - "capabilities": sorted(profile.capabilities), - "palm_axis_observers": _palm_axis_observer_schema(profile), - }, - *_fixed_base_reference_rows(), - *_complete_resume_task_rows(profile, thumb_spec), - *_complete_resume_task_rows(profile, finger_spec), - ] - source_raw.write_text( - "".join(json.dumps(row) + "\n" for row in rows), - encoding="utf-8", - ) - current_raw = tmp_path / "new" / "raw_samples.jsonl" - current_raw.parent.mkdir() - current_raw.touch() - sweep_items = [ - SweepItem(spec, cycle, direction) - for spec in profile.sweep_specs - for cycle in range(4) - for direction in ("decreasing", "increasing") - ] - node = SimpleNamespace( - resume_raw_samples_path=source_raw, - raw_path=current_raw, - model="G20", - hand_type="right", - profile=profile, - baseline_command=tuple(baseline), - source_urdf_path=config.source_urdf, - repetitions=4, - minimum_sweep_bins=32, - records_by_joint={name: [] for name in profile.record_joints}, - baseline_records_by_joint={name: [] for name in profile.record_joints}, - command_records_by_joint={name: [] for name in profile.record_joints}, - palm_axis_records_by_source={ - observer.source_name: [] for observer in profile.palm_axis_observers - }, - sweep_index=0, - sweep_items=sweep_items, - resumed_task_keys=(), - resume_source_session="", - recalibration_scope="thumb", - recalibration_task_keys=thumb_tasks, - views=_resume_reference_views(), - fixed_base_maximum_corner_drift_px=2.0, - ) - - count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint(node) - - assert count == 1 - assert node.resumed_task_keys == (finger_spec.key,) - assert node.sweep_index == 0 - assert all( - not node.records_by_joint[name] - for name in thumb_spec.joints - ) - assert all(node.records_by_joint[name] for name in finger_spec.joints) - imported = [ - json.loads(line) - for line in current_raw.read_text(encoding="utf-8").splitlines() - ][0] - assert imported["recalibration_scope"] == "thumb" - assert imported["scope_invalidated_task_keys"] == list(thumb_tasks) - - def test_runtime_json_is_minimal_v4_with_21_independent_midpoint_curves() -> None: payload = _payload() assert payload["schema_version"] == 4 @@ -1542,453 +453,24 @@ def test_runtime_json_is_minimal_v4_with_21_independent_midpoint_curves() -> Non ) -def test_publication_protects_passive_xml_and_requires_two_matching_sessions( - tmp_path: Path, -) -> None: - config = _config(tmp_path) - first = _make_passed_session(config, "20260819_100000") - first_summary, first_ready = finalize_session_artifacts( - config, first, node_status=_passed_node_status() - ) - assert first_ready is False - assert first_summary["result"] == "PASS_AWAITING_SECOND_SESSION" - assert not (config.session_root / "latest_passed").exists() - - second = _make_passed_session(config, "20260819_110000") - second_summary, second_ready = finalize_session_artifacts( - config, second, node_status=_passed_node_status() - ) - assert second_ready is True - assert second_summary["formal_release"]["comparison_session"] == first.name - assert (config.session_root / "latest_passed").resolve() == second - paths = list(second.glob("*.urdf")) - assert len(paths) == 1 - changed = verify_corrected_urdf(config.source_urdf, paths[0]) - assert set(changed) == ACTIVE_ZERO_JOINTS - resources = verify_urdf_mesh_resources(paths[0]) - assert len(resources) == 22 - assert set(second_summary["hashes"]["mesh_resources_sha256"]) == set( - resources - ) - - commands = json.loads((second / "mujoco_validation_commands.json").read_text()) - assert commands["topic"] == "/g20/cb_right_hand_control_cmd" - assert len(commands["poses"]) == 8 -def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> None: - config = _config(tmp_path, passes=1) - base_session = _make_passed_session(config, "20260828_205005") - session = _make_passed_session(config, "20260830_140000") - status = _passed_node_status() - tasks = list( - recalibration_task_keys( - get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), - "thumb", - ) - ) - status["resume"] = { - "used": True, - "source_session": str(base_session), - "recalibration_scope": "thumb", - "recalibration_task_keys": tasks, - } - - summary, ready = finalize_session_artifacts( - config, session, node_status=status - ) - - assert ready is True - assert summary["calibration_scope"] == "thumb" - assert summary["inherited_base_session"] == str(base_session) - assert summary["freshly_calibrated_task_keys"] == tasks - assert len(summary["preserved_certified_zero_joints"]) == 12 - assert all( - not name.startswith("thumb_") - for name in summary["preserved_certified_zero_joints"] - ) -def test_publication_accepts_standalone_thumb_without_base_session( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - thumb_offsets = { - "thumb_cmc_roll": 0.01, - "thumb_cmc_yaw": -0.02, - "thumb_cmc_pitch": 0.03, - "thumb_mcp": -0.01, - } - all_offsets = {name: 0.0 for name in ACTIVE_ZERO_JOINTS} - all_offsets.update(thumb_offsets) - session = _make_passed_session( - config, "20260831_150000", zero_offsets=all_offsets - ) - full_payload = _payload( - config.serial_number, zero_offsets=all_offsets - ) - standalone_payload = { - "schema_version": 1, - "artifact_type": "g20_right_standalone_thumb_calibration", - "model": "G20", - "side": "right", - "serial_number": config.serial_number, - "angle_unit": "rad", - "command_range": [0, 255], - "baseline_command_u8": full_payload["baseline_command_u8"], - "non_thumb_zero_policy": "source_cad_unchanged", - "joints": { - name: full_payload["joints"][name] - for name in (*thumb_offsets, "thumb_ip") - }, - "quality": { - "passed": True, - "validation_mae_rad": 0.001, - "validation_p95_rad": 0.002, - }, - "hashes": {}, - } - atomic_write_json( - session / f"g20_right_{config.serial_number}_calibration.json", - standalone_payload, - ) - status = _passed_node_status() - status["combination_validation"] = {"enabled": False} - status["resume"] = { - "used": False, - "source_session": "", - "recalibration_scope": "thumb", - "recalibration_task_keys": list( - recalibration_task_keys( - get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), - "thumb", - ) - ), - } - - summary, ready = finalize_session_artifacts( - config, session, node_status=status - ) - - assert ready is True - assert summary["inherited_base_session"] is None - assert summary["non_thumb_zero_policy"] == "source_cad_unchanged" - assert summary["static_zero_calibrated_joints"] == sorted(thumb_offsets) - assert not (config.session_root / "latest_passed").exists() - assert (config.session_root / "latest_thumb_passed").resolve() == session - combined_offsets = dict(all_offsets) - combined_offsets["index_mcp_pitch"] = 0.02 - verify_partial_scope_preserves_certified_zeros( - scope="fingers", - source_session=session, - serial_number=config.serial_number, - current_offsets=combined_offsets, - ) -def test_publication_resolves_node_short_base_session_as_sibling( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - base_session = _make_passed_session(config, "20260828_205005") - session = _make_passed_session(config, "20260831_121828") - status = _passed_node_status() - tasks = list( - recalibration_task_keys( - get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), - "thumb", - ) - ) - status["resume"] = { - "used": True, - # The calibration node intentionally publishes the durable sibling - # session identifier, not a machine-specific absolute path. - "source_session": base_session.name, - "recalibration_scope": "thumb", - "recalibration_task_keys": tasks, - } - - summary, ready = finalize_session_artifacts( - config, session, node_status=status - ) - - assert ready is True - assert summary["inherited_base_session"] == base_session.name -def test_partial_publication_rejects_any_non_target_zero_change( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - base_offsets = { - name: 0.001 * (index + 1) - for index, name in enumerate(sorted(ACTIVE_ZERO_JOINTS)) - } - base_session = _make_passed_session( - config, "20260831_100000", zero_offsets=base_offsets - ) - - thumb_result = dict(base_offsets) - thumb_result["thumb_cmc_yaw"] += 0.01 - verify_partial_scope_preserves_certified_zeros( - scope="thumb", - source_session=base_session, - serial_number=config.serial_number, - current_offsets=thumb_result, - ) - changed_finger = dict(thumb_result) - changed_finger["index_mcp_pitch"] += 0.001 - with pytest.raises(ValueError, match="non-target zeros"): - verify_partial_scope_preserves_certified_zeros( - scope="thumb", - source_session=base_session, - serial_number=config.serial_number, - current_offsets=changed_finger, - ) - - finger_result = dict(base_offsets) - finger_result["index_mcp_pitch"] += 0.01 - verify_partial_scope_preserves_certified_zeros( - scope="fingers", - source_session=base_session, - serial_number=config.serial_number, - current_offsets=finger_result, - ) - changed_thumb = dict(finger_result) - changed_thumb["thumb_mcp"] += 0.001 - with pytest.raises(ValueError, match="non-target zeros"): - verify_partial_scope_preserves_certified_zeros( - scope="fingers", - source_session=base_session, - serial_number=config.serial_number, - current_offsets=changed_thumb, - ) -def test_publication_records_fingers_scope_with_certified_thumb( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - base_session = _make_passed_session(config, "20260831_110000") - session = _make_passed_session(config, "20260831_120000") - tasks = list( - recalibration_task_keys( - get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), - "fingers", - ) - ) - status = _passed_node_status() - status["resume"] = { - "used": True, - "source_session": str(base_session), - "recalibration_scope": "fingers", - "recalibration_task_keys": tasks, - } - - summary, ready = finalize_session_artifacts( - config, session, node_status=status - ) - - assert ready is True - assert summary["calibration_scope"] == "fingers" - assert summary["inherited_base_session"] == str(base_session) - assert summary["freshly_calibrated_task_keys"] == tasks - assert summary["preserved_certified_zero_joints"] == sorted( - name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_") - ) -def test_publication_numerically_binds_json_offsets_and_urdf_limits( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - session = _make_passed_session(config, "20260819_111000") - urdf = next(session.glob("*.urdf")) - offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS} - - verify_corrected_urdf( - config.source_urdf, urdf, expected_offsets_rad=offsets - ) - wrong_offsets = dict(offsets) - wrong_offsets["thumb_mcp"] = 0.02 - with pytest.raises(ValueError, match="published zero offsets"): - verify_corrected_urdf( - config.source_urdf, - urdf, - expected_offsets_rad=wrong_offsets, - ) - - payload = _payload(config.serial_number) - validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) - payload["joints"]["thumb_mcp"]["angle_rad"][0] = 2.0 - with pytest.raises(ValueError, match="exceeds runtime URDF limit"): - validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) -def test_publication_uses_corrected_coordinates_for_negative_endpoint_zero( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS} - endpoint_offset = -0.00801549 - offsets["index_mcp_pitch"] = endpoint_offset - session = _make_passed_session( - config, - "20260826_102518", - zero_offsets=offsets, - ) - paths = session_artifact_paths(session, config.serial_number) - payload = json.loads(paths["json"].read_text(encoding="utf-8")) - curve_maximum = 1.22801310 - payload["joints"]["index_mcp_pitch"]["angle_rad"][0] = curve_maximum - atomic_write_json(paths["json"], payload) - - summary, release_ready = finalize_session_artifacts( - config, - session, - node_status=_passed_node_status(), - ) - - def upper_limit(path: Path) -> float: - joint = next( - node - for node in ET.parse(path).getroot().findall("joint") - if node.get("name") == "index_mcp_pitch" - ) - return float(joint.find("limit").get("upper")) - - source_upper = upper_limit(config.source_urdf) - corrected_upper = upper_limit(paths["urdf"]) - assert release_ready is True - assert summary["result"] == "PASS" - assert corrected_upper == pytest.approx(source_upper - endpoint_offset) - assert curve_maximum <= corrected_upper - assert curve_maximum + endpoint_offset <= source_upper -def test_publication_propagates_schema_v4_rounding_to_mimic_offset( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - offsets = {name: 0.01 for name in ACTIVE_ZERO_JOINTS} - full_precision_offset = -0.06200179363832 - offsets["middle_pip"] = full_precision_offset - session = _make_passed_session( - config, - "20260827_152432", - zero_offsets=offsets, - ) - paths = session_artifact_paths(session, config.serial_number) - payload = json.loads(paths["json"].read_text(encoding="utf-8")) - assert payload["joints"]["middle_pip"]["zero_angles"][ - "urdf_zero_offset_rad" - ] == -0.06200179 - - summary, release_ready = finalize_session_artifacts( - config, - session, - node_status=_passed_node_status(), - ) - - corrected = ET.parse(paths["urdf"]).getroot() - middle_dip = next( - node - for node in corrected.findall("joint") - if node.get("name") == "middle_dip" - ) - assert float(middle_dip.find("mimic").get("offset")) == pytest.approx( - 0.89 * full_precision_offset, - abs=1.0e-14, - ) - assert release_ready is True - assert summary["result"] == "PASS" -def test_publication_rejects_non_joint_urdf_changes(tmp_path: Path) -> None: - config = _config(tmp_path, passes=1) - session = _make_passed_session(config, "20260819_112000") - urdf = next(session.glob("*.urdf")) - text = urdf.read_text(encoding="utf-8") - urdf.write_text( - text.replace("hand_base_link", "tampered_base_link", 1), - encoding="utf-8", - ) - - with pytest.raises(ValueError, match="outside active origin.rpy"): - verify_corrected_urdf(config.source_urdf, urdf) - - -def test_publication_requires_every_combination_target(tmp_path: Path) -> None: - config = _config(tmp_path, passes=1) - session = _make_passed_session(config, "20260819_113000") - status = _passed_node_status() - missing = G20_COMBINATION_REQUIRED_TARGET_KEYS[0] - status["combination_validation"]["validation_counts"][missing] = 0 - - with pytest.raises(ValueError, match="coverage is incomplete"): - finalize_session_artifacts(config, session, node_status=status) - - -def test_publication_accepts_formal_holdout_when_combination_diagnostic_disabled( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - session = _make_passed_session(config, "20260819_113100") - status = _passed_node_status() - status["combination_validation"] = { - "enabled": False, - "completed": False, - "completed_poses": 0, - "total_poses": 0, - } - - _summary, release_ready = finalize_session_artifacts( - config, session, node_status=status - ) - - assert release_ready is True - - -def test_publication_accepts_half_lsb_schema_zero_quantisation( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - session = _make_passed_session(config, "20260819_113200") - json_path = session / f"g20_right_{config.serial_number}_calibration.json" - payload = json.loads(json_path.read_text(encoding="utf-8")) - payload["joints"]["thumb_cmc_pitch"]["zero_angles"][ - "urdf_zero_offset_rad" - ] = 0.01 - 4.9e-9 - atomic_write_json(json_path, payload) - - _summary, release_ready = finalize_session_artifacts( - config, session, node_status=_passed_node_status() - ) - - assert release_ready is True - - -def test_publication_keeps_cmc_runtime_limit_independent_of_certified_zero( - tmp_path: Path, -) -> None: - config = _config(tmp_path, passes=1) - session = _make_passed_session(config, "20260819_113300") - json_path = session / f"g20_right_{config.serial_number}_calibration.json" - payload = json.loads(json_path.read_text(encoding="utf-8")) - payload["joints"]["thumb_cmc_yaw"]["angle_rad"][0] = 2.0 - atomic_write_json(json_path, payload) - - summary, release_ready = finalize_session_artifacts( - config, session, node_status=_passed_node_status() - ) - published = json.loads(json_path.read_text(encoding="utf-8")) - - assert release_ready is True - # CMC has a certified origin, not an assertion that its electrical - # endpoint equals the source-CAD upper coordinate. Its runtime coordinate - # limit therefore remains the protected source value. - assert published["joints"]["thumb_cmc_yaw"]["angle_rad"][0] == 1.57 - assert summary["runtime_limit_clipped_bins"]["thumb_cmc_yaw"] == 1 def test_latest_attempt_pointer_is_atomic_session_binding(tmp_path: Path) -> None: @@ -2153,27 +635,6 @@ def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None: assert _status_timeout_seconds({"state": "RETURN_BASELINE"}) == 90.0 -def test_fitting_announces_state_before_synchronous_model_work() -> None: - spec = SimpleNamespace() - published = [] - node = SimpleNamespace( - profile=SimpleNamespace(sweep_specs=(spec,)), - last_status_publish=0.0, - _publish_status=lambda now: published.append( - (node.state, node.reason, now) - ), - _provisional_fit_failures=lambda _spec: [{"metric": "test_failure"}], - _pause_for_provisional_fit_failure=lambda *_args, **_kwargs: None, - ) - - G20ThreeCameraCalibrationNode._fit_all_curves(node) - - assert len(published) == 1 - assert published[0][0] == "FITTING" - assert published[0][1] == "fitting_3d_axes_and_urdf_zero_offsets" - assert node.last_status_publish == published[0][2] - - def test_stopped_status_heartbeat_has_stable_communication_code() -> None: code, problem, suggestion = classify_error( "MOTION-COMM-303:calibration status stopped", {} @@ -2476,105 +937,6 @@ def test_multiview_progress_marks_occluded_front_base_as_locked() -> None: assert "Tag可见不等于三维位姿有效" in text -def test_device_preflight_defers_tag_gate_until_after_baseline_recovery() -> None: - now = 100.0 - state_times = deque( - (now - 0.98 + index * 0.02 for index in range(50)), maxlen=300 - ) - views = { - name: SimpleNamespace( - camera_info_valid=True, - extrinsics_valid=True, - last_message_at=now, - valid_flags=deque([False] * 30, maxlen=30), - detection_times=deque(maxlen=30), - ) - for name in ("front", "side", "top") - } - node = SimpleNamespace( - extrinsics=object(), - latest_state_u8=tuple([127.0] * 20), - last_state_at=now, - state_receive_times=state_times, - minimum_feedback_hz=25.0, - views=views, - ) - - assert G20ThreeCameraCalibrationNode._all_devices_ready(node, now) - - reset_views: list[str] = [] - node._reset_view_trackers = lambda runtime: reset_views.append(runtime.name) - for name, runtime in views.items(): - runtime.name = name - runtime.detection_times.extend([now - 0.1, now]) - G20ThreeCameraCalibrationNode._finish_startup_baseline_recovery(node) - - assert node.state == "PREFLIGHT" - assert node.startup_baseline_recovered is True - assert node.reason == "waiting_for_baseline_tags_after_recovery" - assert reset_views == ["front", "side", "top"] - assert all(not runtime.valid_flags for runtime in views.values()) - assert all(not runtime.detection_times for runtime in views.values()) - - -def test_startup_state_machine_moves_before_applying_tag_gate() -> None: - node = SimpleNamespace( - state="PREFLIGHT", - reason="waiting_for_devices_and_sdk", - started=False, - startup_baseline_recovered=False, - _all_devices_ready=lambda now: True, - _all_preflight_ready=lambda now: (_ for _ in ()).throw( - AssertionError("Tag gate ran before baseline recovery") - ), - ) - - G20ThreeCameraCalibrationNode._advance(node, 10.0) - - assert node.state == "WAIT_START" - assert node.reason == "call_start_for_baseline_recovery" - - node.state = "PREFLIGHT" - node.started = True - node.startup_baseline_recovered = True - node._all_preflight_ready = lambda now: True - started: list[bool] = [] - node._start_next_sweep = lambda: started.append(True) - - G20ThreeCameraCalibrationNode._advance(node, 11.0) - - assert started == [True] - - -def test_resume_import_waits_until_new_fixed_base_reference_is_locked( - tmp_path: Path, -) -> None: - source = tmp_path / "raw_samples.jsonl" - source.write_text("{}\n", encoding="utf-8") - calls: list[str] = [] - node = SimpleNamespace( - state="PREFLIGHT", - reason="waiting_for_baseline_tags_after_recovery", - started=True, - startup_baseline_recovered=True, - resume_checkpoint_pending=True, - resume_raw_samples_path=source, - _all_preflight_ready=lambda now: True, - _lock_fixed_base_references=lambda: calls.append("lock") or True, - _publish_status=lambda now: calls.append("status"), - _restore_durable_task_checkpoint=( - lambda: calls.append("restore") or 2 - ), - _start_next_sweep=lambda: calls.append("start"), - ) - - G20ThreeCameraCalibrationNode._advance(node, 11.0) - - assert calls == ["lock", "status", "restore", "start"] - assert node.resume_checkpoint_pending is False - assert node.state == "IMPORTING_BASE" - - def test_device_preflight_progress_says_tags_are_checked_after_recovery() -> None: status = { "state": "PREFLIGHT", @@ -2604,22 +966,6 @@ def test_device_preflight_progress_says_tags_are_checked_after_recovery() -> Non assert "本阶段必需" not in text -def test_validation_command_file_has_eight_safe_bounded_poses() -> None: - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - payload = build_mujoco_validation_commands(baseline) - assert len(payload["poses"]) == 8 - assert all( - len(pose["command_u8"]) == 20 - and all(0 <= value <= 255 for value in pose["command_u8"]) - for pose in payload["poses"] - ) - online = _combination_validation_items(baseline) - assert [list(item.command_u8) for item in online] == [ - pose["command_u8"] for pose in payload["poses"] - ] - - def test_combination_kinematics_can_use_independent_passive_curve() -> None: config = load_product_config(PRODUCT, workspace=REPO, check_can=False) model = UrdfKinematicModel(config.source_urdf) @@ -2637,276 +983,3 @@ def test_combination_kinematics_can_use_independent_passive_curve() -> None: independent_mimic_angles=False, ) assert not np.allclose(independent, mimic) - - -def test_combination_angles_follow_baseline_to_target_direction() -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - decreasing = tuple(float(value) for value in range(256)) - increasing = tuple(float(value + 1000) for value in range(256)) - average = tuple(float(value + 500) for value in range(256)) - fits = { - name: JointCurveFit( - angle_rad=average, - decreasing_rad=decreasing, - increasing_rad=increasing, - circle={}, - maximum_monotonic_correction_rad=0.0, - maximum_hysteresis_rad=0.0, - quality={}, - ) - for name in profile.joint_specs - } - items = _combination_validation_items(baseline) - directions = _combination_motor_directions(items, 1, baseline) - - angles = _combination_joint_angles( - profile, fits, items[1].command_u8, directions - ) - - assert angles["thumb_cmc_pitch"] == 160.0 - assert angles["index_mcp_roll"] == 127.0 - assert angles["middle_mcp_roll"] == 127.0 - - -def test_combination_directions_include_previous_return_to_baseline() -> None: - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - items = _combination_validation_items(baseline) - - thumb_pose = _combination_motor_directions(items, 1, baseline) - index_pose = _combination_motor_directions(items, 2, baseline) - - assert thumb_pose[0] == DIRECTION_DECREASING - assert thumb_pose[1] == DIRECTION_DECREASING - assert index_pose[0] == DIRECTION_INCREASING - assert index_pose[5] == DIRECTION_INCREASING - assert index_pose[10] == DIRECTION_INCREASING - assert index_pose[15] == DIRECTION_INCREASING - assert index_pose[1] == DIRECTION_DECREASING - assert index_pose[16] == DIRECTION_DECREASING - - -def test_combination_model_link_is_transformed_into_observer_base() -> None: - model_base_common = np.eye(4) - model_base_common[:3, :3] = Rotation.from_euler( - "xyz", [0.4, -0.2, 0.7] - ).as_matrix() - model_base_common[:3, 3] = [0.3, -0.1, 0.8] - observer_mount = np.eye(4) - observer_mount[:3, :3] = Rotation.from_euler( - "xyz", [-0.3, 0.1, 0.2] - ).as_matrix() - observer_mount[:3, 3] = [0.05, 0.02, -0.01] - observer_base_common = model_base_common @ observer_mount - model_link = np.eye(4) - model_link[:3, :3] = Rotation.from_rotvec([0.0, 0.5, 0.0]).as_matrix() - model_link[:3, 3] = [0.08, 0.03, 0.01] - tag_mount = np.eye(4) - tag_mount[:3, 3] = [0.0, 0.0, 0.02] - observed = ( - np.linalg.inv(observer_base_common) - @ model_base_common - @ model_link - @ tag_mount - ) - - resolved = _model_link_in_observer_base( - observer_base_common, model_base_common, model_link - ) - - assert np.allclose(resolved @ tag_mount, observed) - assert not np.allclose(model_link @ tag_mount, observed) - - -def test_node_drops_imported_task_failing_hard_gates( - tmp_path: Path, -) -> None: - config = _config(tmp_path) - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - source_raw = tmp_path / "old_gates" / "raw_samples.jsonl" - source_raw.parent.mkdir() - rows = [ - { - "kind": "session_start", - "acquisition_policy_version": ACQUISITION_POLICY_VERSION, - "hand_type": "right", - "tag_layout": G20_RIGHT_19_LAYOUT, - "view_tags": { - view: dict(tags) for view, tags in profile.view_tags.items() - }, - "baseline_command_u8": baseline, - "source_urdf_sha256": config.source_urdf_sha256, - "capabilities": sorted(profile.capabilities), - "palm_axis_observers": _palm_axis_observer_schema(profile), - }, - *_fixed_base_reference_rows(), - *_complete_resume_task_rows(profile, profile.sweep_specs[0]), - ] - source_raw.write_text( - "".join(json.dumps(row) + "\n" for row in rows) - ) - current_raw = tmp_path / "new_gates" / "raw_samples.jsonl" - current_raw.parent.mkdir() - current_raw.touch() - sweep_items = [] - for spec in profile.sweep_specs: - sweep_items.extend( - SweepItem(spec, cycle, direction) - for cycle in range(4) - for direction in ("decreasing", "increasing") - ) - node = SimpleNamespace( - resume_raw_samples_path=source_raw, - raw_path=current_raw, - hand_type="right", - profile=profile, - baseline_command=tuple(baseline), - source_urdf_path=config.source_urdf, - repetitions=4, - minimum_sweep_bins=32, - records_by_joint={name: [] for name in profile.record_joints}, - baseline_records_by_joint={ - name: [] for name in profile.record_joints - }, - command_records_by_joint={name: [] for name in profile.record_joints}, - palm_axis_records_by_source={ - observer.source_name: [] - for observer in profile.palm_axis_observers - }, - sweep_index=0, - sweep_items=sweep_items, - resumed_task_keys=(), - resume_source_session="", - views=_resume_reference_views(), - fixed_base_maximum_corner_drift_px=2.0, - recalibration_scope="full", - zero_profile=get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT), - trajectory_maximum_plane_rms_m=0.004, - trajectory_maximum_radial_rms_m=0.004, - trajectory_minimum_radius_m=0.003, - trajectory_minimum_arc_rad=math.radians(15.0), - image_trajectory_maximum_radial_rms_px=2.0, - image_trajectory_maximum_radial_p95_px=3.5, - image_trajectory_minimum_radius_px=20.0, - trajectory_maximum_cycle_travel_difference_rad=math.radians(3.0), - passive_maximum_cycle_travel_difference_rad=math.radians(10.0), - maximum_monotonic_correction_rad=math.radians(2.0), - maximum_hysteresis_rad=math.radians(5.0), - baseline_maximum_hysteresis_rad=math.radians(0.5), - passive_maximum_monotonic_correction_rad=math.radians(3.0), - passive_maximum_hysteresis_rad=math.radians(7.5), - axis_maximum_plane_rms_m=0.003, - passive_axis_maximum_plane_rms_m=0.004, - axis_maximum_radial_rms_m=0.003, - axis_maximum_pose_line_rms_m=0.001, - axis_maximum_rotation_circle_difference_rad=math.radians(1.0), - active_maximum_rotation_orthogonal_rms_rad=math.radians(2.5), - passive_maximum_rotation_orthogonal_rms_rad=math.radians(7.5), - zero_maximum_axis_cycle_difference_rad=math.radians(0.75), - maximum_state_image_skew_ns=50_000_000.0, - minimum_detection_rate=0.95, - ) - node._fit_joint_records = lambda name, records, relaxed=False: ( - G20ThreeCameraCalibrationNode._fit_joint_records( - node, name, records, relaxed=relaxed - ) - ) - node._fit_axis_measurement = lambda name, cycle: ( - G20ThreeCameraCalibrationNode._fit_axis_measurement(node, name, cycle) - ) - - count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( - node - ) - - # Trajectory-free synthetic rows cannot pass the hard gates: the task is - # dropped at import time instead of failing the final fit at the very - # end and dragging the session back to re-collect it. - assert count == 0 - assert node.sweep_index == 0 - assert node.resumed_task_keys == () - assert all( - not records for records in node.records_by_joint.values() - ) - restored = [ - json.loads(line) for line in current_raw.read_text().splitlines() - ] - import_record = next( - row - for row in restored - if row["kind"] == "resume_checkpoint_import" - ) - assert import_record["revalidation_dropped_tasks"] - assert ( - import_record["revalidation_dropped_tasks"][0]["task"] - == profile.sweep_specs[0].key - ) - - -def test_sparse_resume_revalidates_against_only_retained_dependencies( - monkeypatch: pytest.MonkeyPatch, -) -> None: - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - pinky_key = "pinky_roll_multiview" - ring_key = "ring_roll_multiview" - pinky_spec = next( - spec for spec in profile.sweep_specs if spec.key == pinky_key - ) - ring_spec = next(spec for spec in profile.sweep_specs if spec.key == ring_key) - records_by_joint = {name: [] for name in profile.record_joints} - baseline_records_by_joint = {name: [] for name in profile.record_joints} - command_records_by_joint = {name: [] for name in profile.record_joints} - for joint_name in (*pinky_spec.joints, *ring_spec.joints): - records_by_joint[joint_name].append({"task_name": "fixture"}) - - node = SimpleNamespace( - state="", - profile=profile, - sweep_items=[ - SimpleNamespace(spec=pinky_spec), - SimpleNamespace(spec=ring_spec), - ], - records_by_joint=records_by_joint, - baseline_records_by_joint=baseline_records_by_joint, - command_records_by_joint=command_records_by_joint, - palm_axis_records_by_source={ - observer.source_name: [] - for observer in profile.palm_axis_observers - }, - ) - - def failures(_node, spec, *, include_view_validity): - assert include_view_validity is False - if spec.key == pinky_key: - return [{"joint": pinky_spec.joints[0], "metric": "fixture_failure"}] - pinky_records_remain = any( - records_by_joint[joint_name] for joint_name in pinky_spec.joints - ) - return [] if pinky_records_remain else [ - {"joint": ring_spec.joints[0], "metric": "missing_reference"} - ] - - monkeypatch.setattr( - G20ThreeCameraCalibrationNode, - "_provisional_fit_failures", - failures, - ) - - accepted, dropped = ( - G20ThreeCameraCalibrationNode._revalidate_imported_tasks( - node, - [pinky_key, ring_key], - allow_sparse=True, - ) - ) - - assert accepted == [] - assert [failure["task"] for failure in dropped] == [pinky_key, ring_key] - assert all( - not records_by_joint[joint_name] - for joint_name in (*pinky_spec.joints, *ring_spec.joints) - ) diff --git a/src/linkerhand_calibration/test/test_golden_regression.py b/src/linkerhand_calibration/test/test_golden_regression.py index f5b76ff..7e1a367 100644 --- a/src/linkerhand_calibration/test/test_golden_regression.py +++ b/src/linkerhand_calibration/test/test_golden_regression.py @@ -2,7 +2,7 @@ import hashlib import json from pathlib import Path -from linkerhand_calibration.models.g20.golden_regression import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.golden_regression import ( validate_golden_sessions, ) diff --git a/src/linkerhand_calibration/test/test_l6_right_profile.py b/src/linkerhand_calibration/test/test_l6_right_profile.py index feb3ecb..9d05339 100644 --- a/src/linkerhand_calibration/test/test_l6_right_profile.py +++ b/src/linkerhand_calibration/test/test_l6_right_profile.py @@ -1,57 +1,24 @@ from __future__ import annotations from pathlib import Path -import json import re -from types import SimpleNamespace -import xml.etree.ElementTree as ET import numpy as np import pytest from scipy.spatial.transform import Rotation -from linkerhand_calibration.calibrated_joint_state_bridge import ( - CalibratedCommandMapper, - default_input_topic, -) from linkerhand_calibration.core import validate_profile -from linkerhand_calibration.models.l6.artifacts import ( - atomic_write_json, - build_l6_left_transferred_runtime_payload, - build_l6_urdf_input_payload, - build_l6_runtime_payload, - load_l6_urdf_input, - validate_l6_runtime_payload, -) -from linkerhand_calibration.models.l6.fitting import fit_l6_session -from linkerhand_calibration.models.l6.motion import cosine_position_trajectory_u8 -from linkerhand_calibration.models.l6.node import ( - L6ThreeCameraCalibrationNode, - MotionStep, -) -from linkerhand_calibration.models.l6.pipeline import ( - accepted_records_by_joint, - canonical_feedback_command_u8, - finalize_l6_session, -) -from linkerhand_calibration.models.l6.profile import ( - ACTIVE_JOINTS, +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.motion import cosine_position_trajectory_u8 +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.profile import ( CALIBRATED_ACTIVE_JOINTS, COMMAND_NAMES, ENDPOINT_ANCHOR_BY_JOINT, KEY, - PASSIVE_JOINTS, - TRANSFERRED_ACTIVE_SOURCE_BY_JOINT, - TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, build_typed_profile, ) -from linkerhand_calibration.models.l6.runner import render_l6_progress_zh -from linkerhand_calibration.models.l6.urdf import ( - write_l6_corrected_urdf, - write_l6_left_from_right_calibration, -) +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.runner import render_l6_progress_zh from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix -from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import UrdfKinematicModel from linkerhand_calibration.product import load_product_config, sha256_file @@ -271,10 +238,10 @@ def test_l6_profile_declares_six_channels_eight_tags_and_partial_scope() -> None assert profile.artifacts.publication_pointer == "latest_partial_passed" assert profile.zero.coupling_model_by_joint == { "rh_thumb_dip": "linear_mimic", - "rh_pinky_dip": "quadratic_runtime", - "rh_index_dip": "quadratic_runtime", - "rh_middle_dip": "quadratic_runtime", - "rh_ring_dip": "quadratic_runtime", + "rh_pinky_dip": "linear_mimic", + "rh_index_dip": "linear_mimic", + "rh_middle_dip": "linear_mimic", + "rh_ring_dip": "linear_mimic", } assert profile.zero.endpoint_anchor_by_joint == ENDPOINT_ANCHOR_BY_JOINT @@ -338,110 +305,6 @@ def test_l6_six_second_cosine_trajectory_is_monotonic_and_smooth() -> None: assert cosine_position_trajectory_u8(255.0, 127.5, 1.5, 6.0)[2] == pytest.approx(3.0) -def test_l6_node_streams_the_six_second_trajectory_without_command_jumps() -> None: - published: list[list[int]] = [] - fake = SimpleNamespace( - step_started_at=0.0, - step_start_state_u8=(255.0,) * 6, - command_trajectory_full_range_seconds=6.0, - step_last_command_u8=None, - step_trajectory_phase=0.0, - step_requested_u8=255.0, - _publish_command=lambda values: published.append(values), - ) - step = MotionStep("sweep", "thumb_pitch_dip_front", 0, 0, 1, 0, "decreasing") - for tick in range(601): - L6ThreeCameraCalibrationNode._advance_step_trajectory( - fake, step, tick / 100.0 - ) - - channel = [command[0] for command in published] - assert channel[0] == 255 - assert channel[-1] == 0 - assert max(abs(right - left) for left, right in zip(channel, channel[1:])) <= 1 - assert all(command[1:] == [255] * 5 for command in published) - - -def _l6_sweep_quality_fake(tmp_path: Path) -> tuple[SimpleNamespace, MotionStep]: - task_key = "pinky_pitch_dip_side" - feedback = np.linspace(255.0, 0.0, 188) - fake = SimpleNamespace( - raw_records=[ - { - "task_name": task_key, - "cycle": 3, - "direction": "decreasing", - "attempt": 3, - "joint": "rh_pinky_mcp_pitch", - "feedback_u8": float(value), - } - for value in feedback - ], - raw_path=tmp_path / "raw_samples.jsonl", - step_required_roles=("side_base", "pinky_pitch", "pinky_dip"), - step_total_frames=211, - step_tag_seen_frames={ - "side_base": 211, - "pinky_pitch": 210, - "pinky_dip": 208, - }, - # Every Tag independently clears 95%, while the fully joined frames - # match the real 20260901_181435 sweep at about 89.1%. - step_tag_quality_frames={ - "side_base": 209, - "pinky_pitch": 205, - "pinky_dip": 202, - }, - step_all_tags_quality_frames=190, - step_pnp_valid_frames=189, - step_state_sync_frames=188, - step_valid_frames=188, - step_rejection_counts={}, - minimum_sweep_frames=40, - minimum_state_span_u8=240.0, - minimum_sweep_bins=32, - maximum_bin_gap=16, - minimum_detection_rate=0.95, - minimum_joint_frame_rate=0.85, - minimum_feedback_hz=25.0, - profile=build_typed_profile(), - _task=lambda _key: SimpleNamespace( - key=task_key, - joints=("rh_pinky_mcp_pitch", "rh_pinky_dip"), - ), - _feedback_hz=lambda: 58.9, - ) - fake._step_observation_metrics = lambda: ( - L6ThreeCameraCalibrationNode._step_observation_metrics(fake) - ) - step = MotionStep( - "sweep", task_key, 5, 0, 1, 3, "decreasing", attempt=3 - ) - return fake, step - - -def test_l6_quality_gates_each_tag_separately_from_joined_frames( - tmp_path: Path, -) -> None: - fake, step = _l6_sweep_quality_fake(tmp_path) - metrics = L6ThreeCameraCalibrationNode._step_observation_metrics(fake) - assert metrics["tag_detection_rate"] == pytest.approx(202 / 211) - assert metrics["joint_frame_rate"] == pytest.approx(188 / 211) - - # This is a valid sweep: every individual Tag is >=95%, the joined frame - # rate is >=85%, and all existing trajectory coverage gates still pass. - L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step) - - -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 - 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: text = render_l6_progress_zh( { @@ -480,505 +343,3 @@ def test_l6_product_config_and_immutable_source_hash_are_valid() -> None: assert product.camera_extrinsics_sha256 == ( "dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80" ) - - -def test_l6_synthetic_fit_recovers_travel_zero_and_mimic() -> None: - records = accepted_records_by_joint(_synthetic_records()) - result = fit_l6_session(SOURCE, records) - for name, expected in TRAVELS.items(): - assert result.travels_rad[name] == pytest.approx(expected, abs=2.0e-4) - source_limits = { - joint.get("name"): { - field: float(joint.find("limit").get(field)) - for field in ("lower", "upper") - } - for joint in ET.parse(SOURCE).getroot().findall("joint") - if joint.find("limit") is not None - } - for name, expected_offset in THUMB_ZERO_OFFSETS.items(): - assert result.zero_offsets_rad[name] == pytest.approx( - expected_offset, abs=2.0e-4 - ) - assert result.zero_method_by_joint[name] == ( - "urdf_serial_axis_geometry" - ) - assert result.zero_fallback_reason_by_joint == {} - assert result.zero_offsets_rad["rh_pinky_mcp_pitch"] == pytest.approx( - source_limits["rh_pinky_mcp_pitch"]["lower"], - abs=2.0e-4, - ) - assert result.zero_method_by_joint["rh_pinky_mcp_pitch"] == ( - "mechanical_lower_endpoint" - ) - for name, expected in MULTIPLIERS.items(): - assert result.mimic_fits[name].multiplier == pytest.approx(expected, abs=2.0e-4) - assert result.mimic_fits[name].maximum_cycle_range < 1.0e-6 - - -def test_l6_pitch_geometry_bound_centres_measured_range_in_cad_range() -> None: - records = accepted_records_by_joint( - _synthetic_records( - thumb_zero_offsets={ - "rh_thumb_cmc_roll": THUMB_ZERO_OFFSETS[ - "rh_thumb_cmc_roll" - ], - "rh_thumb_cmc_pitch": 1.2, - } - ) - ) - result = fit_l6_session(SOURCE, records) - source_pitch = next( - joint - for joint in ET.parse(SOURCE).getroot().findall("joint") - if joint.get("name") == "rh_thumb_cmc_pitch" - ) - expected = 0.5 * ( - float(source_pitch.find("limit").get("lower")) - + float(source_pitch.find("limit").get("upper")) - - TRAVELS["rh_thumb_cmc_pitch"] - ) - assert result.zero_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx( - expected, abs=2.0e-4 - ) - assert result.zero_method_by_joint["rh_thumb_cmc_pitch"] == ( - "cad_range_center_after_geometry_rejection" - ) - assert result.zero_fallback_reason_by_joint == { - "rh_thumb_cmc_pitch": "zero_offset_reached_diagnostic_bound" - } - # A pitch-only fallback must not replace the independently observed roll. - assert result.zero_offsets_rad["rh_thumb_cmc_roll"] == pytest.approx( - THUMB_ZERO_OFFSETS["rh_thumb_cmc_roll"], abs=2.0e-4 - ) - - -def test_l6_fit_recovers_stable_nonlinear_pinky_coupling( - tmp_path: Path, -) -> None: - result = fit_l6_session( - SOURCE, accepted_records_by_joint(_synthetic_nonlinear_pinky_records()) - ) - coupling = result.mimic_fits["rh_pinky_dip"] - assert coupling.model == "quadratic_runtime" - assert coupling.coefficients == pytest.approx((1.2, -0.2), abs=2.0e-4) - assert coupling.maximum_cycle_prediction_range_rad < 1.0e-6 - assert coupling.residual_p95_rad < 2.0e-5 - correction = write_l6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path, - serial_number="L6_NONLINEAR_TEST", - result=result, - timestamp="20260901_120002", - ) - root = ET.parse(correction.path).getroot() - pinky = next( - joint - for joint in root.findall("joint") - if joint.get("name") == "rh_pinky_dip" - ) - pinky_mimic = pinky.find("mimic") - assert pinky_mimic is not None - assert float(pinky_mimic.get("multiplier")) == pytest.approx( - (1.2 * TRAVELS["rh_pinky_mcp_pitch"] - - 0.2 * TRAVELS["rh_pinky_mcp_pitch"] ** 2) - / TRAVELS["rh_pinky_mcp_pitch"], - abs=2.0e-4, - ) - equality = next( - joint - for joint in root.findall("./mujoco/equality/joint") - if joint.get("joint1") == "rh_pinky_dip" - ) - assert [float(value) for value in equality.get("polycoef").split()] == ( - pytest.approx([0.0, 1.2, -0.2, 0.0, 0.0, 0.0], abs=2.0e-4) - ) - for finger in ("index", "middle", "ring"): - transferred = next( - joint - for joint in root.findall("./mujoco/equality/joint") - if joint.get("joint1") == f"rh_{finger}_dip" - ) - assert [ - float(value) for value in transferred.get("polycoef").split() - ] == pytest.approx( - [0.0, 1.2, -0.2, 0.0, 0.0, 0.0], abs=2.0e-4 - ) - - -def test_l6_feedback_endpoint_deadband_is_canonicalized_for_fitting() -> None: - assert [canonical_feedback_command_u8(value) for value in (0, 1, 2)] == [0] * 3 - assert canonical_feedback_command_u8(3) == 3 - assert canonical_feedback_command_u8(252) == 252 - assert [canonical_feedback_command_u8(value) for value in (253, 254, 255)] == [255] * 3 - - records = _synthetic_records() - for row in records: - if row["feedback_u8"] == 0.0: - row["feedback_u8"] = 1.0 - elif row["feedback_u8"] == 255.0: - row["feedback_u8"] = 254.0 - result = fit_l6_session(SOURCE, accepted_records_by_joint(records)) - for name, expected in TRAVELS.items(): - assert result.travels_rad[name] == pytest.approx(expected, abs=2.0e-4) - - -def test_l6_urdf_writer_changes_only_authorized_joint_fields(tmp_path: Path) -> None: - result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) - correction = write_l6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path, - serial_number="L6_TEST", - result=result, - timestamp="20260901_120000", - ) - original = SOURCE.read_text(encoding="utf-8") - corrected = correction.path.read_text(encoding="utf-8") - original_root = ET.parse(SOURCE).getroot() - original_joints = { - joint.get("name"): joint for joint in original_root.findall("joint") - } - root = ET.parse(correction.path).getroot() - joints = {joint.get("name"): joint for joint in root.findall("joint")} - # The already reviewed DIP geometry and limits stay byte-equivalent; only - # the measured mimic multiplier and matching MuJoCo equality may change. - for passive_name in PASSIVE_JOINTS: - passive = joints[passive_name] - source_passive = original_joints[passive_name] - for element in ("origin", "axis", "limit", "parent", "child"): - assert passive.find(element).attrib == source_passive.find(element).attrib - assert passive.find("mimic").get("offset") == ( - source_passive.find("mimic").get("offset") - ) - for finger in ("index", "middle", "ring"): - active_name = f"rh_{finger}_mcp_pitch" - passive_name = f"rh_{finger}_dip" - active = joints[active_name] - source_active = original_joints[active_name] - assert active.find("origin").get("xyz") == source_active.find("origin").get("xyz") - assert active.find("origin").get("rpy") == source_active.find("origin").get("rpy") - assert active.find("axis").attrib == source_active.find("axis").attrib - assert active.find("parent").attrib == source_active.find("parent").attrib - assert active.find("child").attrib == source_active.find("child").attrib - assert float(active.find("limit").get("lower")) == 0.0 - assert float(active.find("limit").get("upper")) == pytest.approx( - TRAVELS["rh_pinky_mcp_pitch"], abs=2.0e-4 - ) - for field in ("effort", "velocity"): - assert active.find("limit").get(field) == source_active.find("limit").get(field) - passive = joints[passive_name] - source_passive = original_joints[passive_name] - for element in ("origin", "axis", "limit", "parent", "child"): - assert passive.find(element).attrib == source_passive.find(element).attrib - assert passive.find("mimic").get("joint") == active_name - assert float(passive.find("mimic").get("multiplier")) == pytest.approx( - MULTIPLIERS["rh_pinky_dip"], abs=2.0e-4 - ) - assert correction.origin_offsets_rad[active_name] == pytest.approx( - correction.origin_offsets_rad["rh_pinky_mcp_pitch"] - ) - assert correction.origin_offsets_rad[active_name] == pytest.approx(0.0) - pinky = joints["rh_pinky_mcp_pitch"] - source_pinky = original_joints["rh_pinky_mcp_pitch"] - assert pinky.find("origin").get("rpy") == source_pinky.find("origin").get("rpy") - assert correction.origin_offsets_rad["rh_pinky_mcp_pitch"] == pytest.approx(0.0) - assert result.zero_method_by_joint["rh_pinky_mcp_pitch"] == ( - "mechanical_lower_endpoint" - ) - roll = joints["rh_thumb_cmc_roll"] - assert float(roll.find("limit").get("lower")) == 0.0 - assert float(roll.find("limit").get("upper")) == pytest.approx(1.34, abs=2.0e-4) - assert correction.origin_offsets_rad["rh_thumb_cmc_roll"] == pytest.approx( - THUMB_ZERO_OFFSETS["rh_thumb_cmc_roll"], abs=2.0e-4 - ) - assert correction.origin_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx( - THUMB_ZERO_OFFSETS["rh_thumb_cmc_pitch"], abs=2.0e-4 - ) - assert roll.find("origin").get("xyz") == "0.0078133 0.030812 0.025678" - assert float( - joints["rh_thumb_dip"].find("mimic").get("multiplier") - ) == pytest.approx(MULTIPLIERS["rh_thumb_dip"]) - assert joints["rh_pinky_dip"].find("mimic") is not None - assert correction.explicit_runtime_joints == frozenset( - {"rh_index_dip", "rh_middle_dip", "rh_ring_dip", "rh_pinky_dip"} - ) - equalities = { - joint.get("joint1"): joint - for joint in root.findall("./mujoco/equality/joint") - } - assert float( - equalities["rh_thumb_dip"].get("polycoef").split()[1] - ) == pytest.approx(MULTIPLIERS["rh_thumb_dip"]) - pinky_polycoef = [ - float(value) - for value in equalities["rh_pinky_dip"].get("polycoef").split() - ] - assert pinky_polycoef == pytest.approx( - [0.0, MULTIPLIERS["rh_pinky_dip"], 0.0, 0.0, 0.0, 0.0], - abs=2.0e-4, - ) - assert len(list((tmp_path / "meshes").iterdir())) == 13 - assert sha256_file(SOURCE) == ( - "298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666" - ) - - -def test_l6_right_corrections_are_mirrored_onto_left_cad(tmp_path: Path) -> None: - result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) - right = write_l6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path / "right", - serial_number="RIGHT_TEST", - result=result, - timestamp="20260903_120000", - ).path - left_path = write_l6_left_from_right_calibration( - source_left_urdf=LEFT_SOURCE, - source_right_urdf=SOURCE, - calibrated_right_urdf=right, - destination_urdf=tmp_path / "left" / "l6_left_transferred.urdf", - ) - original = { - str(joint.get("name")): joint - for joint in ET.parse(LEFT_SOURCE).getroot().findall("joint") - if joint.get("type") == "revolute" - } - transferred_root = ET.parse(left_path).getroot() - transferred = { - str(joint.get("name")): joint - for joint in transferred_root.findall("joint") - if joint.get("type") == "revolute" - } - corrected_right = { - str(joint.get("name")): joint - for joint in ET.parse(right).getroot().findall("joint") - if joint.get("type") == "revolute" - } - for left_name, joint in transferred.items(): - right_name = left_name.replace("lh_", "rh_", 1) - assert joint.find("origin").get("xyz") == original[left_name].find( - "origin" - ).get("xyz") - assert joint.find("axis").attrib == original[left_name].find("axis").attrib - assert joint.find("limit").get("lower") == corrected_right[ - right_name - ].find("limit").get("lower") - assert joint.find("limit").get("upper") == corrected_right[ - right_name - ].find("limit").get("upper") - if joint.find("mimic") is not None: - assert joint.find("mimic").get("multiplier") == corrected_right[ - right_name - ].find("mimic").get("multiplier") - for name, expected in THUMB_ZERO_OFFSETS.items(): - left_name = name.replace("rh_", "lh_", 1) - source_rotation = Rotation.from_euler( - "xyz", - [float(value) for value in original[left_name].find("origin").get( - "rpy" - ).split()], - ) - corrected_rotation = Rotation.from_euler( - "xyz", - [float(value) for value in transferred[left_name].find("origin").get( - "rpy" - ).split()], - ) - axis = np.asarray([ - float(value) - for value in original[left_name].find("axis").get("xyz").split() - ]) - axis /= np.linalg.norm(axis) - applied = float((source_rotation.inv() * corrected_rotation).as_rotvec() @ axis) - assert applied == pytest.approx(expected, abs=1.0e-6) - left_equalities = { - str(node.get("joint1")): node.get("polycoef") - for node in transferred_root.findall("./mujoco/equality/joint") - } - right_equalities = { - str(node.get("joint1")).replace("rh_", "lh_", 1): node.get("polycoef") - for node in ET.parse(right).getroot().findall("./mujoco/equality/joint") - } - assert left_equalities == right_equalities - - right_payload = build_l6_runtime_payload( - serial_number="RIGHT_TEST", - source_urdf=SOURCE, - result=result, - protected_inputs={ - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - }, - ) - left_payload = build_l6_left_transferred_runtime_payload( - right_payload=right_payload, - source_left_urdf=LEFT_SOURCE, - transferred_left_urdf=left_path, - serial_number="LEFT_TRANSFER_TEST", - ) - validate_l6_runtime_payload(left_payload) - mapper = CalibratedCommandMapper(left_payload, expected_side="left") - assert mapper.profile_id == "L6/left/l6_left_transferred_8/v1" - assert set(mapper.urdf_joint_names) == set(transferred) - assert left_payload["joints"]["lh_pinky_dip"]["source_joint"] == ( - "lh_pinky_mcp_pitch" - ) - assert left_payload["joints"]["lh_index_dip"][ - "transferred_from_joint" - ] == "lh_pinky_dip" - assert left_payload["quality"]["transfer_provenance"][ - "left_hand_measured" - ] is False - - -def test_l6_schema_v6_bridge_uses_feedback_and_rh_joint_names() -> None: - result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records())) - hashes = { - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - } - payload = build_l6_runtime_payload( - serial_number="L6_TEST", - source_urdf=SOURCE, - result=result, - protected_inputs=hashes, - ) - validate_l6_runtime_payload(payload) - assert set(payload["joints"]) == set(ACTIVE_JOINTS + PASSIVE_JOINTS) - assert payload["joints"]["rh_thumb_dip"]["urdf_mimic_enabled"] is True - assert payload["joints"]["rh_pinky_dip"]["urdf_mimic_enabled"] is True - assert payload["joints"]["rh_pinky_dip"]["coupling_model"] == ( - "quadratic_runtime" - ) - assert payload["joints"]["rh_pinky_dip"]["urdf_mimic_policy"] == ( - "endpoint_linear_fallback" - ) - assert payload["joints"]["rh_pinky_dip"]["mimic_multiplier"] == ( - pytest.approx(MULTIPLIERS["rh_pinky_dip"], abs=2.0e-4) - ) - for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items(): - assert payload["joints"][target]["calibration_status"] == ( - "transferred_static_dynamic" - ) - assert payload["joints"][target]["transferred_from_joint"] == donor - assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"] - for target, donor in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items(): - assert payload["joints"][target]["calibration_status"] == ( - "transferred_dynamic_cad_static" - ) - assert payload["joints"][target]["transferred_from_joint"] == donor - assert payload["joints"][target]["angle_rad"] == payload["joints"][donor]["angle_rad"] - mapper = CalibratedCommandMapper(payload, expected_side="right") - assert mapper.profile_id == KEY.profile_id - assert mapper.input_domain == "feedback_u8" - mapped = dict(zip(mapper.urdf_joint_names, mapper.map_positions([0] * 6))) - assert mapped["rh_thumb_cmc_roll"] == pytest.approx(TRAVELS["rh_thumb_cmc_roll"], abs=2.0e-4) - # Accept one-release feedback from an older SDK that mislabeled channel 2. - names = list(COMMAND_NAMES) - names[1] = "thumb_cmc_yaw" - assert mapper.map_positions([255] * 6, names) == mapper.map_positions([255] * 6) - assert default_input_topic("right", "feedback_u8", "L6") == ( - "/l6/cb_right_hand_state" - ) - - -def test_l6_online_and_offline_finalization_are_identical(tmp_path: Path) -> None: - records = _synthetic_records() - hashes = { - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - } - online = tmp_path / "online" - offline = tmp_path / "offline" - first, _, first_urdf = finalize_l6_session( - session_dir=online, - serial_number="L6_TEST", - source_urdf=SOURCE, - protected_inputs=hashes, - records=records, - publish=False, - timestamp="20260901_120001", - ) - second, _, second_urdf = finalize_l6_session( - session_dir=offline, - serial_number="L6_TEST", - source_urdf=SOURCE, - protected_inputs=hashes, - records=records, - publish=False, - timestamp="20260901_120001", - ) - assert first == second - assert first_urdf.path.read_bytes() == second_urdf.path.read_bytes() - - -def test_l6_json_handoff_preserves_existing_urdf_bytes(tmp_path: Path) -> None: - records = _synthetic_records() - result = fit_l6_session(SOURCE, accepted_records_by_joint(records)) - direct = write_l6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path / "direct", - serial_number="L6_JSON_HANDOFF", - result=result, - timestamp="20260902_140000", - ) - input_path = tmp_path / "l6_urdf_correction_input.json" - atomic_write_json( - input_path, - build_l6_urdf_input_payload( - serial_number="L6_JSON_HANDOFF", - source_urdf=SOURCE, - result=result, - ), - ) - reloaded = load_l6_urdf_input( - input_path, - source_urdf=SOURCE, - serial_number="L6_JSON_HANDOFF", - ) - via_json = write_l6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path / "via_json", - serial_number="L6_JSON_HANDOFF", - result=reloaded, - timestamp="20260902_140000", - ) - - assert direct.path.read_bytes() == via_json.path.read_bytes() - - -def test_l6_legacy_relative_only_session_cannot_publish_thumb_zero( - tmp_path: Path, -) -> None: - geometric_fields = { - "relative_translation_xyz_m", - "parent_pose_common", - "child_pose_common", - "view_normal_common_xyz", - "camera_center_common_xyz_m", - "state_u8", - } - records = [ - {key: value for key, value in row.items() if key not in geometric_fields} - for row in _synthetic_records() - ] - with pytest.raises(ValueError, match="session must be reacquired"): - finalize_l6_session( - session_dir=tmp_path / "legacy", - serial_number="L6_LEGACY", - source_urdf=SOURCE, - protected_inputs={ - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - }, - records=records, - publish=False, - timestamp="20260901_120003", - ) diff --git a/src/linkerhand_calibration/test/test_native_rotation_curve.py b/src/linkerhand_calibration/test/test_native_rotation_curve.py new file mode 100644 index 0000000..65fd756 --- /dev/null +++ b/src/linkerhand_calibration/test/test_native_rotation_curve.py @@ -0,0 +1,72 @@ +import math +from dataclasses import replace + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.fitting.rotation_curve import RotationObservation, fit_rotation_curve + + +def observations(unit="rad", slip_cycle=None, hysteresis=0.024): + # Independent generator: arbitrary fixed Tag installation on both links. + parent = Rotation.from_euler("xyz", (0.27, -0.18, 0.31)) + child = Rotation.from_euler("xyz", (-0.16, 0.24, -0.21)) + rows = [] + for cycle in range(4): + for direction in ("increasing", "decreasing"): + for index, phase in enumerate(np.linspace(0, 1, 100)): + angle = 0.13 + 0.9*phase + 0.08*phase**2 + angle += hysteresis/2 if direction == "increasing" else -hysteresis/2 + q = parent.inv() * Rotation.from_euler("z", angle) * child + if cycle == slip_cycle: + q = q * Rotation.from_euler("x", math.radians(5)) + value = -0.94 + 0.89*phase if unit == "rad" else 255*phase + rows.append(RotationObservation(f"{cycle}/{direction}/{index}", value, + tuple(q.as_quat()), cycle, direction)) + return rows + + +@pytest.mark.parametrize("unit", ["rad", "u8"]) +def test_native_domain_recovers_motion_with_frozen_arbitrary_tag_mounts(unit): + rows = observations(unit) + fit = fit_rotation_curve([row for row in rows if row.cycle < 3], + input_domain="feedback_" + unit, sdk_to_joint_direction=1) + assert max(fit.holdout_errors([row for row in rows if row.cycle == 3])) < math.radians(0.01) + assert len(fit.knots) == 65 + if unit == "rad": + assert fit.knots[0] == -0.94 + assert fit.knots[-1] == pytest.approx(-0.05) + assert np.max(np.subtract(fit.increasing_rad, fit.decreasing_rad)) == pytest.approx(0.024, abs=1e-9) + + +def test_fourth_cycle_tag_slip_cannot_be_absorbed_into_a_new_reference(): + rows = observations(slip_cycle=3) + fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=1) + assert min(fit.holdout_errors(rows[600:])) > math.radians(4.9) + + +def test_holdout_never_changes_training_domain_or_axis(): + rows = observations() + with pytest.raises(ValueError, match="partition"): + fit_rotation_curve(rows, input_domain="feedback_rad", sdk_to_joint_direction=1) + fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=1) + with pytest.raises(ValueError, match="outside"): + fit.holdout_errors([replace(rows[-1], input_value=0.1)]) + with pytest.raises(ValueError, match="overlaps"): + fit.holdout_errors([replace(rows[-1], sample_id=rows[0].sample_id)]) + + +def test_relative_tag_reference_is_not_published_as_cad_zero(): + rows = observations() + fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=-1) + assert fit.decreasing_rad[-1] < fit.decreasing_rad[0] + # The primitive exposes a measurement reference, not a fabricated zero. + assert not hasattr(fit, "zero_offset_rad") + + +def test_repeatable_hysteresis_keeps_two_branches_and_passes_independent_holdout(): + rows = observations(hysteresis=math.radians(4.2)) + fit = fit_rotation_curve(rows[:600], input_domain="feedback_rad", sdk_to_joint_direction=1) + assert np.max(np.subtract(fit.increasing_rad, fit.decreasing_rad)) == pytest.approx(math.radians(4.2)) + assert max(fit.holdout_errors(rows[600:])) < math.radians(0.01) diff --git a/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py b/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py index b9dc674..7585f22 100644 --- a/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py +++ b/src/linkerhand_calibration/test/test_o12_axis_residual_policy.py @@ -6,7 +6,7 @@ import numpy as np import pytest from scipy.spatial.transform import Rotation -from linkerhand_calibration.models.g20.zero_solver import ( +from linkerhand_calibration.core.fitting.spatial import ( _fit_axis_point_from_pose_trajectory, fit_joint_axis_measurement, ) diff --git a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py index 82a9fbe..b724fb3 100644 --- a/src/linkerhand_calibration/test/test_o12_full_hand_zero.py +++ b/src/linkerhand_calibration/test/test_o12_full_hand_zero.py @@ -1,23 +1,23 @@ """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 ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import JointCurveFit +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.profile import ( CALIBRATED_ACTIVE_JOINTS, GEOMETRIC_ZERO_JOINTS, STATIC_ZERO_EXCLUDED_JOINTS, build_typed_profile, ) -from linkerhand_calibration.models.o12.zero import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.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 linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.kinematics import PASSIVE_SDK_SOURCE_BY_JOINT + from test_o12_right_profile import SOURCE_URDF, _synthetic_records @@ -87,7 +87,7 @@ def test_profile_authorizes_every_observed_active_zero_without_endpoint_assumpti 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 + from linkerhand_calibration.compat.legacy_diagnostic_tools.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 @@ -181,8 +181,8 @@ def test_active_parallel_axis_direction_bias_does_not_become_a_zero(): 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"): + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.fitting import fit_o12_session + with pytest.raises(ValueError, match="requires identified common-frame pose observations"): fit_o12_session(SOURCE_URDF, _synthetic_records(), require_full_hand_spatial_zero=True) @@ -217,68 +217,10 @@ def test_independent_fourth_cycle_cannot_define_or_hide_a_bad_zero(): 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 + from linkerhand_calibration.runtime.artifacts import finalization import json previous = tmp_path / "previous" previous.mkdir() @@ -286,57 +228,14 @@ def test_failed_spatial_solve_writes_evidence_without_replacing_published_result 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) + monkeypatch.setattr(finalization, "fit_profile_calibration", fail) session = tmp_path / "new" with pytest.raises(O12SpatialZeroError): - pipeline.finalize_o12_session(session_dir=session, serial_number="FULL", + finalization.finalize_profile_session(profile=build_typed_profile(), 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) + report = json.loads((session / "fit_diagnostics.json").read_text()) + assert not report["passed"] and not report["publication_allowed"] + assert "unreliable geometry" in report["reason"] diff --git a/src/linkerhand_calibration/test/test_o12_observation_resolution.py b/src/linkerhand_calibration/test/test_o12_observation_resolution.py index 8ae39d9..e3e0785 100644 --- a/src/linkerhand_calibration/test/test_o12_observation_resolution.py +++ b/src/linkerhand_calibration/test/test_o12_observation_resolution.py @@ -3,7 +3,7 @@ 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.compat.legacy_diagnostic_tools.models.o12 import observations as module from linkerhand_calibration.pnp import SquareTagPose @@ -87,31 +87,19 @@ def test_other_task_evidence_is_not_mixed_into_thumb(monkeypatch): def test_partial_external_projection_cannot_finalize_whole_hand(tmp_path): - from linkerhand_calibration.models.o12.pipeline import finalize_o12_session + from linkerhand_calibration.compat.legacy_diagnostic_tools.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: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12 import pipeline + from linkerhand_calibration.runtime.artifacts import finalization + def must_not_fit(*args, **kwargs): + raise AssertionError('unverified projection must be rejected before fitting') + monkeypatch.setattr(finalization,'fit_profile_calibration',must_not_fit) + with pytest.raises(ValueError, match='camera_projection_unverified'): 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_readonly_bridge.py b/src/linkerhand_calibration/test/test_o12_readonly_bridge.py new file mode 100644 index 0000000..30b362a --- /dev/null +++ b/src/linkerhand_calibration/test/test_o12_readonly_bridge.py @@ -0,0 +1,37 @@ +"""Transport tests never construct a hand or open HCAN.""" + +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.runtime.adapters.o12_bridge import ERROR_FIELDS, read_health + + +def test_hardware_health_is_read_only_and_distinguishes_historical_bit(): + reports = [SimpleNamespace(**{name: False for name in ERROR_FIELDS}) for _ in range(12)] + reports[1].commu_except = True + reads = [] + hand = SimpleNamespace( + get_all_control_modes=lambda: reads.append("mode") or [0]*12, + get_all_error_reports=lambda: reads.append("errors") or reports) + value = read_health(hand) + assert reads == ["mode", "errors"] + assert value["position_mode"] + assert value["communication_latches"] == [1] + assert not value["active_faults"] + reports[2].overheat = True + assert read_health(hand)["active_faults"] == ["channel=2:overheat"] + + +def test_partial_report_is_not_treated_as_good_health(): + hand = SimpleNamespace(get_all_control_modes=lambda: [0]*12, get_all_error_reports=lambda: []) + with pytest.raises(ValueError, match="incomplete"): + read_health(hand) + + +def test_vendor_package_is_pinned_before_any_import(tmp_path): + from linkerhand_calibration.runtime.adapters.vendor_package import load_vendor_sdk + path = tmp_path/"untrusted.whl" + path.write_bytes(b"not a wheel") + with pytest.raises(ValueError, match="changed"): + load_vendor_sdk(path, "0"*64) diff --git a/src/linkerhand_calibration/test/test_o12_recorded_replay.py b/src/linkerhand_calibration/test/test_o12_recorded_replay.py index e12cdf4..e743258 100644 --- a/src/linkerhand_calibration/test/test_o12_recorded_replay.py +++ b/src/linkerhand_calibration/test/test_o12_recorded_replay.py @@ -9,8 +9,8 @@ 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.compat.legacy_diagnostic_tools.models.o12.pipeline import finalize_o12_session, load_o12_raw_samples +from linkerhand_calibration.compat.legacy_diagnostic_tools.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 diff --git a/src/linkerhand_calibration/test/test_o12_right_profile.py b/src/linkerhand_calibration/test/test_o12_right_profile.py index f7f0691..b876c20 100644 --- a/src/linkerhand_calibration/test/test_o12_right_profile.py +++ b/src/linkerhand_calibration/test/test_o12_right_profile.py @@ -1,67 +1,31 @@ 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, - 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 ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.health import ( assess_o12_error_report, decoded_faults, historical_communication_latch_confirmed, ) -from linkerhand_calibration.models.o12.motion import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.motion import ( cosine_position_trajectory_rad, cosine_ramp_velocity_trajectory_rad, ) -from linkerhand_calibration.models.o12.kinematics import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.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.node import MotionStep as O12MotionStep -from linkerhand_calibration.pnp import SquareTagPose -from linkerhand_calibration.models.o12.runner import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.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 ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.runner import _launch_command +from linkerhand_calibration.compat.legacy_diagnostic_tools.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, @@ -70,7 +34,6 @@ from linkerhand_calibration.models.o12.profile import ( 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, @@ -81,31 +44,18 @@ from linkerhand_calibration.models.o12.profile import ( 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 + + 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 = { @@ -155,10 +105,11 @@ def _synthetic_records(): ("decreasing", np.linspace(0.0, 1.0, 65)), ("increasing", np.linspace(1.0, 0.0, 65)), ): - for phase in phases: + for sample_index, phase in enumerate(phases): feedback = task.start_value + phase * (task.end_value - task.start_value) angle = phase * travel[name] rows.append({ + "sample_id": f"{task.key}:{cycle}:{direction}:{sample_index}", "cycle": cycle, "direction": direction, "feedback_rad": float(feedback), @@ -198,14 +149,14 @@ def test_o12_fixed_mapping_tags_and_radian_domain() -> None: 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 + 0.10, 0.16, 0.32, 0.16, 0.32, 0.12, 0.32, 0.32, 0.12, 0.32, 0.32 ] 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", "side_base", "index_pip") side_tag_ids = { tag.role: tag.tag_id for view in profile.vision.views @@ -213,6 +164,7 @@ def test_o12_fixed_mapping_tags_and_radian_domain() -> None: for tag in view.tags } assert side_tag_ids["side_base"] == 4 + assert side_tag_ids["index_pip"] == 10 assert side_tag_ids["index_dip"] == 11 assert { key: (value.view, value.parent_role, value.child_role) @@ -298,9 +250,12 @@ def test_o12_hcan_launch_does_not_emit_empty_socketcan_argument(tmp_path: Path) def test_o12_vendor_node_is_launched_in_documented_namespace() -> None: launch_text = (ROOT / "launch/three_camera_calibration.launch.py").read_text() - vendor = launch_text.split('package="omnihand_node"', 1)[1].split(")", 1)[0] - assert 'executable="omnihand_pro_2025_node"' in vendor - assert 'namespace="o12"' in vendor + assert 'executable="o12_sdk_bridge"' in launch_text + assert 'sdk_package_expected_sha256' in launch_text + from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics + topics = sdk_topics(build_typed_profile()) + assert topics.command == "/o12/right/joint_cmd" + assert topics.feedback == "/o12/right/joint_states" def test_o12_vendor_stream_is_not_artificially_throttled() -> None: @@ -315,38 +270,6 @@ def test_o12_vendor_stream_is_not_artificially_throttled() -> None: 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( - temperature_report_required=False, - temperature_verified=False, - temperature_fallback_active=False, - error_verified=False, - health_check_started_at=10.0, - temperature_fallback_after_seconds=5.0, - raw_path=tmp_path / "raw.jsonl", - protected_inputs={"sdk_config_sha256": "a" * 64}, - get_logger=lambda: SimpleNamespace(warning=warnings.append), - ) - - O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed( - fake, 16.0 - ) - assert not fake.temperature_fallback_active - - fake.error_verified = True - O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed( - fake, 16.0 - ) - assert fake.temperature_fallback_active - assert warnings == [ - "O12 temperature report unavailable; continuing with error-code " - "bit1 overheat protection" - ] - raw = (tmp_path / "raw.jsonl").read_text() - 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)]) @@ -378,339 +301,6 @@ def test_o12_error_health_separates_active_faults_from_historical_comm_latch( ) -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, - temperature_verified=False, - temperature_fallback_active=False, - error_verified=True, - health_check_started_at=10.0, - temperature_fallback_after_seconds=5.0, - ) - O12ThreeCameraCalibrationNode._activate_temperature_fallback_if_allowed( - fake, 30.0 - ) - assert not fake.temperature_fallback_active - - -def test_o12_motion_plan_has_collision_clearance_and_safe_return() -> None: - profile = build_typed_profile() - fake = SimpleNamespace( - profile=profile, - 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_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].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"] - 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: @@ -726,894 +316,16 @@ def test_o12_log_exception_summary_reports_child_traceback( ) -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: @@ -1638,427 +350,6 @@ def test_o12_feedback_domain_contains_but_does_not_expand_command_domain() -> No ) -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: text = render_o12_progress_zh({ "state": "RUNNING", @@ -2137,69 +428,6 @@ def test_o12_operator_progress_uses_unified_layout_and_radians() -> None: ) 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()) - 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, - ) - assert payload["schema_version"] == 7 - assert len(payload["joints"]["thumb_cmc_yaw"]["angle_rad"]) == 65 - assert payload["joints"]["thumb_cmc_roll"]["raw_increasing_curve_branch"] == "decreasing" - assert payload["joints"]["thumb_cmc_yaw"]["raw_increasing_curve_branch"] == "increasing" - assert "command_range" not in payload - assert payload["joints"]["ring_mcp_pitch"]["transferred_from_joint"] == "pinky_mcp_pitch" - 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, - serial_number="O12_TEST", - result=result, - timestamp="20260904_120000", - ) - before = ET.parse(SOURCE_URDF).getroot() - after = ET.parse(correction.path).getroot() - by_name_before = {node.get("name"): node for node in before.findall("joint")} - 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: @@ -2214,148 +442,21 @@ def test_o12_vendor_passive_solver_regression() -> None: ) == 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() - ) + + + + +def test_immutable_o12_cad_ring_mimic_conflict_is_not_waived(): + from linkerhand_calibration.core.urdf.patch import validate_urdf_mimic_ranges + with pytest.raises(ValueError, match="ring_pip"): + validate_urdf_mimic_ranges(SOURCE_URDF) diff --git a/src/linkerhand_calibration/test/test_o12_static_geometry.py b/src/linkerhand_calibration/test/test_o12_static_geometry.py deleted file mode 100644 index 2439c07..0000000 --- a/src/linkerhand_calibration/test/test_o12_static_geometry.py +++ /dev/null @@ -1,156 +0,0 @@ -"""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 index d810799..9d95cef 100644 --- a/src/linkerhand_calibration/test/test_o12_thumb_pnp.py +++ b/src/linkerhand_calibration/test/test_o12_thumb_pnp.py @@ -1,11 +1,7 @@ -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.compat.legacy_diagnostic_tools.models.o12.pnp import O12ThumbPoseTracker, THUMB_ROLES from linkerhand_calibration.pnp import SquareTagPose @@ -63,62 +59,3 @@ def test_missing_tag_discards_observation_and_gap_reinitializes(): 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 8d1c665..5976231 100644 --- a/src/linkerhand_calibration/test/test_o6_right_profile.py +++ b/src/linkerhand_calibration/test/test_o6_right_profile.py @@ -1,7 +1,6 @@ from __future__ import annotations from pathlib import Path -from types import SimpleNamespace import xml.etree.ElementTree as ET import numpy as np @@ -11,17 +10,12 @@ from scipy.spatial.transform import Rotation from linkerhand_calibration.calibrated_joint_state_bridge import CalibratedCommandMapper from linkerhand_calibration.core import validate_profile from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix -from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel -from linkerhand_calibration.models.l6.node import MotionStep -from linkerhand_calibration.models.o6.artifacts import ( - build_o6_left_transferred_runtime_payload, - build_o6_runtime_payload, +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.zero_solver import UrdfKinematicModel +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.artifacts import ( validate_o6_runtime_payload, ) -from linkerhand_calibration.models.o6.fitting import fit_o6_session -from linkerhand_calibration.models.o6.fitting import _zero_profile -from linkerhand_calibration.models.o6.node import O6ThreeCameraCalibrationNode -from linkerhand_calibration.models.o6.profile import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.fitting import _zero_profile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.profile import ( ACTIVE_JOINTS, BASELINE_SPEED_U8, COMMAND_NAMES, @@ -38,11 +32,7 @@ from linkerhand_calibration.models.o6.profile import ( TRANSFERRED_PASSIVE_SOURCE_BY_JOINT, build_typed_profile, ) -from linkerhand_calibration.models.o6.runner import render_o6_progress_zh -from linkerhand_calibration.models.o6.urdf import ( - write_o6_corrected_urdf, - write_o6_left_from_right_calibration, -) +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o6.runner import render_o6_progress_zh PACKAGE = Path(__file__).resolve().parents[1] @@ -73,7 +63,8 @@ def test_o6_profile_declares_reviewed_six_channel_contract() -> None: assert MAXIMUM_HYSTERESIS_DEG == 3.5 assert MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX == 1.5 assert MAXIMUM_CROSS_VIEW_AXIS_LINE_RMS_M == 0.020 - assert GEOMETRIC_ZERO_JOINTS == frozenset({"rh_thumb_cmc_yaw"}) + assert GEOMETRIC_ZERO_JOINTS == frozenset(profile.zero.direct_zero_joints) + assert not profile.zero.endpoint_anchor_by_joint assert profile.vision.extrinsics_quality_limits[ "reprojection_rms_px" ] == 1.5 @@ -90,7 +81,7 @@ def test_o6_profile_declares_reviewed_six_channel_contract() -> None: "thumb_yaw_top", "thumb_pitch_ip_front", "pinky_pitch_dip_side" ] assert profile.zero.coupling_model_by_joint == { - name: "quadratic_runtime" for name in PASSIVE_JOINTS + name: "linear_mimic" for name in PASSIVE_JOINTS } @@ -178,44 +169,6 @@ def test_o6_baseline_failure_identifies_the_slowest_channel() -> None: assert "原因：目标电机连续两秒没有向目标推进" in text -def test_o6_motion_keeps_all_non_target_channels_at_255() -> None: - published: list[list[int]] = [] - fake = SimpleNamespace( - baseline_command=(255,) * 6, - step_started_at=0.0, - step_start_state_u8=(255.0,) * 6, - command_trajectory_full_range_seconds=6.0, - step_last_command_u8=None, - step_trajectory_phase=0.0, - step_requested_u8=255.0, - _publish_command=lambda values: published.append(values), - ) - step = MotionStep("sweep", "thumb_yaw_top", 1, 0, 1, 0, "decreasing") - for tick in range(601): - O6ThreeCameraCalibrationNode._advance_step_trajectory(fake, step, tick / 100.0) - assert published[0] == [255] * 6 - assert published[-1] == [255, 0, 255, 255, 255, 255] - assert all(command[0] == 255 and command[2:] == [255] * 4 for command in published) - - -def test_o6_motion_plan_uses_o6_specific_speed_tiers() -> None: - fake = SimpleNamespace( - profile=build_typed_profile(), - baseline_speed_u8=BASELINE_SPEED_U8, - preflight_speed_u8=PREFLIGHT_SPEED_U8, - formal_speed_u8=FORMAL_SPEED_U8, - ) - steps = O6ThreeCameraCalibrationNode._build_steps(fake) - assert steps[0].phase == "baseline" - assert steps[0].speed_u8 == 80 - assert not [step for step in steps if step.phase == "preflight"] - assert { - step.speed_u8 - for step in steps - if step.phase in {"prepare", "sweep"} - } == {40} - - def _valid_payload() -> dict: profile = build_typed_profile() curves = list(np.linspace(1.0, 0.0, 256)) @@ -313,54 +266,6 @@ def _curve_records(travel: float) -> list[dict]: return rows -def test_o6_fit_artifact_bridge_and_urdf_patch_end_to_end(tmp_path: Path) -> None: - records = { - "rh_thumb_cmc_pitch": _curve_records(0.5), - "rh_thumb_cmc_yaw": _curve_records(1.2), - "rh_thumb_ip": _curve_records(0.5 * 1.86), - "rh_pinky_mcp_pitch": _curve_records(1.2), - "rh_pinky_dip": _curve_records(1.2 * 0.89), - } - result = fit_o6_session(SOURCE, records) - assert result.mimic_fits["rh_thumb_ip"].multiplier == pytest.approx(1.86) - assert result.mimic_fits["rh_pinky_dip"].multiplier == pytest.approx(0.89) - payload = build_o6_runtime_payload( - serial_number="O6_TEST", - source_urdf=SOURCE, - result=result, - protected_inputs={ - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - }, - ) - mapper = CalibratedCommandMapper(payload, expected_side="right") - assert payload["quality"]["maximum_hysteresis_limit_rad"] == pytest.approx( - np.deg2rad(3.5), abs=1e-8 - ) - assert set(payload["quality"]["maximum_hysteresis_by_joint_rad"]) == set( - ACTIVE_JOINTS[:2] + (ACTIVE_JOINTS[-1],) + PASSIVE_JOINTS[::4] - ) - mapped = dict(zip(mapper.urdf_joint_names, mapper.map_positions([0] * 6))) - assert mapped["rh_thumb_ip"] == pytest.approx(0.93, abs=2e-4) - assert mapped["rh_index_mcp_pitch"] == pytest.approx(1.2, abs=2e-4) - correction = write_o6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path, - serial_number="O6_TEST", - result=result, - ) - corrected = ET.parse(correction.path).getroot() - joints = {joint.get("name"): joint for joint in corrected.findall("joint")} - assert result.mimic_fits["rh_thumb_ip"].model == "quadratic_runtime" - assert result.mimic_fits["rh_pinky_dip"].model == "quadratic_runtime" - assert float(joints["rh_thumb_ip"].find("mimic").get("multiplier")) == pytest.approx(1.86) - assert float(joints["rh_pinky_dip"].find("mimic").get("multiplier")) == pytest.approx(0.89) - assert set(correction.mimic_multipliers) == set(PASSIVE_JOINTS) - assert correction.explicit_runtime_joints == frozenset(PASSIVE_JOINTS) - assert float(joints["rh_thumb_ip"].find("limit").get("upper")) == pytest.approx(0.93, abs=2e-4) - assert float(joints["rh_index_mcp_pitch"].find("limit").get("upper")) == pytest.approx(1.2) def _pose(xyz: list[float], rpy: list[float]) -> np.ndarray: @@ -470,98 +375,3 @@ def _geometric_records() -> dict[str, list[dict]]: "state_u8": state, }) return result - - -def test_o6_uses_geometric_yaw_and_cad_endpoint_pitch() -> None: - result = fit_o6_session( - SOURCE, _geometric_records(), require_thumb_axis_zero=True - ) - assert result.zero_offsets_rad["rh_thumb_cmc_yaw"] == pytest.approx(0.03, abs=1e-6) - assert result.zero_offsets_rad["rh_thumb_cmc_pitch"] == pytest.approx(0.0) - assert result.zero_method_by_joint["rh_thumb_cmc_yaw"] == ( - "urdf_serial_axis_geometry" - ) - assert result.zero_method_by_joint["rh_thumb_cmc_pitch"] == ( - "mechanical_lower_endpoint" - ) - assert result.zero_fallback_reason_by_joint == {} - assert result.thumb_zero_result.direct_offsets_rad[ - "rh_thumb_cmc_pitch" - ] == pytest.approx(-0.02, abs=1e-6) - - -def test_o6_right_corrections_are_mirrored_without_replacing_left_geometry( - tmp_path: Path, -) -> None: - result = fit_o6_session( - SOURCE, _geometric_records(), require_thumb_axis_zero=True - ) - right = write_o6_corrected_urdf( - source_urdf=SOURCE, - output_directory=tmp_path / "right", - serial_number="RIGHT_TEST", - result=result, - ).path - destination = write_o6_left_from_right_calibration( - source_left_urdf=LEFT_SOURCE, - source_right_urdf=SOURCE, - calibrated_right_urdf=right, - destination_urdf=tmp_path / "left" / "o6_left_transferred.urdf", - ) - original = _joint_elements(LEFT_SOURCE) - transferred = _joint_elements(destination) - corrected_right = _joint_elements(right) - for left_name, joint in transferred.items(): - right_name = left_name.replace("lh_", "rh_", 1) - assert joint.find("origin").get("xyz") == original[left_name].find( - "origin" - ).get("xyz") - assert joint.find("axis").attrib == original[left_name].find("axis").attrib - assert joint.find("limit").get("upper") == corrected_right[ - right_name - ].find("limit").get("upper") - if joint.find("mimic") is not None: - assert joint.find("mimic").get("multiplier") == corrected_right[ - right_name - ].find("mimic").get("multiplier") - assert transferred["lh_thumb_cmc_pitch"].find("origin").get("rpy") == ( - original["lh_thumb_cmc_pitch"].find("origin").get("rpy") - ) - left_yaw = Rotation.from_euler( - "xyz", - [float(value) for value in transferred["lh_thumb_cmc_yaw"].find( - "origin" - ).get("rpy").split()], - ) - assert left_yaw.as_rotvec()[2] == pytest.approx(0.03, abs=1e-6) - - right_payload = build_o6_runtime_payload( - serial_number="RIGHT_TEST", - source_urdf=SOURCE, - result=result, - protected_inputs={ - "source_urdf_sha256": "0" * 64, - "camera_extrinsics_sha256": "1" * 64, - "calibration_config_sha256": "2" * 64, - "tag_config_sha256": "3" * 64, - }, - ) - left_payload = build_o6_left_transferred_runtime_payload( - right_payload=right_payload, - source_left_urdf=LEFT_SOURCE, - transferred_left_urdf=destination, - serial_number="LEFT_TRANSFER_TEST", - ) - validate_o6_runtime_payload(left_payload) - mapper = CalibratedCommandMapper(left_payload, expected_side="left") - assert mapper.profile_id == "O6/left/o6_left_transferred_8/v1" - assert set(mapper.urdf_joint_names) == set(transferred) - mapped = dict(zip( - mapper.urdf_joint_names, - mapper.map_positions([0, 255, 255, 255, 255, 255]), - )) - assert mapped["lh_thumb_cmc_pitch"] == pytest.approx(0.5, abs=2.0e-4) - assert mapped["lh_thumb_ip"] == pytest.approx(0.93, abs=2.0e-4) - assert left_payload["quality"]["transfer_provenance"][ - "left_hand_measured" - ] is False diff --git a/src/linkerhand_calibration/test/test_observation_capture.py b/src/linkerhand_calibration/test/test_observation_capture.py new file mode 100644 index 0000000..4aa4c39 --- /dev/null +++ b/src/linkerhand_calibration/test/test_observation_capture.py @@ -0,0 +1,92 @@ +"""Independent pinhole projections exercise the real PnP/capture boundary.""" + +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np +from scipy.spatial.transform import Rotation +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.capture import CaptureFrame, ObservationCapture +from linkerhand_calibration.runtime.reference_lock import ReferenceLock +from linkerhand_calibration.runtime.motion_execution import MotionCommand + + +def projected_square(size, translation, rpy=(.1, -.15, .03)): + # IPPE square order: top-left, top-right, bottom-right, bottom-left. + s = size/2 + points = np.array([[-s, s, 0], [s, s, 0], [s, -s, 0], [-s, -s, 0]]) + xyz = points @ Rotation.from_euler("xyz", rpy).as_matrix().T + translation + return (xyz[:, :2]/xyz[:, 2:])*1200 + [640, 480] + + +def make_capture(): + profile = load_bundled_hand_profile("l6_right_8") + front = profile.vision.views[0] + tags = tuple(replace(tag, size_m=.024 if tag.role == "thumb_pitch" else .016) for tag in front.tags) + profile = replace(profile, vision=replace(profile.vision, + views=(replace(front, tags=tags),)+profile.vision.views[1:])) + lock = ReferenceLock({v.name: next(t.tag_id for t in v.tags if t.fixed_reference) for v in profile.vision.views}) + capture = ObservationCapture(profile, reference_lock=lock, + extrinsics=SimpleNamespace(transform=lambda _: np.eye(4))) + return profile, lock, capture + + +def frame(profile, stamp, *, omit=(), shift=0, feedback=True): + roles = {"front_base": (.0, .0, .45), "thumb_pitch": (.05, .03, .45), "thumb_dip": (.08, .05, .45)} + sizes = {tag.role: tag.size_m for tag in profile.vision.views[0].tags} + corners = {role: projected_square(sizes[role], xyz)+[shift, 0] for role, xyz in roles.items() if role not in omit} + values = tuple(map(float, profile.command.baseline_values)) + return CaptureFrame("front", stamp, np.array([[1200, 0, 640], [0, 1200, 480], [0, 0, 1]]), + corners, values if feedback else None, values) + + +def test_only_ten_unique_post_start_valid_frames_lock_and_mounts_keep_per_tag_size(): + profile, lock, capture = make_capture() + for index in range(20): + capture.consume(frame(profile, index+1)) + assert not lock.references # Preview is not a reference. + lock.begin_session() + capture.reset() + lock.start_locking() + for _ in range(20): + capture.consume(frame(profile, 100), collect_installations=True) + assert not lock.references + for index in range(9): + capture.consume(frame(profile, 101+index), collect_installations=True) + fixed, moving = capture.fingerprint_poses() + assert "front" in fixed + np.testing.assert_allclose(fixed["front"]["translation_xyz_m"], [0, 0, .45], atol=1e-7) + assert "front:thumb_pitch" in moving + assert len(capture.installations["front:thumb_pitch"]) == 10 + + +def test_short_visual_loss_discards_frames_cached_base_is_not_fake_image_data(): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for i in range(10): + capture.consume(frame(profile, i+1), collect_installations=True) + motion = MotionCommand("sweep", tuple(profile.command.baseline_values), 1, + "thumb_pitch_dip_front", 0, 0, "decreasing") + rows, moved = capture.consume(frame(profile, 20, omit=("front_base",)), motion) + assert moved is None and len(rows) == 2 + assert rows[0]["locked_reference_roles"] == ["front_base"] + assert "image_relative_xy_px" not in rows[0] + assert capture.consume(frame(profile, 21, feedback=False), motion)[0] == () + assert capture.consume(frame(profile, 22, omit=("thumb_pitch",)), motion)[0] == () + assert len(capture.consume(frame(profile, 23), motion)[0]) == 2 + assert len(capture.installations["front:thumb_pitch"]) == 10 # Frozen during motion. + + +@pytest.mark.parametrize("drift", [6, 200]) +def test_fixed_reference_displacement_requires_ten_confirming_images(drift): + profile, lock, capture = make_capture() + lock.begin_session() + lock.start_locking() + for i in range(10): + capture.consume(frame(profile, i+1)) + for i in range(9): + assert capture.consume(frame(profile, 20+i, shift=drift))[1] is None + assert capture.consume(frame(profile, 29, shift=drift))[1] is not None diff --git a/src/linkerhand_calibration/test/test_package_rename.py b/src/linkerhand_calibration/test/test_package_rename.py index 7a3a6a4..658cded 100644 --- a/src/linkerhand_calibration/test/test_package_rename.py +++ b/src/linkerhand_calibration/test/test_package_rename.py @@ -35,7 +35,7 @@ def test_new_and_legacy_executable_names_share_one_implementation() -> None: setup_text, ) assert ( - "linkerhand_calibration.runtime.nodes.calibration:main" + "linkerhand_calibration.runtime.ros.entrypoint:main" in setup_text ) diff --git a/src/linkerhand_calibration/test/test_pnp.py b/src/linkerhand_calibration/test/test_pnp.py index f66923e..cc6f166 100644 --- a/src/linkerhand_calibration/test/test_pnp.py +++ b/src/linkerhand_calibration/test/test_pnp.py @@ -140,54 +140,31 @@ def test_clear_reprojection_advantage_releases_stale_mirror_branch() -> None: assert selected == true_pose -def test_active_motion_can_prioritise_continuous_branch() -> None: - previous = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 55.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.0, 0.0, 0.25), - reprojection_error_px=0.25, - ) - continuous = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 54.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.0, 0.0, 0.25), - reprojection_error_px=0.24, - ) - discontinuous = SquareTagPose( - quaternion_xyzw=tuple( - Rotation.from_euler("y", 2.0, degrees=True).as_quat() - ), - translation_xyz_m=(0.001, 0.0, 0.25), - reprojection_error_px=0.05, - ) - - selected, reason = select_continuous_pose( - [continuous, discontinuous], - previous=previous, - maximum_reprojection_error_px=1.5, - reprojection_tie_px=1.5, +@pytest.mark.parametrize("tie", [-0.01, 1.5, 2.0, float("nan"), float("inf")]) +def test_invalid_branch_tolerance_is_rejected_before_tracking(tie) -> None: + parameters = dict( + maximum_reprojection_error_px=1.5, reprojection_tie_px=tie, maximum_pose_jump_rad=np.deg2rad(35.0), - maximum_translation_jump_m=0.04, - maximum_tag_tilt_rad=np.deg2rad(75.0), + maximum_translation_jump_m=0.04, maximum_tag_tilt_rad=np.deg2rad(75.0), ) - - assert reason == "" - assert selected == continuous + with pytest.raises(ValueError, match="reprojection_tie_px"): + SquareTagPoseTracker(**parameters, reset_after_seconds=5.0) + with pytest.raises(ValueError, match="reprojection_tie_px"): + select_continuous_pose([], previous=None, **parameters) def test_tracker_recovers_after_timestamp_gap() -> None: tracker = SquareTagPoseTracker( maximum_reprojection_error_px=1.5, - reprojection_tie_px=1.5, - maximum_pose_jump_rad=np.deg2rad(5.0), + maximum_pose_jump_rad=np.deg2rad(1.0), maximum_translation_jump_m=0.04, maximum_tag_tilt_rad=np.deg2rad(75.0), reset_after_seconds=0.5, ) first_rotation = Rotation.from_euler("y", 0.0, degrees=True) - second_rotation = Rotation.from_euler("y", 20.0, degrees=True) + # Keep both IPPE solutions near-tied so this exercises gap recovery, + # rather than correction of a branch with clearly worse image fit. + second_rotation = Rotation.from_euler("y", 2.0, degrees=True) first, first_reason = tracker.estimate( "t0", _project(first_rotation, np.asarray([0.0, 0.0, 0.25])), @@ -237,7 +214,6 @@ def _pose( def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None: per_tag = SquareTagPoseTracker( maximum_reprojection_error_px=1.5, - reprojection_tie_px=1.5, maximum_pose_jump_rad=np.deg2rad(35.0), maximum_translation_jump_m=0.04, maximum_tag_tilt_rad=np.deg2rad(75.0), diff --git a/src/linkerhand_calibration/test/test_pnp_runtime_policy.py b/src/linkerhand_calibration/test/test_pnp_runtime_policy.py new file mode 100644 index 0000000..f720942 --- /dev/null +++ b/src/linkerhand_calibration/test/test_pnp_runtime_policy.py @@ -0,0 +1,72 @@ +"""Exercise branch selection with shipped parameters and capture defaults.""" + +from pathlib import Path +from dataclasses import asdict + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation +import yaml + +from linkerhand_calibration.core.geometry import pnp +from linkerhand_calibration.core.geometry.tag_pose import tracking +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.capture import ObservationCapture +from linkerhand_calibration.runtime.ros.parameters import parameter_defaults, tracking_parameters + + +PACKAGE = Path(__file__).resolve().parents[1] +LAYOUTS = ("g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16") + + +def _pose(angle, error): + return pnp.SquareTagPose( + tuple(Rotation.from_euler("y", angle, degrees=True).as_quat()), + (0.0, 0.0, 0.25), error, + ) + + +def _check_branch_selection(tracker, monkeypatch): + stale = _pose(55, 0.25) + accurate = _pose(2, 0.05) + continued = _pose(3, 0.11) + frames = iter([ + (stale,), + (_pose(54, 0.24), accurate), + # A real near-tie still favors continuous motion over a mirror flip. + (_pose(55, 0.10), continued), + ]) + monkeypatch.setattr(tracking, "solve_square_tag_ippe", lambda *a, **kw: next(frames)) + for index, expected in enumerate((stale, accurate, continued)): + selected, reason = tracker.estimate( + "moving", np.zeros((4, 2)), tag_size_m=0.016, + camera_matrix=np.eye(3), stamp_ns=1_000_000_000 + index * 30_000_000, + ) + assert reason == "" + assert selected == expected + assert tracker.branch_correction_counts == {"moving": 1} + + +@pytest.mark.parametrize("layout,filename", [ + ("g20_right_19", "three_camera_calibration.yaml"), + ("l6_right_8", "l6_three_camera_calibration.yaml"), + ("o6_right_8", "o6_three_camera_calibration.yaml"), + ("o12_right_16", "o12_three_camera_calibration.yaml"), + ("g20_right_19", "calibration.yaml"), +]) +def test_shipped_ros_parameters_release_stale_branch(layout, filename, monkeypatch): + profile = load_bundled_hand_profile(layout) + config = yaml.safe_load((PACKAGE / "config" / filename).read_text()) + params = dict(next(iter(config.values()))["ros__parameters"]) + params = {**parameter_defaults(profile), **params} + for _view in profile.vision.view_names: + tracker = pnp.SquareTagPoseTracker(**asdict(tracking_parameters(params.__getitem__))) + _check_branch_selection(tracker, monkeypatch) + + +@pytest.mark.parametrize("layout", LAYOUTS) +def test_capture_defaults_release_stale_branch(layout, monkeypatch): + capture = ObservationCapture(load_bundled_hand_profile(layout), + reference_lock=None, extrinsics=None) + for tracker in capture.trackers.values(): + _check_branch_selection(tracker, monkeypatch) diff --git a/src/linkerhand_calibration/test/test_profile_finalization.py b/src/linkerhand_calibration/test/test_profile_finalization.py new file mode 100644 index 0000000..684df26 --- /dev/null +++ b/src/linkerhand_calibration/test/test_profile_finalization.py @@ -0,0 +1,221 @@ +"""Production finalizer recovery using an independent elementary FK generator. + +The virtual URDF preserves the vendor's geometry but uses explicit synthetic +limits. It is not evidence that a real hand or its original limits passed. +""" + +from dataclasses import replace +from pathlib import Path +import hashlib +import xml.etree.ElementTree as ET + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.geometry.extrinsics import matrix_payload +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.core.domain.profile import ProfileKey +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session + + +PACKAGE = Path(__file__).resolve().parents[1] + + +def rigid(rpy, xyz): + matrix = np.eye(4) + matrix[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + matrix[:3, 3] = xyz + return matrix + + +def virtual_capture(tmp_path, layout): + base_layout = "l6_right_8" if layout == "virtual_reordered" else layout + profile = load_bundled_hand_profile(base_layout) + if layout == "virtual_reordered": + profile = replace(profile, key=ProfileKey("VIRTUAL", "right", layout), + namespace="/virtual_calibration", + artifacts=replace(profile.artifacts, output_schema_version=2), + motion=replace(profile.motion, tasks=tuple(reversed(profile.motion.tasks)))) + filename = {"l6_right_8": "l6_right/linkerhand_l6v3.1_right.urdf", + "o6_right_8": "o6_right/linkerhand_o6_right.urdf", + "g20_right_19": "g20_right/linkerhand_g20_right.urdf", + "o12_right_16": "o12_right/linkerhand_o12_t3_right-0703.urdf"}[base_layout] + original = PACKAGE / "urdf" / filename + if not original.exists(): + original = next((PACKAGE / "urdf/o6_right").glob("*.urdf")) + tree = ET.parse(original) + root = tree.getroot() + root.set("name", "virtual_geometry_acceptance_not_hardware") + joints = {node.get("name"): node for node in root.findall("joint")} + for node in joints.values(): + limit = node.find("limit") + if limit is not None and node.get("type") == "revolute": + limit.set("lower", "-10" if node.find("mimic") is not None else "-2") + limit.set("upper", "10" if node.find("mimic") is not None else "2") + for mesh in root.iter("mesh"): + filename = mesh.get("filename") + if not filename.startswith(("package://", "file://", "/")): + mesh.set("filename", (original.parent / filename).resolve().as_uri()) + source = tmp_path / "virtual_source.urdf" + tree.write(source) + parents = {node.find("child").get("link"): name for name, node in joints.items()} + base = rigid((0.11, -0.08, 0.17), (0.11, -0.04, 0.72)) + base_tag = rigid((0.2, -0.1, 0.3), (0.003, 0.008, 0.055)) + tags = {name: rigid((0.12, -0.09, 0.2), (0.009, -0.025, 0.016)) + for name in profile.measurement.measurements} + # Independent fixture declarations: one actual mount per physical Tag, + # including Tags reused by several measurement tasks. + native_tag_joint = {"thumb_cmc": "thumb_cmc_pitch", "thumb_mcp": "thumb_mcp", + "thumb_dip": "thumb_dip", "thumb_yaw": "thumb_cmc_yaw", + "middle_roll": "middle_mcp_roll", "index_roll": "index_mcp_roll", + "pinky_mcp": "pinky_mcp_pitch", "pinky_pip": "pinky_pip", "pinky_dip": "pinky_dip", + "middle_pip": "middle_pip", "middle_dip": "middle_dip", "index_pip": "index_pip", "index_dip": "index_dip"} + native_tag_joint.update(thumb_ip="thumb_ip", ring_roll="ring_mcp_roll", pinky_roll="pinky_mcp_roll", + ring_pip="ring_pip", ring_dip="ring_dip") + direct = profile.zero.direct_zero_joints + offsets = {name: (0.04 if name.endswith(("roll", "yaw")) else -0.02 if "thumb" in name else 0.02) + for name in direct} + for target, donor in profile.zero.transferred_zero_sources.items(): + offsets[target] = offsets[donor] + def elementary_fk(joint, angles): + # Deliberately independent of UrdfKinematicModel and patch helpers. + node = joints[joint] + parent = parents.get(node.find("parent").get("link")) + transform = np.eye(4) if parent is None else elementary_fk(parent, angles) + origin = node.find("origin") + if origin is not None: + transform = transform @ rigid([float(v) for v in origin.get("rpy", "0 0 0").split()], + [float(v) for v in origin.get("xyz", "0 0 0").split()]) + if node.get("type") != "fixed": + axis = np.asarray([float(v) for v in node.find("axis").get("xyz").split()]) + motion = np.eye(4) + motion[:3, :3] = Rotation.from_rotvec(axis/np.linalg.norm(axis)*angles[joint]).as_matrix() + transform = transform @ motion + return transform + records = [] + for task in profile.motion.tasks: + for cycle in range(4): + for direction in ("increasing", "decreasing"): + codes = (np.linspace(0, 255, 81).round().astype(int) if profile.command.unit == "u8" + else np.linspace(min(task.start_value, task.end_value), max(task.start_value, task.end_value), 81)) + if direction == "decreasing": + codes = codes[::-1] + for index, code in enumerate(codes): + state = list(profile.command.baseline_values) + for channel, value in task.auxiliary_commands: + state[channel] = value + state[task.command_index] = float(code) + angles = {name: offsets.get(name, 0) + ((profile.command.baseline_values[channel]-state[channel])/255 * + (0.65 if "thumb" in name and "pitch" in name else 1.05) if profile.command.unit == "u8" + else profile.command.joint_directions[channel]*state[channel]*0.7) + for name, channel in profile.command.command_index_by_joint.items()} + def passive_angle(name): + if name in angles: + return angles[name] + mimic = joints[name].find("mimic") + angles[name] = float(mimic.get("multiplier", "1"))*passive_angle(mimic.get("joint")) + float(mimic.get("offset", "0")) + return angles[name] + for name in profile.zero.passive_joints: + passive_angle(name) + measurements = [(name, name, False) for name in task.joints] + measurements += [(name, profile.measurement.cross_view_sources[name], True) + for name in task.joints if name in profile.measurement.cross_view_sources] + for name, spec_name, secondary in measurements: + spec = profile.measurement.measurements[spec_name] + if base_layout in {"o12_right_16", "g20_right_19"}: + def tag_pose(role): + joint = native_tag_joint.get(role) + return base @ base_tag if joint is None else base @ elementary_fk(joint, angles) @ tags[joint] + parent, child = tag_pose(spec.parent_role), tag_pose(spec.child_role) + else: + child = base @ elementary_fk(name, angles) @ tags[name] + parent_joint = profile.zero.mimic_source_by_joint.get(name) + parent = base @ base_tag if parent_joint is None else base @ elementary_fk(parent_joint, angles) @ tags[parent_joint] + relative = np.linalg.inv(parent) @ child + records.append({"kind": "joint_sample", "observation_joint" if secondary else "joint": name, "task_name": task.key, + "view": spec.view, "sample_id": f"{spec.view}:{task.key}:{cycle}:{direction}:{index}", + "cycle": cycle, "direction": direction, "attempt": 1, f"feedback_{profile.command.unit}": float(code), + f"command_{profile.command.unit}": float(code), f"command_vector_{profile.command.unit}": list(state), + f"state_{profile.command.unit}": state, "relative_quaternion_xyzw": Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(), + "image_relative_xy_px": [30+100*np.cos(angles[name]+.23), 20+100*np.sin(angles[name]+.23)], + "relative_translation_xyz_m": relative[:3, 3].tolist(), + "parent_pose_common": matrix_payload(parent), "child_pose_common": matrix_payload(child), + "view_normal_common_xyz": [0.577350269]*3, "camera_center_common_xyz_m": [0, 0, -0.5]}) + # Independent sensor observations at the requested steady grid, using the + # same elementary FK truth above, not the fitted calibration as truth. + from linkerhand_calibration.runtime.engine import CalibrationEngine + from linkerhand_calibration.runtime.steady import steady_targets + steady = [] + for unit in CalibrationEngine(profile).scan_units(): + samples = [r for r in records if (r["task_name"], r["cycle"], r["direction"]) + == (unit.task_key, unit.cycle, unit.direction)] + for index, target in enumerate(steady_targets(profile, unit)): + matched = [r for r in samples if abs(r[f"command_{profile.command.unit}"]-target) < 1e-8] + assert matched + for row in matched: + for frame in range(3): + steady.append({**row, "sample_id": row["sample_id"]+f":steady:{frame}", + "sample_phase": "steady", "steady_index": index, "steady_target": target}) + records.extend(steady) + return profile, source, records, offsets + + +@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "virtual_reordered", "o12_right_16", "g20_right_19"]) +def test_profile_only_finalizer_recovers_geometry_and_serialized_urdf(tmp_path, layout): + profile, source, records, expected = virtual_capture(tmp_path, layout) + if layout == "virtual_reordered": + # The new hand is loaded through the product's real YAML contract; + # there is no Python registry entry or model-specific engine binding. + import yaml + from linkerhand_calibration.profiles import dump_hand_profile + from linkerhand_calibration.product import load_product_config + path = tmp_path/"new_hand.yaml" + path.write_text(dump_hand_profile(profile)) + product = yaml.safe_load((PACKAGE/"config/l6_right_product.yaml").read_text()) + product.update(profile_id=profile.key.profile_id, model=profile.key.model, + namespace=profile.namespace, tag_layout=profile.key.layout, + profile_config=str(path), profile_config_sha256=hashlib.sha256(path.read_bytes()).hexdigest()) + product["artifacts"].update(source_urdf=str(source), source_urdf_sha256=hashlib.sha256(source.read_bytes()).hexdigest()) + product_path = tmp_path/"new_hand_product.yaml" + product_path.write_text(yaml.safe_dump(product)) + profile = load_product_config(product_path, workspace=PACKAGE.parents[1], check_can=False).calibration_contract.typed_profile + from engine_capture_fixture import collect_with_engine + from linkerhand_calibration.runtime.session import CalibrationPhase as Phase + execution, records = collect_with_engine(profile, records) + phase_events = { + "fit_complete": execution.session.fit_complete, + "holdout_complete": lambda: execution.session.holdout_complete(passed=True), + "artifacts_built": execution.session.artifacts_built, + "urdf_validated": lambda: execution.session.urdf_validated(passed=True), + } + session = tmp_path / "session" + loaded = [] + hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} + hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() + payload, fit, correction = finalize_profile_session(profile=profile, session_dir=session, + serial_number="VIRTUAL_ONLY", source_urdf=source, protected_inputs=hashes, + records=records, standard_loader=lambda path: loaded.append(path), + phase_changed=lambda event: phase_events[event]()) + assert execution.session.phase == Phase.PUBLISH + execution.session.published() + assert loaded + assert (tmp_path / profile.artifacts.publication_pointer).resolve() == session + assert payload["format"] == "unified_calibration_v2" + assert payload["schema_version"] == 2 + assert (session / "calibration_report.json").is_file() + assert "quality" not in payload + for row in payload["joints"].values(): + assert "command_to_rad" not in row + assert set(row) == {"sdk_channel", "angle_rad"} | ({"input_values"} if profile.command.unit == "rad" else set()) + if profile.command.unit == "u8": + assert len(row["angle_rad"]) == 256 + if payload["schema_version"] == 7: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.o12.artifacts import validate_o12_runtime_payload + validate_o12_runtime_payload(payload) + if payload["schema_version"] == 4: + from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import validate_compact_payload + validate_compact_payload(payload) + for name, value in expected.items(): + assert fit.zero_offsets_rad[name] == pytest.approx(value, abs=0.004) + assert correction.path.is_file() diff --git a/src/linkerhand_calibration/test/test_profile_loader.py b/src/linkerhand_calibration/test/test_profile_loader.py new file mode 100644 index 0000000..f3c943f --- /dev/null +++ b/src/linkerhand_calibration/test/test_profile_loader.py @@ -0,0 +1,136 @@ +from __future__ import annotations + +from pathlib import Path +import pytest +import yaml + +from linkerhand_calibration.profiles import load_hand_profile +from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry +from linkerhand_calibration.profiles import dump_hand_profile +from linkerhand_calibration.runtime import CalibrationEngine +from linkerhand_calibration.profiles.loader import load_bundled_hand_profile + + +@pytest.mark.parametrize("section,key", [(None, "commmand"), ("command", "maximum_velocty"), + ("motion", "resume_verfication_waypoints"), ("zero", "cad_frozen_joint")]) +def test_profile_typo_is_rejected_before_hardware(tmp_path, section, key): + payload = yaml.safe_load(dump_hand_profile(load_bundled_hand_profile("l6_right_8"))) + (payload if section is None else payload[section])[key] = [] + path = tmp_path/"invalid.yaml" + path.write_text(yaml.safe_dump(payload)) + with pytest.raises(ValueError, match="unknown"): + load_hand_profile(path) + + +def test_new_model_loads_without_model_python_state_machine(tmp_path: Path) -> None: + profile_file = tmp_path / "virtual.yaml" + profile_file.write_text( + """schema_version: 1 +profile_id: VIRTUAL/right/one_joint/v1 +namespace: /virtual_calibration +sdk_adapter: legacy_byte_sdk +command: + names: [finger] + baseline_u8: [255] + command_index_by_joint: {finger_joint: 0} + urdf_joint_by_joint: {finger_joint: finger_joint} +vision: + common_frame: camera + extrinsic_reference_view: camera + views: + - name: camera + tags: + - {role: base, id: 0, fixed_reference: true} + - {role: finger, id: 1} +motion: + tasks: + - key: finger_sweep + view: camera + command_index: 0 + joints: [finger_joint] + start_u8: 255 + end_u8: 0 + formal_speed_u8: 40 +measurement: + measurements: + finger_joint: {kind: rotation, view: camera, parent_role: base, child_role: finger} +zero: + active_joints: [finger_joint] + passive_joints: [] + direct_zero_joints: [finger_joint] + axis_joints: [finger_joint] + mechanical_endpoint_joints: [] + post_solve_endpoint_joints: [] + mimic_source_by_joint: {} + cad_frozen_joints: [] +quality: + training_cycles: [0, 1, 2] + holdout_cycle: 3 + hard_threshold_keys: [holdout] + isolated_holdout: true +scope: + default_scope: full + calibrate_joints: {full: [finger_joint]} + frozen_joints: {full: []} +artifacts: + output_schema_version: 1 + calibration_filename: calibration.json + corrected_urdf_filename: corrected.urdf + protected_input_fields: [source_urdf_sha256] + publish_corrected_urdf: true +acquisition: {policy_version: unified_engine_v4_dual_mapping} +urdf: + authorized_fields: + finger_joint: [origin.rpy, limit.lower, limit.upper] +joint_coverage: {finger_joint: measured_static_dynamic} +""", + encoding="utf-8", + ) + profile = load_hand_profile(profile_file) + assert profile.sdk_adapter == "legacy_byte_sdk" + assert profile.urdf_authorized_fields["finger_joint"] == { + "origin.rpy", "limit.lower", "limit.upper" + } + assert len(CalibrationEngine(profile).scan_units()) == 8 + + +def test_all_product_profiles_have_lossless_declarative_contracts( + tmp_path: Path, +) -> None: + expected = {"g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"} + checked = set() + for registered in get_default_registry(): + profile = registered.profile + if profile.key.layout not in expected: + continue + assert profile == load_bundled_hand_profile(profile.key.layout) + path = tmp_path / f"{profile.key.layout}.yaml" + path.write_text(dump_hand_profile(profile), encoding="utf-8") + assert load_hand_profile(path) == profile + checked.add(profile.key.layout) + assert checked == expected + + +def test_product_loading_a_new_profile_does_not_construct_model_registry(tmp_path, monkeypatch): + import hashlib + import yaml + from linkerhand_calibration import product + root = Path(__file__).resolve().parents[1] + profile_data = yaml.safe_load((root / "config/profiles/g20_right_19.yaml").read_text()) + profile_data["profile_id"] = "VIRTUAL/right/new_configuration_only/v1" + profile_data["namespace"] = "/virtual_calibration" + profile_path = tmp_path / "virtual.yaml" + profile_path.write_text(yaml.safe_dump(profile_data)) + data = yaml.safe_load((root / "config/g20_right_product.yaml").read_text()) + data.update(profile_id=profile_data["profile_id"], model="VIRTUAL", + tag_layout="new_configuration_only", profile_config=str(profile_path), + profile_config_sha256=hashlib.sha256(profile_path.read_bytes()).hexdigest()) + path = tmp_path / "product.yaml" + path.write_text(yaml.safe_dump(data)) + def forbidden(): + raise AssertionError("declarative product must not resolve a Python model") + monkeypatch.setattr(product, "get_default_registry", forbidden) + loaded = product.load_product_config(path, workspace=root.parents[1], check_can=False) + assert loaded.model == "VIRTUAL" + assert loaded.calibration_contract.typed_profile.key.layout == "new_configuration_only" + # This is the input-contract test, deliberately not an end-to-end claim. diff --git a/src/linkerhand_calibration/test/test_progress_status.py b/src/linkerhand_calibration/test/test_progress_status.py new file mode 100644 index 0000000..6f5ffc4 --- /dev/null +++ b/src/linkerhand_calibration/test/test_progress_status.py @@ -0,0 +1,105 @@ +from __future__ import annotations +import pytest + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry +from linkerhand_calibration.runtime import ( + CalibrationPhase, + CalibrationSession, + ResumeDecision, + SweepQuality, +) + + +def _profile(layout: str): + return next( + item.profile for item in get_default_registry() + if item.profile.key.layout == layout + ) + + +@pytest.mark.parametrize("reason", ["fixed_reference_moved", "feedback_stale", "hardware_fault", + "duplicate_controller", "physical_range_exceeded", "protected_camera_model_changed"]) +def test_live_failures_have_specific_chinese_reasons_and_suggestions(reason): + from linkerhand_calibration.runtime.reporting.reasons_zh import reason_zh + code, message, suggestion = reason_zh({"reason": reason+":details"}, model_name="VIRTUAL") + assert code != "CALIBRATION-500" and message and suggestion + session = CalibrationSession(_profile("l6_right_8")) + session.pause(code, message, {"raw_reason": reason}, suggestion=suggestion) + assert session.status().pause.suggestion == suggestion + + +def test_one_session_machine_reports_task_tags_and_locked_warning() -> None: + session = CalibrationSession(_profile("o12_right_16")) + session.device_ready() + session.start(resume_requested=True) + session.baseline_complete() + session.reference_locked() + session.resume_checked(ResumeDecision(False, "基准变化，已重新采集")) + session.preparation_complete() + while session.phase == CalibrationPhase.MAPPING_PROBE: + session.mapping_probe_complete() + status = session.status() + assert status.state == "SWEEP" + assert status.reference_locked + assert "请勿移动" in status.reference_message + assert status.task.required_tag_ids_by_view + + +def test_one_same_speed_rescan_then_pause_with_reason() -> None: + session = CalibrationSession(_profile("l6_right_8")) + session.device_ready() + session.start() + session.baseline_complete() + session.reference_locked() + session.preparation_complete() + failed = SweepQuality(False, ("frames=20",), (), {}) + session.sweep_complete() + session.evaluation_complete(failed) + assert session.phase == CalibrationPhase.PREPARE + session.preparation_complete() + assert session.phase == CalibrationPhase.RESCAN + session.sweep_complete() + session.evaluation_complete(failed) + assert session.phase == CalibrationPhase.PAUSED + assert session.status().pause.code == "sweep_quality_failed" + + +def test_physical_mapping_probe_occurs_after_each_task_prepare() -> None: + session = CalibrationSession(_profile("o12_right_16")) + session.device_ready() + session.start() + session.baseline_complete() + session.reference_locked() + session.preparation_complete() + assert session.phase == CalibrationPhase.MAPPING_PROBE + session.mapping_probe_complete() + passed = SweepQuality(True, (), (), {}) + first_task = session.current_unit.task_key + while session.current_unit is not None and session.current_unit.task_key == first_task: + if session.phase == CalibrationPhase.PREPARE: + session.preparation_complete() + assert session.phase == CalibrationPhase.SWEEP + session.sweep_complete() + session.evaluation_complete(passed) + assert session.phase == CalibrationPhase.PREPARE + session.preparation_complete() + assert session.phase == CalibrationPhase.MAPPING_PROBE + + +def test_resume_skips_only_complete_units_and_still_prepares_current_task() -> None: + session = CalibrationSession(_profile("l6_right_8")) + first_task = session.current_unit.task_key + completed = tuple( + (unit.task_key, unit.cycle, unit.direction) + for unit in session.engine.scan_units() + if unit.task_key == first_task + ) + session.device_ready() + session.start(resume_requested=True) + session.baseline_complete() + session.reference_locked() + session.resume_checked( + ResumeDecision(True, "verified", completed_units=completed) + ) + assert session.phase == CalibrationPhase.PREPARE + assert session.current_unit.task_key != first_task diff --git a/src/linkerhand_calibration/test/test_rectified_camera_contract.py b/src/linkerhand_calibration/test/test_rectified_camera_contract.py index 71dd358..16d4654 100644 --- a/src/linkerhand_calibration/test/test_rectified_camera_contract.py +++ b/src/linkerhand_calibration/test/test_rectified_camera_contract.py @@ -28,16 +28,46 @@ def test_invalid_p_cannot_silently_fall_back_to_k(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 + node = camera_host(tmp_path, msg) + callback=accept_camera callback(node,'front',msg); callback(node,'front',msg) - np.testing.assert_allclose(node.camera_matrices['front'],np.array(p).reshape(3,4)[:,:3]) + np.testing.assert_allclose(node.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 + assert 'front' not in node.matrices + + +def camera_host(tmp_path, message): + from linkerhand_calibration.runtime.cameras import CameraObservations + from linkerhand_calibration.core.geometry.extrinsics import camera_info_fingerprint + identity = SimpleNamespace(width=message.width, height=message.height, + intrinsics_sha256=camera_info_fingerprint(width=message.width, height=message.height, + camera_matrix=message.k, distortion=message.d, rectification=message.r, projection=message.p)) + return CameraObservations(SimpleNamespace(cameras={'front': identity}), tmp_path/'raw.jsonl', lambda: 10.) + + +def accept_camera(host, view, message, *, started=False, pause=lambda _: None): + from linkerhand_calibration.runtime.ros.io import camera_input + host.accept(view, camera_input(message), started=started, pause=pause, trackers={}) + + +@pytest.mark.parametrize('field', ['k', 'd', 'r', 'p']) +def test_live_camera_model_must_match_protected_extrinsics(tmp_path, field): + msg = SimpleNamespace(p=[500.,0,320,0,0,510,240,0,0,0,1,0], + k=[600.,0,300,0,620,220,0,0,1], d=[.1]*5, + r=np.eye(3).ravel().tolist(), width=640, height=480) + node = camera_host(tmp_path, msg) + callback = accept_camera + callback(node, 'front', msg) + assert 'front' in node.matrices + pauses = [] + getattr(msg, field)[0] += .01 + callback(node, 'front', msg, started=True, pause=pauses.append) + assert 'front' not in node.matrices + assert 'front' not in node.info_received_at + assert pauses == ['protected_camera_model_changed:front:camera_info_does_not_match_extrinsics'] diff --git a/src/linkerhand_calibration/test/test_reference_lock.py b/src/linkerhand_calibration/test/test_reference_lock.py new file mode 100644 index 0000000..5494277 --- /dev/null +++ b/src/linkerhand_calibration/test/test_reference_lock.py @@ -0,0 +1,45 @@ +from __future__ import annotations + +import numpy as np + +from linkerhand_calibration.runtime import ReferenceLock + + +def _corners(offset: float = 0.0): + return np.asarray( + [[10.0, 10.0], [20.0, 10.0], [20.0, 20.0], [10.0, 20.0]] + ) + offset + + +def test_preview_and_baseline_motion_are_discarded_before_lock() -> None: + lock = ReferenceLock({"camera": 7}) + for _ in range(20): + lock.observe("camera", 7, _corners(100.0)) + lock.begin_session() + for _ in range(20): + lock.observe("camera", 7, _corners(50.0)) + assert not lock.locked + + lock.start_locking() + for _ in range(9): + lock.observe("camera", 7, _corners()) + assert not lock.locked + lock.observe("camera", 7, _corners()) + assert lock.locked + assert np.allclose(lock.references["camera"], _corners()) + + +def test_locked_reference_is_not_rewritten_and_needs_ten_bad_frames() -> None: + lock = ReferenceLock({"camera": 7}) + lock.begin_session() + lock.start_locking() + for _ in range(10): + lock.observe("camera", 7, _corners()) + original = lock.fingerprint() + + for _ in range(9): + assert lock.observe("camera", 7, _corners(6.0)) is None + movement = lock.observe("camera", 7, _corners(6.0)) + assert movement is not None + assert movement.confirmation_frames == 10 + assert lock.fingerprint() == original diff --git a/src/linkerhand_calibration/test/test_reference_waypoints.py b/src/linkerhand_calibration/test/test_reference_waypoints.py new file mode 100644 index 0000000..2d21a58 --- /dev/null +++ b/src/linkerhand_calibration/test/test_reference_waypoints.py @@ -0,0 +1,59 @@ +from dataclasses import replace +import pytest + +from linkerhand_calibration.core.domain.profile import ReferenceWaypoint +from linkerhand_calibration.profiles import load_bundled_hand_profile, dump_hand_profile, load_hand_profile +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from linkerhand_calibration.runtime.resume import select_installation_evidence, ResumeVerifier +from test_resume_v2 import _fingerprint + + +def test_reference_waypoint_cannot_drive_disabled_channel(): + from linkerhand_calibration.core.domain.profile import validate_profile + profile = load_bundled_hand_profile("g20_right_19") + channel = next(iter(profile.command.disabled_indices)) + pose = list(profile.command.baseline_values) + pose[channel] -= 1 + view = profile.vision.views[0] + tag = next(tag for tag in view.tags if not tag.fixed_reference) + waypoint = ReferenceWaypoint("show", tuple(pose), {view.name: (tag.tag_id,)}) + profile = replace(profile, motion=replace(profile.motion, resume_verification_waypoints=(waypoint,))) + with pytest.raises(ValueError, match="disabled channels"): + validate_profile(profile) + + +def test_reference_waypoints_share_executor_and_return_before_resume(tmp_path): + profile = load_bundled_hand_profile("l6_right_8") + pose = (230., 255., 255., 255., 255., 255.) + waypoint = ReferenceWaypoint("show_tip", pose, {"front": (2,)}) + profile = replace(profile, motion=replace(profile.motion, resume_verification_waypoints=(waypoint,))) + path = tmp_path/"virtual.yaml" + path.write_text(dump_hand_profile(profile)) + assert load_hand_profile(path) == profile + engine = SessionExecution(profile) + engine.session.device_ready() + engine.session.start(resume_requested=True) + current = profile.command.baseline_values + while engine.session.phase == Phase.BASELINE: + motion = engine.motion(current); current = motion.target; engine.motion_complete() + engine.session.reference_locked() + assert engine.session.phase == Phase.REFERENCE_POSES + motion = engine.motion(current) + assert motion.phase == "reference_pose" and motion.target == pose + while engine.session.phase == Phase.REFERENCE_POSES: + motion = engine.motion(current); current = motion.target; engine.motion_complete() + assert current == profile.command.baseline_values + assert engine.session.phase == Phase.RESUME_VERIFY + + +def test_resume_never_compares_mounts_from_different_waypoint_poses(): + fingerprint = _fingerprint() + pose = fingerprint.moving_tag_poses["finger"] + old = replace(fingerprint, moving_tag_poses={"show/finger": pose}) + new = replace(fingerprint, moving_tag_poses={"other/finger": pose}) + checker = ResumeVerifier(required_moving_poses=("finger",)) + assert not checker.compare(*select_installation_evidence(old, new, {"finger"})).reuse + assert checker.compare(*select_installation_evidence(old, old, {"finger"})).reuse + missing = replace(fingerprint, moving_tag_poses={}) + assert not checker.compare(*select_installation_evidence(old, missing, {"finger"})).reuse diff --git a/src/linkerhand_calibration/test/test_relative_standard_mimic.py b/src/linkerhand_calibration/test/test_relative_standard_mimic.py new file mode 100644 index 0000000..14caefa --- /dev/null +++ b/src/linkerhand_calibration/test/test_relative_standard_mimic.py @@ -0,0 +1,53 @@ +import math +from dataclasses import replace + +import numpy as np +import pytest + +from linkerhand_calibration.core.fitting.coupling import ( + PairedJointObservation, fit_relative_standard_mimic, +) + + +def observations(*, nonlinear=False): + rows = [] + for cycle in range(4): + for direction in ("increasing", "decreasing"): + for index, q in enumerate(np.linspace(0, 1.4, 65)): + # Independently generated fixed mounting phases; neither is a + # physical zero. Backlash in the driver affects both joints. + q += .003 if direction == "decreasing" else 0 + child = .87*q + (.22*q*q if nonlinear else 0) + rows.append(PairedJointObservation(f"camera:{cycle}:{direction}:{index}", + cycle, direction, q+.24, child-.31)) + return rows + + +def test_unknown_mounting_phase_is_not_exported_as_passive_zero(): + result = fit_relative_standard_mimic("driver", "follower", observations(), cad_offset_rad=.012) + assert result.fit.multiplier == pytest.approx(.87, abs=1e-10) + assert result.fit.offset_rad == .012 + assert not result.fit.offset_observed + assert result.installation_phase_rad == pytest.approx(-.31-.87*.24) + assert max(abs(v) for v in result.holdout_errors_rad) < 1e-10 + + +def test_nonlinear_motion_cannot_hide_behind_json_or_endpoint_ratio(): + with pytest.raises(ValueError, match="standard_urdf_mimic_not_expressive"): + fit_relative_standard_mimic("driver", "follower", observations(nonlinear=True), cad_offset_rad=0) + + +def test_holdout_installation_slip_cannot_be_recentred(): + rows = [replace(row, target_rad=row.target_rad + (math.radians(5) if row.cycle == 3 else 0)) + for row in observations()] + with pytest.raises(ValueError, match="phase=holdout"): + fit_relative_standard_mimic("driver", "follower", rows, cad_offset_rad=0) + + +def test_both_directions_require_independent_paired_images(): + rows = observations() + with pytest.raises(ValueError, match="duplicated"): + fit_relative_standard_mimic("driver", "follower", rows + [rows[0]], cad_offset_rad=0) + with pytest.raises(ValueError, match="missing paired"): + fit_relative_standard_mimic("driver", "follower", + [row for row in rows if not (row.cycle == 3 and row.direction == "decreasing")], cad_offset_rad=0) diff --git a/src/linkerhand_calibration/test/test_resume_v2.py b/src/linkerhand_calibration/test/test_resume_v2.py new file mode 100644 index 0000000..830ca56 --- /dev/null +++ b/src/linkerhand_calibration/test/test_resume_v2.py @@ -0,0 +1,48 @@ +from __future__ import annotations + +import math + +from linkerhand_calibration.runtime import ( + ACQUISITION_POLICY_VERSION, + ResumeFingerprint, + ResumeVerifier, + TagPoseFingerprint, +) + + +def _fingerprint(*, corner_offset=0.0, hash_value="a", angle=0.0): + return ResumeFingerprint( + profile_id="TEST/right/layout/v1", + protected_hashes={"source": hash_value * 64}, + fixed_corners_by_view={ + "view": tuple( + (x + corner_offset, y + corner_offset) + for x, y in ((0, 0), (10, 0), (10, 10), (0, 10)) + ) + }, + fixed_poses={ + "base": TagPoseFingerprint( + (0.0, 0.0, math.sin(angle / 2), math.cos(angle / 2)), + (0.0, 0.0, 0.4), + ) + }, + moving_tag_poses={ + "finger": TagPoseFingerprint((0.0, 0.0, 0.0, 1.0), (0.1, 0, 0.4)) + }, + acquisition_policy_version=ACQUISITION_POLICY_VERSION, + ) + + +def test_matching_v2_reference_reuses_checkpoint() -> None: + decision = ResumeVerifier().compare(_fingerprint(), _fingerprint()) + assert decision.reuse + + +def test_reference_or_protected_input_change_starts_fresh_without_pause() -> None: + changed = ResumeVerifier().compare( + _fingerprint(), _fingerprint(corner_offset=6.0, hash_value="b") + ) + assert not changed.reuse + assert "重新采集" in changed.reason + assert "protected_hashes" in changed.incompatible_fields + assert "fixed_corners[view]" in changed.incompatible_fields diff --git a/src/linkerhand_calibration/test/test_runtime_readiness.py b/src/linkerhand_calibration/test/test_runtime_readiness.py new file mode 100644 index 0000000..6aa334d --- /dev/null +++ b/src/linkerhand_calibration/test/test_runtime_readiness.py @@ -0,0 +1,96 @@ +"""Device readiness and progress must reflect evidence, for any view layout.""" + +from dataclasses import asdict, replace +from types import MethodType, SimpleNamespace +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.adapters import HardwareHealth +from linkerhand_calibration.runtime.motion_execution import MotionCommand +from linkerhand_calibration.runtime.coordinator import CalibrationCoordinator +from linkerhand_calibration.runtime.status import normalize_status, render_status_zh + + +def test_missing_stale_and_recovered_camera_pipeline_are_named_without_tag_gate(): + profile = load_bundled_hand_profile("o6_right_8") + host = SimpleNamespace(profile=profile, command_count=6, latest_feedback=(0,)*6, + ports=SimpleNamespace(command_publisher_count=lambda: 1), + sdk_adapter=SimpleNamespace(health=lambda: HardwareHealth(True, True)), + cameras=SimpleNamespace(matrices={"front": True, "side": True, "top": True}, + info_received_at={"front": 10., "side": 10., "top": 5.}, + detections_received_at={"side": 10.}, errors={})) + status = asdict(CalibrationCoordinator._device_status(host, 10.)) + assert not status["ready"] + assert not status["cameras"]["front"]["detections_ready"] + assert status["cameras"]["side"]["detections_ready"] + assert not status["cameras"]["top"]["camera_info_ready"] + text = render_status_zh(normalize_status({"state": "WAIT_DEVICES", "devices": status}, + profile=profile, serial_number="12345")) + assert "O6 / right / 12345" in text + assert "front 未收到" in text and "top 未收到" in text + # Readiness requires messages, not recognized Tag IDs or a rate floor. + host.cameras.info_received_at = dict.fromkeys(profile.vision.view_names, 10.) + host.cameras.detections_received_at = dict(host.cameras.info_received_at) + assert CalibrationCoordinator._device_status(host, 10.).ready + host.ports.command_publisher_count = lambda: 2 + assert "存在其他机械手指令发布者" in CalibrationCoordinator._device_status(host, 10.).waiting_for + + +def test_steady_checkpoint_does_not_erase_direction_statistics(): + host = SimpleNamespace(_steady_rows=[], _unit_rows=[], _capture_unit_key=None) + motion = MotionCommand("sweep", (0.,), 1., task_key="test", command_index=0, + cycle=0, direction="decreasing") + begin = MethodType(CalibrationCoordinator._begin_motion_capture, host) + begin(motion) + host._unit_rows.append({"sample_phase": "sweep"}) + begin(replace(motion, phase="steady", steady_index=0)) + host._steady_rows.append({"sample_phase": "steady"}) + host._unit_rows.extend(host._steady_rows) + begin(replace(motion, phase="steady", steady_index=1)) + assert len(host._unit_rows) == 2 + assert host._steady_rows == [] + begin(replace(motion, attempt=2)) + assert host._unit_rows == [] + + +def test_ros_io_loads_profile_views_instead_of_three_camera_compatibility(tmp_path): + import json + from linkerhand_calibration.core.geometry.extrinsics import camera_info_fingerprint + from linkerhand_calibration.runtime.ros.parameters import parameter_defaults, load_runtime_parameters + profile = load_bundled_hand_profile("o6_right_8") + vision = replace(profile.vision, + views=(replace(profile.vision.views[0], name="inspection"),), + extrinsic_reference_view="inspection", extrinsics_quality_limits={}, minimum_capture_counts={}) + profile = replace(profile, vision=vision) + extrinsics = tmp_path / "extrinsics.json" + extrinsics.write_text(json.dumps({"schema_version": 1, "reference_view": "inspection", + "quality": {"passed": True}, + "cameras": {"inspection": {"serial_number": "test", "width": 640, "height": 480, + "intrinsics_sha256": camera_info_fingerprint(width=640, height=480, + camera_matrix=[500,0,320,0,500,240,0,0,1])}}, + "inspection_from_view": {"inspection": {"translation_xyz_m": [0,0,0], + "quaternion_xyzw": [0,0,0,1]}}})) + params = parameter_defaults(profile) + params.update(serial_number="test", session_dir=str(tmp_path), source_urdf_path=str(extrinsics), + camera_extrinsics_file=str(extrinsics)) + for key in profile.artifacts.protected_input_fields: + params[key.replace("_sha256", "_expected_sha256")] = "a"*64 + resolved = load_runtime_parameters(profile, params.__getitem__) + assert set(resolved.extrinsics.cameras) == {"inspection"} + assert set(resolved.detection_topics) == {"inspection"} + + +@pytest.mark.parametrize("schema", [1, 4, 6, 7]) +def test_production_cannot_fall_back_to_feedback_only_legacy_output(tmp_path, schema): + from pathlib import Path + from linkerhand_calibration.profiles.validator import validate_executable_profile + from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session + profile = load_bundled_hand_profile("o6_right_8") + profile = replace(profile, artifacts=replace(profile.artifacts, output_schema_version=schema)) + source = Path(__file__).resolve().parents[1] / "urdf/o6_right/linkerhand_o6_right.urdf" + with pytest.raises(ValueError, match="unified output_schema_version"): + validate_executable_profile(profile, source) + with pytest.raises(ValueError, match="unified output_schema_version"): + finalize_profile_session(profile=profile, session_dir=tmp_path / "session", serial_number="test", + source_urdf=source, protected_inputs={}, records=()) + assert not (tmp_path / "session").exists() diff --git a/src/linkerhand_calibration/test/test_safety_policy.py b/src/linkerhand_calibration/test/test_safety_policy.py new file mode 100644 index 0000000..a13ea3f --- /dev/null +++ b/src/linkerhand_calibration/test/test_safety_policy.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +import pytest + +from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry +from linkerhand_calibration.runtime import SafetyPolicy, SafetySample +from linkerhand_calibration.runtime.adapters import HardwareHealth + + +@pytest.fixture(params=["g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"]) +def profile(request): + return next(item.profile for item in get_default_registry() + if item.profile.key.layout == request.param) + + +def test_normal_lag_coupling_and_low_visual_rate_are_not_safety_inputs(profile) -> None: + decision = SafetyPolicy(profile).evaluate( + SafetySample( + now_seconds=10.0, + feedback_timestamp_seconds=9.5, + command=profile.command.baseline_values, + feedback=tuple(value + (0.01 if profile.command.unit == "rad" else -1) + for value in profile.command.baseline_values), + health=HardwareHealth(True, True), + ) + ) + assert decision.safe + + +def test_stale_feedback_hardware_fault_and_reference_motion_stop(profile) -> None: + policy = SafetyPolicy(profile) + common = dict( + now_seconds=10.0, + command=profile.command.baseline_values, + feedback=profile.command.baseline_values, + health=HardwareHealth(True, True), + ) + assert policy.evaluate( + SafetySample(feedback_timestamp_seconds=8.9, **common) + ).code == "feedback_stale" + assert policy.evaluate( + SafetySample( + feedback_timestamp_seconds=9.9, + **{**common, "health": HardwareHealth(True, True, ("overheat",))}, + ) + ).code == "hardware_fault" + assert policy.evaluate( + SafetySample( + feedback_timestamp_seconds=9.9, + **common, + reference_moved=True, + ) + ).code == "fixed_reference_moved" + + +def test_two_second_no_progress_stops_but_ordinary_lag_does_not(profile) -> None: + policy = SafetyPolicy(profile) + + def sample(now: float, feedback: float) -> SafetySample: + values = list(profile.command.baseline_values) + values[0] = feedback if profile.command.unit == "rad" else 255 - 100 * feedback + command = list(profile.command.baseline_values) + command[0] = 0.8 if profile.command.unit == "rad" else 0 + return SafetySample( + now_seconds=now, + feedback_timestamp_seconds=now, + command=tuple(command), + feedback=tuple(values), + health=HardwareHealth(True, True), + motion_expected=True, + target_channel=0, + target_value=command[0], + ) + + assert policy.evaluate(sample(0.0, 0.0)).safe + assert policy.evaluate(sample(1.0, 0.1)).safe + assert policy.evaluate(sample(2.5, 0.1)).safe + stopped = policy.evaluate(sample(3.1, 0.1)) + assert not stopped.safe + assert stopped.code == "mechanical_stall" diff --git a/src/linkerhand_calibration/test/test_session_execution.py b/src/linkerhand_calibration/test/test_session_execution.py new file mode 100644 index 0000000..4691521 --- /dev/null +++ b/src/linkerhand_calibration/test/test_session_execution.py @@ -0,0 +1,120 @@ +"""Virtual-clock tests use the same effect driver intended for the ROS host.""" + + +import pytest + +from linkerhand_calibration.profiles import load_bundled_hand_profile +from linkerhand_calibration.runtime.execution import SessionExecution +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase +from linkerhand_calibration.runtime.motion_execution import MotionExecution +from linkerhand_calibration.runtime.scan_quality import observation_streams + + +@pytest.mark.parametrize("layout", ["l6_right_8", "o6_right_8", "o12_right_16", "g20_right_19"]) +def test_one_driver_executes_every_profile_and_one_same_speed_retry(layout): + profile = load_bundled_hand_profile(layout) + driver = SessionExecution(profile) + driver.session.device_ready() + driver.session.start() + command = profile.command.baseline_values + rows, stamp, failed_once, swept = [], 0, False, [] + for _ in range(4000): + phase = driver.session.phase + if phase == Phase.FIT: + break + if phase == Phase.REFERENCE_LOCKING: + driver.session.reference_locked() + continue + if phase == Phase.EVALUATE: + driver.evaluate(rows) + continue + motion = driver.motion(command) + assert motion is not None, phase + command = motion.target + if motion.recording: + if motion.phase == "sweep": + swept.append(motion) + # The first direction fails once; no speed reduction or extra retry. + if not failed_once: + failed_once = True + else: + unit = driver.action.scan_unit + task = next(t for t in profile.motion.tasks if t.key == unit.task_key) + for field, joint, spec in observation_streams(profile, task): + for index in range(129 if motion.phase == "sweep" else 3): + stamp += 1 + rows.append({field: joint, "view": spec.view, "task_name": task.key, + "cycle": unit.cycle, "direction": unit.direction, + "attempt": motion.attempt, "image_stamp_ns": stamp, + "sample_phase": motion.phase, "steady_index": motion.steady_index, + "steady_target": motion.target[task.command_index], + f"feedback_{profile.command.unit}": unit.start+(unit.end-unit.start)*index/128}) + driver.motion_complete() + assert driver.session.phase == Phase.FIT + assert len(driver.completed_units) == len(profile.motion.tasks)*8 + assert len(swept) == len(profile.motion.tasks)*8+1 + assert swept[0].speed == swept[1].speed + assert swept[1].attempt == 2 + + +def test_feedback_bias_does_not_turn_completed_travel_into_a_stall(): + profile = load_bundled_hand_profile("o12_right_16") + from linkerhand_calibration.runtime.motion_execution import MotionCommand + from linkerhand_calibration.runtime.safety import SafetyPolicy, SafetySample + from linkerhand_calibration.runtime.adapters import HardwareHealth + start = list(profile.command.baseline_values) + feedback = start.copy() + feedback[1] = -0.023 + target = start.copy() + target[1] = -0.1 + segment = MotionExecution(profile, MotionCommand("prepare", tuple(target), .04), + initial_command=start, initial_feedback=feedback, now=0, identity="one-segment") + policy = SafetyPolicy(profile) + for index in range(1000): + now = index*.02 + command = segment.sample(now) + feedback[1] = -.023+command[1] + segment.observe(feedback, stamp=index, now=now) + assert policy.evaluate(SafetySample(now, now, command, tuple(feedback), HardwareHealth(True, True), + motion_expected=segment.motion_expected, motion_id=segment.identity, motion_goals=segment.goals)).safe + assert segment.arrived(now) + + +def test_duplicate_images_and_missing_secondary_view_fail_after_motion_only(): + profile = load_bundled_hand_profile("o12_right_16") + from linkerhand_calibration.runtime.scan_quality import evaluate_capture_unit + from linkerhand_calibration.runtime.engine import CalibrationEngine + unit = next(u for u in CalibrationEngine(profile).scan_units() if u.task_key == "middle_roll_front") + rows = [{"joint": "middle_mcp_roll", "view": "front", "task_name": unit.task_key, + "cycle": 0, "direction": unit.direction, "attempt": 1, "image_stamp_ns": 1, + "feedback_rad": unit.start}]*100 + quality = evaluate_capture_unit(profile, unit, 1, rows, first_cycle_spans={}) + assert not quality.passed + assert any("observation_joint" in item for item in quality.failures) + assert any("frames=1" in item for item in quality.failures) + + +def test_tiny_baseline_bias_and_mapping_jog_have_different_completion_rules(): + from linkerhand_calibration.runtime.motion_execution import MotionCommand + profile = load_bundled_hand_profile("o12_right_16") + start = list(profile.command.baseline_values) + start[0], start[1] = .021, -.023 + segment = MotionExecution(profile, MotionCommand("baseline", profile.command.baseline_values, .1), + initial_command=start, initial_feedback=start, now=0, identity="baseline") + for n in range(50): + now = n*.1 + segment.sample(now) + segment.observe(start, stamp=n, now=now) + assert not segment.motion_expected + assert segment.arrived(now) + target = list(profile.command.baseline_values) + target[1] = -profile.acquisition.mapping_probe_maximum_rad + jog = MotionExecution(profile, MotionCommand("mapping_probe", tuple(target), .06), + initial_command=profile.command.baseline_values, initial_feedback=start, now=0, identity="jog") + moved = start.copy() + moved[1] -= .02 + for n in range(50): + now = n*.1 + jog.sample(now) + jog.observe(moved, stamp=n, now=now) + assert jog.arrived(now) # Basic movement proves mapping; not an 80% accuracy gate. diff --git a/src/linkerhand_calibration/test/test_standard_correction_plan.py b/src/linkerhand_calibration/test/test_standard_correction_plan.py new file mode 100644 index 0000000..c1068fb --- /dev/null +++ b/src/linkerhand_calibration/test/test_standard_correction_plan.py @@ -0,0 +1,84 @@ +from dataclasses import replace +import hashlib + +import numpy as np +import pytest + +from linkerhand_calibration.core.urdf import build_standard_correction_plan +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel + + +XML = ''' + + + + +''' + + +@pytest.fixture +def source(tmp_path): + path = tmp_path / "original.urdf" + path.write_text(XML) + return path + + +def plan(source, offsets=None, ranges=None, rights=None): + return build_standard_correction_plan(source_urdf=source, + source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(), + zero_offsets_rad=offsets or {"drive": 0.1, "follower": -0.04}, + measured_ranges_output_rad=ranges, + authorized_fields=rights if rights is not None else { + "drive": ("origin.rpy", "limit.lower", "limit.upper"), + "follower": ("origin.rpy", "limit.lower", "limit.upper", "mimic.offset"), + }) + + +def test_zero_origin_both_limits_and_mimic_shift_together(source, tmp_path): + correction = plan(source) + output = correction.write(source, tmp_path / "corrected.urdf") + old, new = UrdfKinematicModel(source), UrdfKinematicModel(output) + assert new.joints["drive"].lower == pytest.approx(-0.1) + assert new.joints["drive"].upper == pytest.approx(0.9) + assert new.joints["follower"].mimic_offset == pytest.approx(0.14) + for cad_angle in np.linspace(0, 1, 81): + for joint in ("drive", "follower"): + before = old.link_transform(joint, zero_offsets={}, joint_angles={"drive": cad_angle}) + after = new.link_transform(joint, zero_offsets={}, joint_angles={"drive": cad_angle - 0.1}) + np.testing.assert_allclose(after, before, atol=1e-12) + + +def test_range_expansion_cannot_hide_a_bad_zero(source): + with pytest.raises(ValueError, match="mechanical range"): + plan(source, ranges={"drive": (0, 1)}) + + +def test_authorization_is_independent_of_the_result(source): + with pytest.raises(ValueError, match="authorization"): + plan(source, rights={"drive": ("origin.rpy",)}) + + +def test_changed_source_cannot_be_patched(source, tmp_path): + correction = plan(source) + source.write_text(XML.replace('effort="1"', 'effort="2"')) + with pytest.raises(ValueError, match="SHA256"): + correction.write(source, tmp_path / "corrected.urdf") + assert not (tmp_path / "corrected.urdf").exists() + + +def test_invalid_mimic_never_materializes_an_output(source, tmp_path): + correction = plan(source) + from linkerhand_calibration.core.urdf import UrdfJointPatch, UrdfPatchSet + correction = replace(correction, patches=UrdfPatchSet({"follower": UrdfJointPatch(mimic_multiplier="3")}), + authorized_fields={"follower": ("mimic.multiplier",)}) + with pytest.raises(ValueError, match="mimic reachable range"): + correction.write(source, tmp_path / "corrected.urdf") + assert not (tmp_path / "corrected.urdf").exists() + + +def test_existing_output_cannot_be_overwritten_or_deleted(source, tmp_path): + output = tmp_path / "corrected.urdf" + output.write_text("user file") + with pytest.raises(ValueError, match="overwrite"): + plan(source).write(source, output) + assert output.read_text() == "user file" diff --git a/src/linkerhand_calibration/test/test_standard_loader.py b/src/linkerhand_calibration/test/test_standard_loader.py new file mode 100644 index 0000000..7c7152f --- /dev/null +++ b/src/linkerhand_calibration/test/test_standard_loader.py @@ -0,0 +1,26 @@ +"""Offline ROS parser check; no SDK or command-topic publisher is started.""" +from pathlib import Path + +import pytest + +from linkerhand_calibration.runtime.artifacts.standard_loader import validate_with_robot_state_publisher + + +def test_actual_standard_parser_loads_isolated_robot_and_rejects_broken_tree(tmp_path): + pytest.importorskip("ament_index_python") + from ament_index_python.packages import get_package_prefix, PackageNotFoundError + try: + prefix = get_package_prefix("robot_state_publisher") + except PackageNotFoundError: + pytest.skip("robot_state_publisher is not installed") + if not (Path(prefix) / "lib/robot_state_publisher/robot_state_publisher").is_file(): + pytest.skip("robot_state_publisher binary is not installed") + source = tmp_path / "virtual.urdf" + source.write_text(''' + + + ''') + validate_with_robot_state_publisher(source) + source.write_text('') + with pytest.raises(ValueError, match="standard_urdf_loader_failed"): + validate_with_robot_state_publisher(source) diff --git a/src/linkerhand_calibration/test/test_standard_urdf_acceptance.py b/src/linkerhand_calibration/test/test_standard_urdf_acceptance.py new file mode 100644 index 0000000..986bff9 --- /dev/null +++ b/src/linkerhand_calibration/test/test_standard_urdf_acceptance.py @@ -0,0 +1,124 @@ +from dataclasses import replace +import math + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from linkerhand_calibration.core.urdf.acceptance import ( + JointHoldout, SerializedJointMapping, validate_standard_urdf_holdout, +) +from linkerhand_calibration.core.urdf.kinematics import ( + UrdfKinematicModel, corrected_mimic_offset, corrected_origin_rpy, +) +from linkerhand_calibration.core.urdf.validate import validate_structural_urdf_diff + + +XML = ''' + + + + +''' + + +@pytest.fixture +def artifacts(tmp_path): + path = tmp_path / "hand.urdf" + path.write_text(XML) + knots = np.linspace(0, 1, 65).tolist() + payload = {"schema_version": 7, "curve_input_domain": "feedback_rad", "joints": { + name: {"motor_index": 0, "curve_input_knots_rad": knots, "angle_rad": knots} + for name in ("drive", "follower")}} + return path, payload + + +def rows(nonlinear=False): + records = [] + for i, q in enumerate(np.linspace(0, 1, 64)): + for name in ("drive", "follower"): + observed = q + (0.18 * math.sin(math.pi * q) if nonlinear and name == "follower" else 0) + # Independent planar forward-kinematics oracle, not the URDF parser. + t = np.eye(4) + a = q if name == "drive" else 2*q + t[:3, :3] = ((math.cos(a), -math.sin(a), 0), (math.sin(a), math.cos(a), 0), (0, 0, 1)) + if name == "follower": + t[:3, 3] = (0.03*math.cos(q), 0.03*math.sin(q), 0) + records.append(JointHoldout(str(i), name, (float(q),), observed, "increasing", base_from_link=tuple(map(tuple, t)))) + return records + + +def test_serialized_urdf_and_independent_geometry_pass(artifacts): + path, payload = artifacts + metrics = validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, + observations=rows(), required_joints=("drive", "follower"), required_pose_joints=("drive", "follower")) + assert all(value.maximum_deg < 1e-10 for value in metrics.values()) + + +def test_perfect_nonlinear_json_cannot_compensate_wrong_urdf(artifacts): + path, payload = artifacts + payload["joints"]["follower"]["angle_rad"] = [q + 0.18*math.sin(math.pi*q) for q in payload["joints"]["follower"]["curve_input_knots_rad"]] + with pytest.raises(ValueError, match="standard_urdf_mimic_not_accurate"): + validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, + observations=rows(nonlinear=True), required_joints=("drive", "follower")) + + +def test_wrong_origin_cannot_pass_correct_angle_curves(artifacts): + path, payload = artifacts + path.write_text(XML.replace('rpy="0 0 0"', 'rpy="0.1 0 0"', 1)) + with pytest.raises(ValueError, match="spatial_holdout_failed"): + validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, + observations=rows(), required_joints=("drive", "follower"), required_pose_joints=("drive",)) + + +@pytest.mark.parametrize("corrupt", ["training", "duplicate", "missing", "nan"]) +def test_holdout_evidence_cannot_be_invented(artifacts, corrupt): + path, payload = artifacts + observations = rows() + if corrupt == "training": + observations[0] = replace(observations[0], cycle=2) + elif corrupt == "duplicate": + observations.append(observations[0]) + elif corrupt == "missing": + observations = [row for row in observations if row.joint == "drive"] + else: + observations[0] = replace(observations[0], observed_rad=math.nan) + with pytest.raises(ValueError): + validate_standard_urdf_holdout(corrected_urdf=path, payload=payload, + observations=observations, required_joints=("drive", "follower")) + + +def test_mimic_resolves_source_on_sibling_branch(artifacts): + path, _ = artifacts + path.write_text(XML.replace('', '')) + model = UrdfKinematicModel(path) + pose = model.link_transform("follower", zero_offsets={}, joint_angles={"drive": 0.4, "follower": 0.9}) + assert Rotation.from_matrix(pose[:3, :3]).as_rotvec()[2] == pytest.approx(0.4) + + +def test_zero_composes_about_local_axis_and_mimic_coordinates(artifacts): + path, _ = artifacts + path.write_text(XML.replace('rpy="0 0 0"', 'rpy="0.2 -0.3 0.4"', 1).replace('axis xyz="0 0 1"', 'axis xyz="1 2 3"', 1)) + joint = UrdfKinematicModel(path).joints["drive"] + output = corrected_origin_rpy(joint, 0.17) + expected = Rotation.from_euler("xyz", (0.2, -0.3, 0.4)) * Rotation.from_rotvec(np.array((1, 2, 3))/math.sqrt(14)*0.17) + assert np.allclose(Rotation.from_euler("xyz", output).as_matrix(), expected.as_matrix()) + for parent in np.linspace(-0.2, 0.8, 20): + b = corrected_mimic_offset(1.2, 0.08, 0.1, -0.03) + assert 1.2*parent + b - 0.03 == pytest.approx(1.2*(parent+0.1)+0.08) + + +def test_nested_origin_is_not_authorized_joint_origin(artifacts, tmp_path): + path, _ = artifacts + source = XML.replace('', '', 1) + path.write_text(source) + changed = tmp_path / "changed.urdf" + changed.write_text(source.replace(' list[SweepItem]: - return [ - SweepItem(spec, cycle, direction) - for spec in SWEEP_SPECS - for cycle in range(3) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) - ] - - -def _validated_zero_result( - endpoint_offsets: dict[str, float], -) -> SimpleNamespace: - all_offsets = { - name: float(endpoint_offsets.get(name, 0.0)) - for name in RIGHT_19_HAND_PROFILE.active_joints - } - return SimpleNamespace( - passed=True, - direct_offsets_rad=dict(all_offsets), - all_active_offsets_rad=dict(all_offsets), - offset_confidence_half_width_rad={}, - cycle_offsets_rad={}, - training_cycles=(0, 1, 2), - validation_cycle=3, - axis_line_rms_m=0.0001, - validation_line_error_by_joint_m={}, - observability_rank=22, - observability_parameter_count=22, - observability_condition_number=100.0, - offset_covariance_rad2={}, - ) - - -def test_finalize_publishes_the_frozen_validated_endpoint_state( - tmp_path, monkeypatch -) -> None: - endpoint_offsets = { - name: 0.001 * (index + 1) - for index, name in enumerate( - sorted(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) - ) - } - zero_result = _validated_zero_result(endpoint_offsets) - session_dir = tmp_path / "20260826_100727" - session_dir.mkdir() - source_urdf = tmp_path / "source.urdf" - source_urdf.write_text("", encoding="utf-8") - extrinsics = tmp_path / "extrinsics.yaml" - extrinsics.write_text("test: true\n", encoding="utf-8") - corrected_urdf = tmp_path / "corrected.urdf" - captured: dict[str, object] = {} - - def fake_write_zero_corrected_urdf(**kwargs): - captured.update(kwargs) - corrected_urdf.write_text("", encoding="utf-8") - return corrected_urdf - - monkeypatch.setattr( - three_camera_node, - "write_zero_corrected_urdf", - fake_write_zero_corrected_urdf, - ) - monkeypatch.setattr( - three_camera_node, - "build_compact_payload", - lambda **kwargs: {"passed": kwargs["passed"]}, - ) - def fake_atomic_write_json(path, payload): - if str(path).endswith("_urdf_correction_input.json"): - path.write_text( - json.dumps(payload, ensure_ascii=False, indent=2) + "\n", - encoding="utf-8", - ) - return - captured.update({"final_path": path, "payload": payload}) - - monkeypatch.setattr( - three_camera_node, - "atomic_write_json", - fake_atomic_write_json, - ) - node = SimpleNamespace( - zero_result=zero_result, - standalone_thumb_calibration=False, - validated_endpoint_zero_offsets_rad=dict(endpoint_offsets), - validation_errors_rad=[], - combination_validation_enabled=False, - combination_validation_completed=False, - validation_enabled=False, - fit_quality_passed=True, - session_dir=session_dir, - profile=RIGHT_19_HAND_PROFILE, - zero_profile=get_zero_calibration_profile("right", "g20_right_19"), - source_urdf_path=source_urdf, - corrected_urdf_output_dir=tmp_path, - serial_number="G20_RIGHT_001", - measured_fits={}, - baseline_command=THREE_CAMERA_BASELINE_COMMAND, - hand_type="right", - camera_extrinsics_file=extrinsics, - final_path=session_dir / "calibration.json", - completed_payload=None, - state="RETURN_BASELINE", - reason="finalize", - get_logger=lambda: SimpleNamespace(info=lambda message: None), - ) - - G20ThreeCameraCalibrationNode._finalize(node) - - assert captured["endpoint_anchored_offsets_rad"] == endpoint_offsets - assert captured["payload"] == {"passed": True} - assert node.state == STATE_COMPLETE - assert node.corrected_urdf_path == corrected_urdf - - -def test_endpoint_state_must_match_the_validated_zero_solution() -> None: - endpoint_offsets = { - name: 0.001 - for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - } - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - zero_result=_validated_zero_result(endpoint_offsets), - validated_endpoint_zero_offsets_rad={ - name: value - for name, value in endpoint_offsets.items() - if name != "index_pip" - }, - ) - - with pytest.raises( - RuntimeError, match="validated_endpoint_zero_state_incomplete" - ): - G20ThreeCameraCalibrationNode._endpoint_zero_offsets_for_publication( - node - ) - - -def test_refit_invalidates_all_derived_calibration_artifacts() -> None: - node = SimpleNamespace( - measured_fits={"index_pip": object()}, - axis_measurements=[object()], - zero_result=object(), - validated_endpoint_zero_offsets_rad={"index_pip": 0.01}, - corrected_urdf_path=object(), - fit_quality_passed=True, - validation_errors_rad=[0.01], - cross_view_roll_metrics={"index_mcp_roll": {"rms": 0.01}}, - validation_only_fits={"index_mcp_roll_side": object()}, - joint_dynamic_diagnostics={"index_pip": {"holdout": 0.01}}, - ) - - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(node) - - assert node.measured_fits == {} - assert node.axis_measurements == [] - assert node.zero_result is None - assert node.validated_endpoint_zero_offsets_rad == {} - assert node.corrected_urdf_path is None - assert node.fit_quality_passed is False - assert node.validation_errors_rad == [] - assert node.cross_view_roll_metrics == {} - assert node.validation_only_fits == {} - assert node.joint_dynamic_diagnostics == {} - - -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) * 8 - for spec_index, spec in enumerate(RIGHT_19_HAND_PROFILE.sweep_specs): - 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 - ] == [ - *[ - (False, cycle, direction) - for cycle in range(4) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ], - ] - transitions = [ - _sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right) - for left, right in zip(task, task[1:]) - ] - assert transitions == [ - "immediate_reverse", - "cycle_reset", - "immediate_reverse", - "cycle_reset", - "immediate_reverse", - "cycle_reset", - "immediate_reverse", - ] - task_boundaries = [ - (plan[index], plan[index + 1]) - for index in range(7, len(plan) - 1, 8) - ] - assert all( - _sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right) - == "task_change" - for left, right in task_boundaries - ) - - -def test_right_19_complete_plan_has_no_unobserved_immediate_handoff() -> None: - """Every continuous boundary inherits the endpoint it already observed.""" - plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4) - immediate_boundaries = 0 - cycle_resets = 0 - task_changes = 0 - - for completed, following in zip(plan, plan[1:]): - transition = _sweep_plan_transition( - RIGHT_19_HAND_PROFILE, completed, following - ) - if transition == "cycle_reset": - cycle_resets += 1 - continue - if transition == "task_change": - task_changes += 1 - continue - - immediate_boundaries += 1 - endpoint_state = list( - build_calibration_motion_command( - completed.spec, - completed.target_u8, - baseline=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - ) - ) - terminal_frames = [ - _frame(view, endpoint_state) - for view in _sweep_views( - RIGHT_19_HAND_PROFILE, completed.spec - ) - ] - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - sweep_frames=terminal_frames, - carried_sweep_start_frames=[], - preflight_frames=30, - raw_path=None, - synchronised_endpoint_tolerance_margin_u8=1.0, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, - ) - - carried = ( - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - node, completed, following - ) - ) - - assert carried == len(terminal_frames) - assert node.carried_sweep_start_frames == terminal_frames - assert _frames_cover_sweep_joints( - node.carried_sweep_start_frames, - following.spec, - minimum_per_joint=1, - ) - - # 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 - - -def test_right_19_initializes_pnp_once_per_normal_task() -> None: - plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4) - reset_items = [ - item - for item in plan - if _requires_pnp_tracker_reset_for_sweep( - RIGHT_19_HAND_PROFILE, - item, - is_fit_retry=False, - ) - ] - - assert len(reset_items) == len(RIGHT_19_HAND_PROFILE.sweep_specs) - 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 - ) - - -def test_pnp_reset_policy_preserves_legacy_and_retry_recovery() -> None: - right_spec = RIGHT_19_HAND_PROFILE.sweep_specs[0] - legacy_spec = RIGHT_HAND_PROFILE.sweep_specs[0] - - assert not _requires_pnp_tracker_reset_for_sweep( - RIGHT_19_HAND_PROFILE, - SweepItem(right_spec, 2, DIRECTION_DECREASING), - is_fit_retry=False, - ) - assert _requires_pnp_tracker_reset_for_sweep( - RIGHT_19_HAND_PROFILE, - SweepItem(right_spec, 2, DIRECTION_DECREASING), - is_fit_retry=True, - ) - assert not _requires_pnp_tracker_reset_for_sweep( - RIGHT_19_HAND_PROFILE, - SweepItem(right_spec, 2, DIRECTION_DECREASING), - is_fit_retry=True, - preserve_retry_continuity=True, - ) - assert _requires_pnp_tracker_reset_for_sweep( - RIGHT_HAND_PROFILE, - SweepItem(legacy_spec, 2, DIRECTION_DECREASING), - is_fit_retry=False, - ) - - -def test_task_roles_remain_owned_during_intercycle_reset() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "ring_pitch_side" - ) - view = "side" - all_roles = tuple(RIGHT_19_HAND_PROFILE.view_tags[view]) - runtime = SimpleNamespace( - preflight_roles=all_roles, - roles=all_roles, - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={view: runtime}, - active_combination_validation=None, - active_sweep=None, - active_validation=None, - retry_sweep_spec=None, - pnp_task_spec=spec, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, view - ) - - assert required == ("side_base", "ring_pip") - assert required != all_roles - - -def _linear_curve_fit(offset_scale_rad: float = 0.0) -> JointCurveFit: - base = np.asarray( - [0.5 * (127 - command) / 127 for command in range(256)], - dtype=float, - ) - shape = np.abs(np.arange(256, dtype=float) - 127.0) / 128.0 - values = tuple(float(value) for value in base + offset_scale_rad * shape) - return JointCurveFit(values, values, values, {}, 0.0, 0.0, {}) - - -def test_cross_view_curve_stable_four_cycle_bias_is_not_rescanned() -> None: - primary = _linear_curve_fit() - biased = _linear_curve_fit(math.radians(1.8)) - - failure = _cross_view_curve_failure( - "pinky_mcp_roll", - "pinky_mcp_roll_side", - primary, - biased, - scope="all_cycles", - maximum_rms_difference_rad=math.radians(1.0), - maximum_branch_gap_difference_rad=math.radians(0.5), - cycle_fits=[(primary, biased)] * 4, - ) - - assert failure is not None - assert failure["systematic"] is True - assert failure["quality_source_joints"] == ["pinky_mcp_roll_side"] - assert "cycle" not in failure - assert _fit_failure_is_systematic([failure], 4) is True - - -def test_cross_view_curve_single_bad_cycle_localizes_side_retry() -> None: - primary = _linear_curve_fit() - good = _linear_curve_fit(math.radians(0.6)) - bad = _linear_curve_fit(math.radians(1.9)) - - failure = _cross_view_curve_failure( - "middle_mcp_roll", - "middle_mcp_roll_side", - primary, - bad, - scope="all_cycles", - maximum_rms_difference_rad=math.radians(1.0), - maximum_branch_gap_difference_rad=math.radians(0.5), - cycle_fits=[(primary, good)] * 3 + [(primary, bad)], - ) - - assert failure is not None - assert failure["cycle"] == 4 - assert failure.get("systematic", False) is False - - -def _frame(view: str, state_u8: list[float]) -> FrameObservation: - return FrameObservation( - stamp_ns=1, - received_at=2.0, - view=view, - state_u8=tuple(state_u8), - state_sync_error_ns=0, - joint_vectors_xyz_m={}, - image_vectors_xy_px={}, - joint_quaternions_xyzw={}, - parent_poses_common={}, - child_poses_common={}, - joint_reprojection_error_px={}, - ) - - -def _joint_frame( - view: str, state_u8: list[float], joint_name: str -) -> FrameObservation: - identity_pose = { - "translation_xyz_m": [0.0, 0.0, 0.0], - "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], - } - return replace( - _frame(view, state_u8), - joint_vectors_xyz_m={joint_name: (0.02, 0.0, 0.0)}, - image_vectors_xy_px={joint_name: (20.0, 0.0)}, - joint_quaternions_xyzw={joint_name: (0.0, 0.0, 0.0, 1.0)}, - parent_poses_common={joint_name: identity_pose}, - child_poses_common={joint_name: identity_pose}, - joint_reprojection_error_px={joint_name: 0.05}, - ) - - -def test_right_roll_uses_one_motion_with_two_camera_observations() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - state = [255.0] * 20 - front = _joint_frame("front", state, "pinky_mcp_roll") - side = _joint_frame("side", state, "pinky_mcp_roll_side") - - assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("front", "side") - assert not _frames_cover_sweep_joints([front] * 3, spec, 3) - assert _frames_cover_sweep_joints([front] * 3 + [side] * 3, spec, 3) - - -def test_right_roll_requires_both_views_task_local_tags() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={ - "front": SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["front"]), - preflight_roles=("front_base",), - ), - "side": SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["side"]), - preflight_roles=("side_base",), - ), - }, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - active_combination_validation=None, - ) - - assert G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "front" - ) == ("front_base", "pinky_roll") - assert G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) == ("side_base", "pinky_pip") - - -def test_thumb_yaw_top_axis_observer_does_not_change_front_sweep_contract() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_pitch_front" - ) - observer = _palm_axis_observer_for_sweep( - RIGHT_19_HAND_PROFILE, spec, "top" - ) - assert observer is not None - assert observer.model_joint == "thumb_cmc_pitch" - assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("front",) - - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={ - view: SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags[view]), - preflight_roles=(f"{view}_base",), - ) - for view in ("front", "side", "top") - }, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - active_combination_validation=None, - ) - assert G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "top" - ) == ("top_base",) - - -def test_pitch_task_does_not_add_a_front_direction_observer() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_pitch_side" - ) - - assert _sweep_views(RIGHT_19_HAND_PROFILE, spec) == ("side",) - observer = _palm_axis_observer_for_sweep( - RIGHT_19_HAND_PROFILE, spec, "front" - ) - assert observer is None - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={ - view: SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags[view]), - preflight_roles=(f"{view}_base",), - ) - for view in ("front", "side", "top") - }, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - active_combination_validation=None, - ) - assert G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "front" - ) == ("front_base",) - assert G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) == ("side_base", "pinky_pip") - - -def test_disabled_palm_axis_side_channel_is_not_persisted(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_pitch_side" - ) - observer = _palm_axis_observer_for_sweep( - RIGHT_19_HAND_PROFILE, spec, "front" - ) - assert observer is None - observer = PalmAxisObserver( - source_name="pinky_mcp_pitch_front_axis", - task_name=spec.key, - view="front", - parent_role="front_base", - child_role="pinky_roll", - model_joint="pinky_mcp_pitch", - motor_index=9, - ) - item = SweepItem(spec, 0, DIRECTION_DECREASING) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - active_sweep=item, - sweep_attempts={spec.key: 1}, - palm_axis_records_by_source={observer.source_name: []}, - raw_path=tmp_path / "raw_samples.jsonl", - ) - selected = { - "front_base": SquareTagPose( - (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 - ), - "pinky_roll": SquareTagPose( - tuple( - Rotation.from_euler("y", 12.0, degrees=True).as_quat() - ), - (0.02, 0.0, 1.0), - 0.2, - ), - } - state = list(THREE_CAMERA_BASELINE_COMMAND) - state[observer.motor_index] = 230.0 - - G20ThreeCameraCalibrationNode._record_palm_axis_sample( - node, - observer, - selected, - np.eye(4), - state, - sync_error_ns=100_000, - stamp_ns=123, - ) - - records = node.palm_axis_records_by_source[observer.source_name] - assert len(records) == 1 - assert records[0]["kind"] == "palm_axis_sample" - assert "joint" not in records[0] - assert records[0]["command_u8"] == 230 - G20ThreeCameraCalibrationNode._persist_palm_axis_samples(node, item) - assert not node.raw_path.exists() - - -def test_multiview_roll_uses_locked_front_base_but_live_moving_tag() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "middle_roll_multiview" - ) - locked_pose = SquareTagPose( - (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 - ) - front = SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["front"]), - preflight_roles=("front_base",), - locked_base_pose=locked_pose, - locked_base_center_xy_px=(100.0, 200.0), - locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1), - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={"front": front}, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - active_combination_validation=None, - ) - node._locked_base_role_for_active_capture = lambda view: ( - G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( - node, view - ) - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "front" - ) - live = G20ThreeCameraCalibrationNode._live_required_roles_for_view( - node, "front", required - ) - - assert required == ("front_base", "middle_roll") - assert live == ("middle_roll",) - - -@pytest.mark.parametrize( - "task_key", - ( - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - "thumb_cmc_yaw_top", - ), -) -def test_thumb_top_tasks_freeze_tag8_but_keep_it_live(task_key: str) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == task_key - ) - top = SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["top"]), - preflight_roles=("top_base", "thumb_yaw"), - locked_base_pose=SquareTagPose( - (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 - ), - locked_base_center_xy_px=(100.0, 200.0), - locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1), - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={"top": top}, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - active_combination_validation=None, - ) - node._locked_base_role_for_active_capture = lambda view: ( - G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( - node, view - ) - ) - - assert _sweep_uses_locked_base_reference( - RIGHT_19_HAND_PROFILE, spec, "top" - ) - assert ( - G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( - node, "top" - ) - == "top_base" - ) - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "top" - ) - live = G20ThreeCameraCalibrationNode._live_required_roles_for_view( - node, "top", required - ) - assert "top_base" in required - assert "top_base" in live - - -def test_fixed_base_corner_drift_uses_all_four_ordered_corners() -> None: - reference = np.asarray( - [[10.0, 10.0], [30.0, 10.0], [30.0, 30.0], [10.0, 30.0]] - ) - subpixel_noise = reference + np.asarray( - [[0.1, -0.1], [0.2, 0.0], [0.0, 0.2], [-0.1, 0.1]] - ) - moved = reference.copy() - moved[2] += [1.5, 2.0] - - assert _maximum_corner_drift_px( - reference, subpixel_noise - ) < 0.3 - assert _maximum_corner_drift_px(reference, moved) == pytest.approx(2.5) - - -def test_resume_start_position_requires_all_fixed_tags_to_match() -> None: - reference = ( - (10.0, 10.0), - (30.0, 10.0), - (30.0, 30.0), - (10.0, 30.0), - ) - rows = [ - { - "kind": "fixed_base_reference_locked", - "view": view, - "corner_reference_xy": reference, - } - for view in ("front", "side", "top") - ] - current = { - "front": np.asarray(reference) + [0.2, -0.1], - "side": np.asarray(reference) + [4.0, 0.0], - "top": None, - } - - drift, changed, unverifiable = ( - _resume_fixed_base_position_compatibility(rows, current, 2.0) - ) - - assert drift["front"] < 0.3 - assert drift["side"] == pytest.approx(4.0) - assert changed == ("side",) - assert unverifiable == ("top",) - - -def test_side_only_finger_task_keeps_occluded_inactive_front_base_locked() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "middle_pitch_side" - ) - front = SimpleNamespace( - roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["front"]), - preflight_roles=("front_base",), - locked_base_pose=SquareTagPose( - (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 - ), - locked_base_center_xy_px=(100.0, 200.0), - locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1), - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - views={"front": front}, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - active_combination_validation=None, - ) - - assert ( - G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( - node, "front" - ) - == "front_base" - ) - - -def test_hidden_locked_base_uses_baseline_quality_without_becoming_live() -> None: - locked_pose = SquareTagPose( - (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.17 - ) - moving_pose = SquareTagPose( - (0.0, 0.0, 0.0, 1.0), (0.02, 0.0, 1.0), 0.23 - ) - live = {"middle_roll": TagQuality(0, 90.0, 32.0)} - - combined = _selected_pose_qualities( - {"front_base": locked_pose, "middle_roll": moving_pose}, - live, - locked_base_role="front_base", - locked_base_quality=TagQuality(0, 100.0, 35.0, 0.11), - ) - - assert set(live) == {"middle_roll"} - assert combined["front_base"].reprojection_error_px == 0.17 - assert combined["middle_roll"].reprojection_error_px == 0.23 - - -def test_baseline_preflight_locks_robust_fixed_tag_reference(tmp_path) -> None: - observations = deque(maxlen=60) - for index in range(30): - observations.append( - ( - SquareTagPose( - (0.0, 0.0, 0.0, 1.0), - (0.001 * (index % 2), 0.0, 1.0), - 0.1, - ), - (100.0 + index % 2, 200.0), - TagQuality(0, 100.0, 30.0, 0.1), - ) - ) - views = { - view: SimpleNamespace( - fixed_base_observations=deque(observations, maxlen=60), - fixed_base_corner_observations=deque( - ( - np.asarray( - [ - [90.0 + index % 2, 190.0], - [110.0 + index % 2, 190.0], - [110.0 + index % 2, 210.0], - [90.0 + index % 2, 210.0], - ] - ) - for index in range(30) - ), - maxlen=60, - ), - locked_base_pose=None, - locked_base_center_xy_px=None, - locked_base_corners_xy=None, - locked_base_quality=None, - view_tags={ - {"front": "front_base", "side": "side_base", "top": "top_base"}[ - view - ]: {"front": 0, "side": 4, "top": 8}[view] - }, - ) - for view in ("front", "side", "top") - } - node = SimpleNamespace( - preflight_frames=60, - views=views, - raw_path=tmp_path / "raw_samples.jsonl", - ) - - assert G20ThreeCameraCalibrationNode._lock_fixed_base_references(node) - assert views["front"].locked_base_pose is not None - assert views["front"].locked_base_center_xy_px == (100.5, 200.0) - assert views["front"].locked_base_corners_xy == ( - (90.5, 190.0), - (110.5, 190.0), - (110.5, 210.0), - (90.5, 210.0), - ) - events = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert {event["tag_id"] for event in events} == {0, 4, 8} - - -def test_right_roll_checkpoint_persists_each_camera_independently( - tmp_path, -) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - item = SweepItem(spec, 0, DIRECTION_DECREASING) - state = [255.0] * 20 - state[spec.motor_index] = 224.0 - frames = ( - [_joint_frame("front", state, "pinky_mcp_roll") for _ in range(3)] - + [ - _joint_frame("side", state, "pinky_mcp_roll_side") - for _ in range(3) - ] - ) - records = {name: [] for name in spec.joints} - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - sweep_attempts={spec.key: 1}, - command_records_by_joint=records, - raw_path=tmp_path / "raw_samples.jsonl", - ) - - G20ThreeCameraCalibrationNode._record_command_checkpoint( - node, item, 224, frames - ) - - assert [records[name][0]["view"] for name in spec.joints] == [ - "front", - "side", - ] - assert all(records[name][0]["valid_frames"] == 3 for name in spec.joints) - - -def test_final_steady_checkpoint_unlatches_checkpoint_mode() -> None: - node = SimpleNamespace( - sweep_checkpoint_commands=deque(), - sweep_checkpoint_target_u8=0, - sweep_checkpoint_mode=True, - ) - - advanced = G20ThreeCameraCalibrationNode._publish_next_checkpoint( - node, 1.0 - ) - - assert advanced is False - assert node.sweep_checkpoint_target_u8 is None - assert node.sweep_checkpoint_mode is False - - -def test_steady_checkpoint_accepts_stable_command_feedback_deadband() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "thumb_cmc_yaw_top" - ) - node = SimpleNamespace( - latest_state_u8=(), - baseline_command=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - endpoint_tolerance_u8=2.0, - thumb_yaw_zero_endpoint_tolerance_u8=4.0, - right_thumb_yaw_255_endpoint_tolerance_u8=5.0, - pinky_pip_zero_endpoint_tolerance_u8=5.0, - steady_checkpoint_command_feedback_tolerance_u8=8.0, - steady_checkpoint_maximum_feedback_range_u8=2.0, - ) - node._endpoint_tolerance_for_spec = lambda selected, command: ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, selected, command - ) - ) - state = list( - build_calibration_motion_command( - spec, - 32, - baseline=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - ) - ) - state[10] = 35.0 - - assert not G20ThreeCameraCalibrationNode._motion_command_reached( - node, spec, 32, tuple(state) - ) - assert G20ThreeCameraCalibrationNode._steady_checkpoint_reached( - node, spec, 32, tuple(state) - ) - - state[10] = 41.0 - assert not G20ThreeCameraCalibrationNode._steady_checkpoint_reached( - node, spec, 32, tuple(state) - ) - - -def test_steady_checkpoint_requires_feedback_to_stop_moving() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "thumb_cmc_yaw_top" - ) - node = SimpleNamespace( - steady_checkpoint_maximum_feedback_range_u8=2.0 - ) - stable_frames = [] - moving_frames = [] - for value in (35.0, 35.5, 35.0): - state = [255.0] * 20 - state[10] = value - stable_frames.append(_frame("top", state)) - for value in (38.0, 36.0, 35.0): - state = [255.0] * 20 - state[10] = value - moving_frames.append(_frame("top", state)) - - assert G20ThreeCameraCalibrationNode._steady_checkpoint_feedback_is_stable( - node, spec, stable_frames - ) - assert not G20ThreeCameraCalibrationNode._steady_checkpoint_feedback_is_stable( - node, spec, moving_frames - ) - - -def test_right_thumb_pitch_requires_only_front_base_and_thumb_tag() -> None: - profile = RIGHT_HAND_PROFILE - spec = next(item for item in profile.sweep_specs if item.motor_index == 0) - runtime = SimpleNamespace( - roles=tuple(profile.view_tags["front"]), - preflight_roles=profile.preflight_view_roles["front"], - ) - node = SimpleNamespace( - profile=profile, - views={"front": runtime}, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "front" - ) - - assert required == ("front_base", "thumb_cmc") - - -def test_right_side_preflight_requires_only_pinky_chain_tags() -> None: - profile = RIGHT_HAND_PROFILE - runtime = SimpleNamespace( - roles=tuple(profile.view_tags["side"]), - preflight_roles=profile.preflight_view_roles["side"], - ) - node = SimpleNamespace( - profile=profile, - views={"side": runtime}, - active_sweep=None, - active_validation=None, - retry_sweep_spec=None, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) - - assert required == ("side_base", "pinky_mcp", "pinky_pip", "pinky_dip") - - -def test_right_19_preflight_requires_only_fixed_palm_tag() -> None: - profile = RIGHT_19_HAND_PROFILE - runtime = SimpleNamespace( - roles=tuple(profile.view_tags["side"]), - preflight_roles=profile.preflight_view_roles["side"], - ) - node = SimpleNamespace( - profile=profile, - views={"side": runtime}, - active_sweep=None, - active_validation=None, - retry_sweep_spec=None, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) - - assert required == ("side_base",) - - -def test_right_19_combination_allows_occluded_neighbouring_fingers() -> None: - profile = RIGHT_19_HAND_PROFILE - runtime = SimpleNamespace( - roles=tuple(profile.view_tags["side"]), - preflight_roles=profile.preflight_view_roles["side"], - ) - node = SimpleNamespace( - profile=profile, - views={"side": runtime}, - active_combination_validation=SimpleNamespace(name="index_middle"), - active_sweep=None, - active_validation=None, - retry_sweep_spec=None, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) - observable = _combination_observable_joints( - profile, - "side", - ("side_base", "index_pip", "index_dip"), - ) - - assert required == ("side_base",) - assert observable == ("index_pip", "index_dip") - - -def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item - for item in profile.sweep_specs - if item.task_name == "index_pip_side" - ) - runtime = SimpleNamespace( - roles=tuple(profile.view_tags["side"]), - preflight_roles=profile.preflight_view_roles["side"], - ) - node = SimpleNamespace( - profile=profile, - views={"side": runtime}, - active_combination_validation=None, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) - - assert required == ("side_base", "index_pip", "index_dip") - - -def test_group_pnp_failure_event_persists_candidate_boundary_once( - tmp_path, -) -> None: - profile = RIGHT_19_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs if item.key == "index_pip_side" - ) - state_u8 = [255.0] * 20 - state_u8[spec.motor_index] = 195.0 - runtime = SimpleNamespace( - name="side", - tracker=SimpleNamespace( - last_candidate_diagnostics_by_role={ - "index_pip": { - "solved_candidate_count": 2, - "reprojection_candidate_count": 2, - "independent_tilt_candidate_count": 2, - }, - "index_dip": { - "solved_candidate_count": 2, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "minimum_reprojection_error_px": 1.7, - }, - } - ), - latest_group_missing_candidate_roles=("index_dip",), - last_pnp_diagnostic_signature=None, - view_tags=profile.view_tags["side"], - intrinsics_sha256="side-intrinsics", - camera_matrix=np.eye(3), - ) - node = SimpleNamespace( - profile=profile, - active_sweep=SweepItem( - spec, -1, DIRECTION_DECREASING, precheck=True - ), - state=STATE_SWEEP, - latest_state_u8=tuple(state_u8), - raw_path=tmp_path / "raw_samples.jsonl", - ) - arguments = dict( - runtime=runtime, - stamp_ns=123, - required_roles=("side_base", "index_pip", "index_dip"), - pose_roles=("index_pip", "index_dip"), - corners_by_role={ - "index_pip": np.zeros((4, 2)), - "index_dip": np.ones((4, 2)), - }, - qualities={ - "index_pip": TagQuality(0, 80.0, 40.0), - "index_dip": TagQuality(0, 70.0, 35.0), - }, - matched_tracking=(tuple(state_u8), 2_000_000), - selected=None, - pnp_rejections={ - "index_dip": "no_pose_within_reprojection_or_tilt_limit" - }, - group_pnp_reason="group_missing_pose_candidates", - ) - - G20ThreeCameraCalibrationNode._record_group_pnp_candidate_event( - node, **arguments - ) - G20ThreeCameraCalibrationNode._record_group_pnp_candidate_event( - node, **arguments - ) - - rows = [ - json.loads(line) - for line in node.raw_path.read_text(encoding="utf-8").splitlines() - ] - assert len(rows) == 1 - assert rows[0]["kind"] == "group_pnp_candidate_event" - assert rows[0]["feedback_u8"] == 195.0 - assert rows[0]["group_missing_candidate_roles"] == ["index_dip"] - assert rows[0]["candidate_diagnostics"]["index_dip"][ - "reprojection_candidate_count" - ] == 0 - - -def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None: - profile = RIGHT_HAND_PROFILE - spec = next( - item for item in profile.sweep_specs if item.motor_index == 19 - ) - runtime = SimpleNamespace( - roles=tuple(profile.view_tags["side"]), - preflight_roles=profile.preflight_view_roles["side"], - ) - node = SimpleNamespace( - profile=profile, - views={"side": runtime}, - active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), - active_validation=None, - retry_sweep_spec=None, - ) - - required = G20ThreeCameraCalibrationNode._required_roles_for_view( - node, "side" - ) - - assert required == ( - "side_base", - "pinky_mcp", - "pinky_pip", - "pinky_dip", - ) - - -def test_prepare_sweep_retains_synchronised_start_endpoint_frame() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) - item = SweepItem(spec, 2, DIRECTION_DECREASING) - baseline = [255] * 20 - endpoint_state = list(baseline) - endpoint_state[6] = 253.0 - endpoint_state[7:10] = [0.0, 0.0, 0.0] - outside_state = list(endpoint_state) - outside_state[6] = 252.0 - node = SimpleNamespace( - state=STATE_PREPARE_SWEEP, - active_sweep=item, - baseline_command=tuple(baseline), - endpoint_tolerance_u8=2.0, - preflight_frames=30, - latest_state_u8=tuple(endpoint_state), - sweep_start_frames=[], - ) - node._motion_command_reached = lambda selected, command, state=None: ( - G20ThreeCameraCalibrationNode._motion_command_reached( - node, selected, command, state - ) - ) - node._endpoint_tolerance_for_spec = lambda selected, endpoint: ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, selected, endpoint - ) - ) - - endpoint_frame = _frame("front", endpoint_state) - G20ThreeCameraCalibrationNode._accept_frame(node, endpoint_frame) - G20ThreeCameraCalibrationNode._accept_frame( - node, _frame("front", outside_state) - ) - - assert node.sweep_start_frames == [endpoint_frame] - - -def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) - item = SweepItem(spec, 2, DIRECTION_DECREASING) - baseline = [255] * 20 - endpoint_state = list(baseline) - endpoint_state[6] = 253.0 - endpoint_state[7:10] = [0.0, 0.0, 0.0] - endpoint_frame = _frame("front", endpoint_state) - published: list[list[int]] = [] - node = SimpleNamespace( - active_sweep=item, - baseline_command=tuple(baseline), - sweep_frames=[], - sweep_start_frames=[endpoint_frame], - _publish_command=lambda command: published.append(command), - _motion_command_error_u8=lambda selected, command: 255.0, - _reset_motion_progress=lambda now, error: None, - ) - - G20ThreeCameraCalibrationNode._begin_active_sweep(node, 10.0) - - assert node.state == STATE_SWEEP - assert node.sweep_frames == [endpoint_frame] - assert node.sweep_start_frames == [] - assert node.sweep_last_valid_at == 10.0 - assert published[0][6] == 0 - assert published[0][7:10] == [0, 0, 0] - - -def test_immediate_reverse_reuses_proven_terminal_endpoint_frames(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 1 - ) - completed = SweepItem( - spec, -1, DIRECTION_DECREASING, precheck=True - ) - following = SweepItem( - spec, -1, DIRECTION_INCREASING, precheck=True - ) - endpoint_state = list( - build_calibration_motion_command( - spec, - 0, - baseline=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - ) - ) - endpoint_state[1] = 2.5 - outside_state = list(endpoint_state) - outside_state[1] = 3.01 - endpoint_frame = _frame("side", endpoint_state) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - sweep_frames=[endpoint_frame, _frame("side", outside_state)], - carried_sweep_start_frames=[], - preflight_frames=30, - raw_path=tmp_path / "raw_samples.jsonl", - synchronised_endpoint_tolerance_margin_u8=1.0, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, - ) - - count = ( - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - node, completed, following - ) - ) - - assert count == 1 - assert node.carried_sweep_start_frames == [endpoint_frame] - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "sweep_endpoint_frames_carried" - assert event["endpoint_u8"] == 0 - assert event["to_direction"] == DIRECTION_INCREASING - - -def test_right_19_roll_sweep_stops_at_settled_baseline_first() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - item = SweepItem(spec, 0, DIRECTION_DECREASING) - baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) - endpoint_state = list(baseline) - endpoint_state[9] = 255.0 - published: list[list[int]] = [] - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - active_sweep=item, - baseline_command=baseline, - sweep_frames=[], - sweep_start_frames=[_frame("front", endpoint_state)], - sweep_baseline_frames=[], - _publish_command=lambda command: published.append(command), - _motion_command_error_u8=lambda selected, command: 128.0, - _reset_motion_progress=lambda now, error: None, - ) - - G20ThreeCameraCalibrationNode._begin_active_sweep(node, 10.0) - - assert node.sweep_baseline_pending is True - assert node.sweep_baseline_hold_since is None - assert published[0][9] == 127 - - -def test_dedicated_baseline_buffer_ignores_moving_arrival_frame() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - item = SweepItem(spec, 0, DIRECTION_DECREASING) - baseline_state = build_calibration_motion_command( - spec, - 127, - baseline=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - ) - moving_state = list(baseline_state) - moving_state[9] = 130.0 - node = SimpleNamespace( - state=STATE_SWEEP, - profile=RIGHT_19_HAND_PROFILE, - active_sweep=item, - baseline_command=THREE_CAMERA_BASELINE_COMMAND, - endpoint_tolerance_u8=2.0, - latest_state_u8=tuple(baseline_state), - sweep_frames=[], - sweep_baseline_frames=[], - sweep_baseline_pending=True, - sweep_baseline_hold_since=None, - baseline_hold_seconds=0.5, - sweep_last_valid_at=0.0, - ) - node._motion_command_reached = lambda selected, command, state=None: ( - G20ThreeCameraCalibrationNode._motion_command_reached( - node, selected, command, state - ) - ) - node._endpoint_tolerance_for_spec = lambda selected, endpoint: ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, selected, endpoint - ) - ) - - G20ThreeCameraCalibrationNode._accept_frame( - node, _frame("front", baseline_state) - ) - assert node.sweep_baseline_frames == [] - - node.sweep_baseline_hold_since = 1.0 - G20ThreeCameraCalibrationNode._accept_frame( - node, _frame("front", moving_state) - ) - G20ThreeCameraCalibrationNode._accept_frame( - node, _frame("front", baseline_state) - ) - - assert len(node.sweep_baseline_frames) == 1 - assert node.sweep_baseline_frames[0].state_u8[9] == 127 - - -def test_dedicated_baseline_hold_is_persisted_separately(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - item = SweepItem(spec, 2, DIRECTION_INCREASING) - state = build_calibration_motion_command( - spec, - 127, - baseline=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - ) - frames = [ - replace( - _frame("front", state), - joint_quaternions_xyzw={ - "pinky_mcp_roll": (0.0, 0.0, 0.0, 1.0) - }, - joint_vectors_xyz_m={"pinky_mcp_roll": (0.02, 0.0, 0.0)}, - image_vectors_xy_px={"pinky_mcp_roll": (20.0, 0.0)}, - parent_poses_common={ - "pinky_mcp_roll": { - "translation_xyz_m": [0.0, 0.0, 0.0], - "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], - } - }, - child_poses_common={ - "pinky_mcp_roll": { - "translation_xyz_m": [0.02, 0.0, 0.0], - "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], - } - }, - joint_reprojection_error_px={"pinky_mcp_roll": 0.05}, - ) - for _ in range(3) - ] - frames.extend( - replace( - frame, - view="side", - joint_quaternions_xyzw={ - "pinky_mcp_roll_side": (0.0, 0.0, 0.0, 1.0) - }, - joint_vectors_xyz_m={"pinky_mcp_roll_side": (0.03, 0.0, 0.0)}, - image_vectors_xy_px={"pinky_mcp_roll_side": (30.0, 0.0)}, - parent_poses_common={ - "pinky_mcp_roll_side": frame.parent_poses_common[ - "pinky_mcp_roll" - ] - }, - child_poses_common={ - "pinky_mcp_roll_side": frame.child_poses_common[ - "pinky_mcp_roll" - ] - }, - joint_reprojection_error_px={"pinky_mcp_roll_side": 0.05}, - ) - for frame in list(frames) - ) - records = {"pinky_mcp_roll": [], "pinky_mcp_roll_side": []} - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - baseline_command=THREE_CAMERA_BASELINE_COMMAND, - sweep_baseline_frames=frames, - minimum_baseline_hold_frames=3, - baseline_hold_seconds=0.5, - sweep_attempts={spec.key: 2}, - baseline_records_by_joint=records, - raw_path=tmp_path / "raw_samples.jsonl", - ) - - G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold(node, item) - - assert len(records["pinky_mcp_roll"]) == 1 - record = records["pinky_mcp_roll"][0] - assert record["kind"] == "baseline_hold_sample" - assert record["cycle"] == 2 - assert record["direction"] == DIRECTION_INCREASING - assert record["command_u8"] == 127 - assert record["valid_frames"] == 3 - assert record["relative_translation_xyz_m"] == [0.02, 0.0, 0.0] - assert record["parent_pose_common"]["translation_xyz_m"] == [0.0] * 3 - - -def test_cross_view_roll_hysteresis_classification() -> None: - assert _classify_cross_view_roll_hysteresis( - [0.99, 1.00, 0.98, 0.99], - [0.91, 0.94, 0.93, 0.92], - limit_deg=0.5, - ) == "both_views_confirm_direction_dependent_pose" - assert _classify_cross_view_roll_hysteresis( - [0.99] * 4, - [0.20] * 4, - limit_deg=0.5, - ) == "front_only_difference_check_roll_tag_bracket_or_front_pnp" - - -def test_diagnostic_mode_uses_one_multiview_physical_task() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - node = SimpleNamespace( - cross_view_roll_diagnostic_finger="pinky", - ) - - role = G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - node, spec - ) - - assert role == "multiview" - assert spec.joints == ("pinky_mcp_roll", "pinky_mcp_roll_side") - - -def test_diagnostic_completion_locks_publication(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - pauses: list[str] = [] - node = SimpleNamespace( - cross_view_roll_diagnostic_finger="pinky", - baseline_maximum_hysteresis_rad=math.radians(0.5), - raw_path=tmp_path / "raw_samples.jsonl", - fit_failure={}, - cross_view_roll_diagnostic_result={}, - _roll_baseline_hysteresis_degrees=lambda name: ( - [0.99, 1.00, 0.98, 0.99] - if name == "pinky_mcp_roll" - else [0.92, 0.94, 0.93, 0.91] - ), - _pause=lambda reason: pauses.append(reason), - ) - - G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( - node, - spec, - [ - { - "joint": "pinky_mcp_roll_side", - "metric": "rotation_circle_axis_difference_deg", - "actual": 24.3, - "limit": 1.0, - } - ], - ) - - result = node.cross_view_roll_diagnostic_result - assert result["publication_locked"] is True - assert result["interpretation"] == ( - "both_views_confirm_direction_dependent_pose" - ) - assert result["side_quality_failures"][0]["actual"] == 24.3 - assert pauses == ["cross_view_roll_diagnostic_complete"] - - -def test_joint_specific_zero_endpoint_deadbands() -> None: - yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10) - index_roll = next(item for item in SWEEP_SPECS if item.motor_index == 6) - pinky_pip = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 19 - ) - node = SimpleNamespace( - endpoint_tolerance_u8=2.0, - thumb_yaw_zero_endpoint_tolerance_u8=4.0, - right_thumb_yaw_255_endpoint_tolerance_u8=5.0, - pinky_pip_zero_endpoint_tolerance_u8=5.0, - ) - - assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, yaw, 0 - ) == 4.0 - assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, yaw, 255 - ) == 2.0 - node.profile = RIGHT_HAND_PROFILE - assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, yaw, 255 - ) == 5.0 - assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, index_roll, 0 - ) == 2.0 - assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, pinky_pip, 0 - ) == 5.0 - assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, pinky_pip, 255 - ) == 2.0 - - -def test_pose_transition_uses_slow_roll_speed_for_motor_six() -> None: - state = [255.0] * 20 - target = [255] * 20 - state[6] = 172.0 - target[6] = 127 - node = SimpleNamespace( - latest_state_u8=tuple(state), - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - _normal_speed_profile=lambda: [15] * 5, - ) - - speeds = G20ThreeCameraCalibrationNode._transition_speed_profile( - node, target - ) - - assert speeds == [15, 5, 15, 15, 15] - - -def test_pose_transition_uses_flex_speed_for_pitch_and_pip() -> None: - state = [255.0] * 20 - target = [255] * 20 - target[3] = 0 - target[18] = 0 - node = SimpleNamespace( - latest_state_u8=tuple(state), - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - _normal_speed_profile=lambda: [15] * 5, - ) - - speeds = G20ThreeCameraCalibrationNode._transition_speed_profile( - node, target - ) - - assert speeds == [15, 15, 15, 10, 15] - - -def test_pinky_pip_command_zero_accepts_firmware_feedback_five() -> None: - pinky_pip = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 19 - ) - state = [255.0] * 20 - state[19] = 5.0 - node = SimpleNamespace( - latest_state_u8=tuple(state), - baseline_command=tuple([255] * 20), - endpoint_tolerance_u8=2.0, - pinky_pip_zero_endpoint_tolerance_u8=5.0, - ) - node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, spec, endpoint - ) - ) - - assert G20ThreeCameraCalibrationNode._motion_command_reached( - node, pinky_pip, 0, tuple(state) - ) - state[19] = 6.0 - assert not G20ThreeCameraCalibrationNode._motion_command_reached( - node, pinky_pip, 0, tuple(state) - ) - - -def test_right_baseline_and_auxiliary_accept_thumb_yaw_feedback_250() -> None: - state = [253.0] * 20 - state[0] = 254.0 - state[5] = 254.0 - state[6:10] = [127.0] * 4 - state[10] = 250.0 - state[15] = 255.0 - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - latest_state_u8=tuple(state), - baseline_command=tuple(baseline), - endpoint_tolerance_u8=2.0, - thumb_yaw_zero_endpoint_tolerance_u8=4.0, - right_thumb_yaw_255_endpoint_tolerance_u8=5.0, - pinky_pip_zero_endpoint_tolerance_u8=5.0, - ) - node._motor_endpoint_tolerance = lambda motor, endpoint: ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - node, motor, endpoint - ) - ) - node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, spec, endpoint - ) - ) - - assert G20ThreeCameraCalibrationNode._baseline_reached(node) - details = G20ThreeCameraCalibrationNode._baseline_error_details(node) - assert details["motor_index"] == 10 - assert details["actual_u8"] == 250.0 - assert details["tolerance_u8"] == 5.0 - - thumb_pitch = next( - spec for spec in RIGHT_HAND_PROFILE.sweep_specs - if spec.motor_index == 0 - ) - state[0] = 255.0 - assert G20ThreeCameraCalibrationNode._motion_command_reached( - node, thumb_pitch, 255, tuple(state) - ) - - -def test_safe_waypoint_ignores_reserved_channels_and_reports_real_motor() -> None: - state = [ - 255.0, 254.0, 254.0, 254.0, 254.0, 254.0, - 127.0, 127.0, 127.0, 127.0, 253.0, - 0.0, 0.0, 0.0, 0.0, - 255.0, 253.0, 253.0, 253.0, 253.0, - ] - command = [int(round(value)) for value in state] - command[5] = 255 - # Reserved SDK feedback is zero even if an old full command contains 255. - command[11:15] = [255, 255, 255, 255] - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - latest_state_u8=tuple(state), - endpoint_tolerance_u8=2.0, - right_thumb_yaw_255_endpoint_tolerance_u8=5.0, - pinky_pip_zero_endpoint_tolerance_u8=5.0, - ) - - assert G20ThreeCameraCalibrationNode._command_vector_reached(node, command) - assert G20ThreeCameraCalibrationNode._command_vector_error_u8( - node, command - ) == 1.0 - - command[3] = 240 - details = G20ThreeCameraCalibrationNode._command_vector_error_details( - node, command, "prepare_motor_0" - ) - assert details["motor_index"] == 3 - assert details["actual_u8"] == 254.0 - assert details["target_u8"] == 240.0 - assert details["error_u8"] == 14.0 - - -def test_feedback_inside_endpoint_tolerance_does_not_create_noop_transition() -> None: - state = [ - 254.0, 254.0, 254.0, 254.0, 254.0, 254.0, - 127.0, 127.0, 127.0, 127.0, 250.0, - 0.0, 0.0, 0.0, 0.0, - 255.0, 254.0, 254.0, 254.0, 254.0, - ] - target = [255] * 20 - target[6:10] = [127] * 4 - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - endpoint_tolerance_u8=2.0, - right_thumb_yaw_255_endpoint_tolerance_u8=5.0, - pinky_pip_zero_endpoint_tolerance_u8=5.0, - ) - - snapped = G20ThreeCameraCalibrationNode._snap_reached_state_to_command( - node, state, target - ) - - controlled = { - spec.motor_index for spec in RIGHT_19_HAND_PROFILE.joint_specs.values() - } - assert all(snapped[index] == target[index] for index in controlled) - assert snapped[11:15] == (0, 0, 0, 0) - - -def test_thumb_yaw_command_zero_accepts_feedback_four_only_for_swept_motor() -> None: - yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10) - state = [255.0] * 20 - state[10] = 4.0 - state[5] = 143.0 - node = SimpleNamespace( - latest_state_u8=tuple(state), - baseline_command=tuple([255] * 20), - endpoint_tolerance_u8=2.0, - thumb_yaw_zero_endpoint_tolerance_u8=4.0, - ) - node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - node, spec, endpoint - ) - ) - - assert G20ThreeCameraCalibrationNode._motion_command_reached( - node, yaw, 0, tuple(state) - ) - state[10] = 5.0 - assert not G20ThreeCameraCalibrationNode._motion_command_reached( - node, yaw, 0, tuple(state) - ) - state[10] = 4.0 - state[5] = 142.0 - assert not G20ThreeCameraCalibrationNode._motion_command_reached( - node, yaw, 0, tuple(state) - ) - - -def test_thumb_yaw_direction_recovery_keeps_motor_five_at_clearance() -> None: - yaw = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 10 - ) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - for direction, expected_start in ( - (DIRECTION_DECREASING, 255), - (DIRECTION_INCREASING, 0), - ): - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - baseline_command=tuple(baseline), - active_sweep=SweepItem(yaw, 1, direction), - retry_sweep_items=[], - ) - - command = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "retry_sweep" - ) - - assert command[5] == 145 - assert command[10] == expected_start - assert all( - command[index] == baseline[index] - for index in range(20) - if index not in {5, 10} - ) - - -def test_thumb_yaw_cycle_and_manual_recovery_use_queued_direction_start() -> None: - yaw = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 10 - ) - other = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index != 10 - ) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - queued = SweepItem(yaw, 2, DIRECTION_INCREASING) - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - baseline_command=tuple(baseline), - active_sweep=SweepItem(other, 0, DIRECTION_DECREASING), - retry_sweep_items=[queued], - ) - - command = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "resume_sweep" - ) - - assert command[5] == 145 - assert command[10] == 0 - - -def test_thumb_yaw_clearance_is_released_only_for_normal_transition() -> None: - yaw = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 10 - ) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - baseline_command=tuple(baseline), - active_sweep=SweepItem(yaw, 2, DIRECTION_INCREASING), - retry_sweep_items=[], - ) - - command = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "fit" - ) - - assert command == tuple(baseline) - assert command[5] == 255 - - -def test_next_cycle_keeps_side_clearance_pose_parked() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "ring_roll_multiview" - ) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - next_item = SweepItem(spec, 1, DIRECTION_DECREASING) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - baseline_command=tuple(baseline), - sweep_items=[next_item], - sweep_index=0, - retry_sweep_items=[], - ) - - command = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "next_cycle" - ) - - # Ring and pinky roll remain at their safe 127 baselines. The already- - # cleared pinky remains bent instead of being unfolded and bent again. - assert command[8] == 127 - assert command[4] == 0 - assert command[9] == 127 - assert command[19] == 0 - assert command[1] == 255 - assert command[6] == 255 - - -def test_middle_fit_retry_keeps_side_clearance_pose_parked() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "middle_roll_multiview" - ) - baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) - queued = SweepItem(spec, 0, DIRECTION_DECREASING) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - baseline_command=baseline, - active_sweep=None, - retry_sweep_items=[queued], - ) - - command = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "resume_sweep" - ) - - expected = tuple( - build_calibration_motion_command( - spec, - queued.start_u8, - baseline=baseline, - profile=RIGHT_19_HAND_PROFILE, - ) - ) - assert command == expected - # Pinky and ring remain flexed out of both cameras instead of unfolding - # to the global baseline and immediately being flexed again. - assert command[3] == 0 and command[18] == 0 - assert command[4] == 0 and command[19] == 0 - assert command[8] == 127 and command[9] == 127 - assert command[7] == queued.start_u8 - - -def test_middle_fit_retry_has_no_unfold_refold_waypoint() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "middle_roll_multiview" - ) - baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) - queued = SweepItem(spec, 0, DIRECTION_DECREASING) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - baseline_command=baseline, - active_sweep=None, - retry_sweep_items=[queued], - ) - current = list( - build_calibration_motion_command( - spec, - 255, - baseline=baseline, - profile=RIGHT_19_HAND_PROFILE, - ) - ) - - target = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "resume_sweep" - ) - returns = build_calibration_return_waypoints( - target, - current_command=current, - profile=RIGHT_19_HAND_PROFILE, - ) - after_return = returns[-1] - preparations = build_calibration_preparation_waypoints( - spec, - queued.start_u8, - current_command=after_return, - baseline=baseline, - profile=RIGHT_19_HAND_PROFILE, - ) - - for waypoint in (*returns, *preparations): - assert waypoint[3] == 0 and waypoint[18] == 0 - assert waypoint[4] == 0 and waypoint[19] == 0 - - -def test_same_finger_next_task_keeps_neighbour_clearance_parked() -> None: - ring_roll_multiview = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "ring_roll_multiview" - ) - ring_pitch_side = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "ring_pitch_side" - ) - baseline = tuple(THREE_CAMERA_BASELINE_COMMAND) - next_item = SweepItem( - ring_pitch_side, -1, DIRECTION_DECREASING, precheck=True - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - baseline_command=baseline, - sweep_items=[next_item], - sweep_index=0, - retry_sweep_items=[], - ) - - transition = ( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - node, ring_roll_multiview - ) - ) - command = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, transition - ) - - assert transition == "next_task_same_finger" - # Pinky was bent once for ring side visibility and stays parked through - # ring roll, MCP pitch and PIP instead of unfolding between tasks. - assert command[9] == 127 - assert command[4] == 0 - assert command[19] == 0 - assert command[8] == 127 - assert command[3] == 255 - assert command[18] == 255 - - -def test_changing_finger_still_requires_global_safe_transition() -> None: - ring_pip = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "ring_pip_side" - ) - middle_roll = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "middle_roll_multiview" - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - sweep_items=[SweepItem(middle_roll, -1, DIRECTION_DECREASING, True)], - sweep_index=0, - retry_sweep_items=[], - ) - - assert ( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - node, ring_pip - ) - == "next_sweep" - ) - - -def test_retry_return_anchors_roll_order_on_failed_target_finger() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "pinky_roll_multiview" - ) - node = SimpleNamespace( - retry_sweep_items=[ - SweepItem(spec, 0, DIRECTION_DECREASING) - ], - sweep_items=[], - sweep_index=0, - active_sweep=None, - ) - - anchor = G20ThreeCameraCalibrationNode._return_anchor_roll_motor(node) - - assert anchor == 9 - - -def test_precheck_dense_coverage_selects_faster_formal_speed(tmp_path) -> None: - spec = next( - item - for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 0 - ) - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - active_sweep=None, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - adaptive_formal_speed_enabled=True, - adaptive_formal_speed_max_scale=1.5, - adaptive_formal_speed_minimum_bins=64, - adaptive_formal_speed_maximum_bin_gap=8, - precheck_speed_metrics={}, - formal_speed_scales={}, - sweep_retry_counts={}, - raw_path=tmp_path / "raw_samples.jsonl", - ) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): - item = SweepItem(spec, -1, direction, precheck=True) - node.active_sweep = item - G20ThreeCameraCalibrationNode._record_precheck_speed_metric( - node, - item, - bin_count=205, - maximum_bin_gap=3, - valid_frames=320, - ) - - assert math.isclose(node.formal_speed_scales[spec.key], 22.0 / 15.0) - node.active_sweep = SweepItem(spec, 0, DIRECTION_DECREASING) - speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) - assert speeds[0] == 22 - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "task_formal_speed_selected" - assert event["base_speed"] == 15 - assert event["formal_speed"] == 22 - - -def test_right_19_dense_precheck_keeps_deterministic_formal_speed(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "thumb_cmc_pitch_front" - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - active_sweep=None, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - adaptive_formal_speed_enabled=True, - adaptive_formal_speed_max_scale=1.5, - adaptive_formal_speed_minimum_bins=64, - adaptive_formal_speed_maximum_bin_gap=8, - precheck_speed_metrics={}, - formal_speed_scales={}, - sweep_retry_counts={}, - raw_path=tmp_path / "raw_samples.jsonl", - ) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): - item = SweepItem(spec, -1, direction, precheck=True) - node.active_sweep = item - G20ThreeCameraCalibrationNode._record_precheck_speed_metric( - node, - item, - bin_count=220, - maximum_bin_gap=3, - valid_frames=380, - ) - - assert node.formal_speed_scales[spec.key] == 1.0 - node.active_sweep = SweepItem(spec, 0, DIRECTION_DECREASING) - speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) - assert speeds[0] == 15 - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["stability_speed_lock"] is True - assert event["ineligible_reason"] == ( - "g20_right_deterministic_acquisition_speed" - ) - - -def test_precheck_large_gap_keeps_conservative_formal_speed(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "thumb_cmc_yaw_top" - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - active_sweep=None, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - adaptive_formal_speed_enabled=True, - adaptive_formal_speed_max_scale=1.5, - adaptive_formal_speed_minimum_bins=64, - adaptive_formal_speed_maximum_bin_gap=8, - precheck_speed_metrics={}, - formal_speed_scales={}, - sweep_retry_counts={}, - raw_path=tmp_path / "raw_samples.jsonl", - ) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): - item = SweepItem(spec, -1, direction, precheck=True) - node.active_sweep = item - G20ThreeCameraCalibrationNode._record_precheck_speed_metric( - node, - item, - bin_count=220, - maximum_bin_gap=17, - valid_frames=640, - ) - - assert node.formal_speed_scales[spec.key] == 1.0 - node.active_sweep = SweepItem(spec, 0, DIRECTION_DECREASING) - speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) - assert speeds[0] == 15 - - -def test_roll_precheck_never_accelerates_strict_backlash_scan(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "pinky_roll_multiview" - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - active_sweep=None, - active_sweep_is_fit_retry=False, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - adaptive_formal_speed_enabled=True, - adaptive_formal_speed_max_scale=1.5, - adaptive_formal_speed_minimum_bins=64, - adaptive_formal_speed_maximum_bin_gap=8, - precheck_speed_metrics={}, - formal_speed_scales={}, - sweep_retry_counts={}, - raw_path=tmp_path / "raw_samples.jsonl", - ) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): - item = SweepItem(spec, -1, direction, precheck=True) - node.active_sweep = item - G20ThreeCameraCalibrationNode._record_precheck_speed_metric( - node, - item, - bin_count=100, - maximum_bin_gap=4, - valid_frames=140, - ) - - assert node.formal_speed_scales[spec.key] == 1.0 - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["eligible"] is False - assert event["ineligible_reason"] == ( - "g20_right_deterministic_acquisition_speed" - ) - - -def test_fit_retry_uses_base_speed_instead_of_adaptive_speed() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.task_name == "thumb_cmc_pitch_front" - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - active_sweep=SweepItem(spec, 1, DIRECTION_DECREASING), - active_sweep_is_fit_retry=True, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - formal_speed_scales={spec.key: 1.5}, - sweep_retry_counts={}, - ) - - speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec(node, spec) - - assert speeds[0] == 15 - - -def test_thumb_yaw_recovery_zero_accepts_feedback_four() -> None: - yaw = next( - item for item in RIGHT_HAND_PROFILE.sweep_specs - if item.motor_index == 10 - ) - baseline = [255] * 20 - baseline[6:10] = [127] * 4 - return_command = list(baseline) - return_command[5] = 145 - return_command[10] = 0 - state = [float(value) for value in return_command] - state[10] = 4.0 - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - baseline_command=tuple(baseline), - return_command_u8=tuple(return_command), - latest_state_u8=tuple(state), - endpoint_tolerance_u8=2.0, - thumb_yaw_zero_endpoint_tolerance_u8=4.0, - right_thumb_yaw_255_endpoint_tolerance_u8=5.0, - ) - - assert G20ThreeCameraCalibrationNode._baseline_reached(node) - details = G20ThreeCameraCalibrationNode._motion_command_error_details( - node, yaw, 0, "return_baseline" - ) - assert details["motor_index"] == 10 - assert details["actual_u8"] == 4.0 - assert details["tolerance_u8"] == 4.0 - - -def _image_cycle_records( - travels_rad: list[float], *, depth_slope: float = 0.0, command_step: int = 16 -) -> list[dict]: - commands = list(range(0, 256, command_step)) + [255] - records = [] - for cycle, travel in enumerate(travels_rad): - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): - for command in commands: - angle = travel * (255.0 - command) / 255.0 - x = 0.03 * math.cos(angle) - y = 0.03 * math.sin(angle) - z = float(depth_slope) * (x - 0.03) - records.append( - { - "cycle": cycle, - "direction": direction, - "command_u8": command, - "relative_translation_xyz_m": [ - x, - y, - z, - ], - "image_relative_xy_px": [ - 100.0 * math.cos(angle), - 100.0 * math.sin(angle), - ], - "relative_quaternion_xyzw": Rotation.from_rotvec( - np.asarray([0.0, 0.0, angle]) - ).as_quat().tolist(), - "parent_pose_common": { - "translation_xyz_m": [0.0, 0.0, 0.0], - "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], - }, - "child_pose_common": { - "translation_xyz_m": [ - x, - y, - z, - ], - "quaternion_xyzw": Rotation.from_rotvec( - np.asarray([0.0, 0.0, angle]) - ).as_quat().tolist(), - }, - "state_u8": [float(command)] + [255.0] * 19, - } - ) - return records - - -def _fit_check_node(records_by_joint: dict) -> SimpleNamespace: - node = SimpleNamespace( - records_by_joint=records_by_joint, - repetitions=3, - trajectory_maximum_plane_rms_m=0.004, - trajectory_maximum_radial_rms_m=0.004, - trajectory_minimum_radius_m=0.003, - trajectory_minimum_arc_rad=math.radians(15.0), - image_trajectory_maximum_radial_rms_px=2.0, - image_trajectory_maximum_radial_p95_px=3.5, - image_trajectory_minimum_radius_px=20.0, - trajectory_maximum_cycle_travel_difference_rad=math.radians(3.0), - passive_maximum_cycle_travel_difference_rad=math.radians(10.0), - maximum_monotonic_correction_rad=math.radians(2.0), - maximum_hysteresis_rad=math.radians(5.0), - baseline_maximum_hysteresis_rad=math.radians(0.5), - passive_maximum_monotonic_correction_rad=math.radians(3.0), - passive_maximum_hysteresis_rad=math.radians(7.5), - baseline_command=[255] * 20, - axis_maximum_plane_rms_m=0.003, - passive_axis_maximum_plane_rms_m=0.004, - axis_maximum_radial_rms_m=0.003, - axis_maximum_pose_line_rms_m=0.001, - axis_maximum_rotation_circle_difference_rad=math.radians(1.0), - active_maximum_rotation_orthogonal_rms_rad=math.radians(2.5), - passive_maximum_rotation_orthogonal_rms_rad=math.radians(7.5), - zero_maximum_axis_cycle_difference_rad=math.radians(0.75), - ) - node._fit_joint_records = lambda name, records, relaxed=False: ( - G20ThreeCameraCalibrationNode._fit_joint_records( - node, name, records, relaxed=relaxed - ) - ) - node._fit_axis_measurement = lambda name, cycle: ( - G20ThreeCameraCalibrationNode._fit_axis_measurement(node, name, cycle) - ) - return node - - -def test_right_19_end_on_flexion_uses_image_curve_not_planar_pnp_tilt() -> None: - records = _image_cycle_records([math.radians(100.0)] * 3) - # Reproduce a command-dependent out-of-axis PnP tilt while preserving the - # accurately observed projected Tag-centre circle. - for record in records: - command = int(record["command_u8"]) - angle = math.radians(100.0) * (255.0 - command) / 255.0 - rotation = Rotation.from_rotvec([0.0, 0.0, angle]) * Rotation.from_rotvec( - [math.radians(12.0) * math.sin(angle), 0.0, 0.0] - ) - record["relative_quaternion_xyzw"] = rotation.as_quat().tolist() - - node = _fit_check_node( - { - "pinky_mcp_roll": records, - "pinky_pip": records, - "pinky_dip": records, - } - ) - node.profile = RIGHT_19_HAND_PROFILE - roll_fit = G20ThreeCameraCalibrationNode._fit_joint_records( - node, "pinky_mcp_roll", records - ) - image_fit = G20ThreeCameraCalibrationNode._fit_joint_records( - node, "pinky_pip", records - ) - passive_fit = G20ThreeCameraCalibrationNode._fit_joint_records( - node, "pinky_dip", records - ) - - assert "pinky_pip" in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS - assert roll_fit.circle["space"] == "image_2d" - assert image_fit.circle["space"] == "image_2d" - assert image_fit.quality["radial_rms_px"] < 1.0e-8 - # DIP is measured against its moving PIP parent, so it must remain a - # relative-orientation curve and must never inherit the PIP image motion. - assert passive_fit.circle["space"] == "relative_rotation_3d" - - -def test_right_19_finger_rolls_share_reference_axis_direction() -> None: - pinky = _image_cycle_records([math.radians(47.0)] * 3) - ring = _image_cycle_records([math.radians(47.0)] * 3) - for record in ring: - command = int(record["command_u8"]) - angle = math.radians(47.0) * (255.0 - command) / 255.0 - record["relative_quaternion_xyzw"] = ( - Rotation.from_rotvec([angle, 0.0, 0.0]).as_quat().tolist() - ) - - node = _fit_check_node( - {"pinky_mcp_roll": pinky, "ring_mcp_roll": ring} - ) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", RIGHT_19_HAND_PROFILE.layout_id - ) - reference = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - node, "pinky_mcp_roll", 0 - ) - measured = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - node, "ring_mcp_roll", 0 - ) - - assert measured.axis_direction_source == "upstream_constraint" - assert abs( - float( - np.asarray(reference.axis_common_xyz) - @ np.asarray(measured.axis_common_xyz) - ) - ) > math.cos(math.radians(1.0e-4)) - - -def test_thumb_mcp_ip_group_uses_mimic_only_as_pnp_branch_prior() -> None: - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - pnp_maximum_pose_jump_rad=math.radians(35.0), - pnp_maximum_translation_jump_m=0.04, - pnp_group_initialization_frames=8, - pnp_group_normal_alignment_scale_rad=math.radians(5.0), - pnp_group_maximum_normal_alignment_rad=math.radians(15.0), - pnp_tracker_reset_seconds=5.0, - thumb_ip_pnp_coupling_multiplier=1.03, - thumb_ip_pnp_coupling_scale_rad=math.radians(3.0), - thumb_ip_pnp_maximum_coupling_residual_rad=math.radians(7.5), - ) - - tracker = G20ThreeCameraCalibrationNode._make_group_pose_tracker( - node, - "front", - ("front_base", "thumb_cmc", "thumb_mcp", "thumb_ip"), - ) - - assert tracker.coupled_rotation_pairs == ( - ("thumb_cmc", "thumb_mcp", "thumb_mcp", "thumb_ip", 1.03), - ) - assert tracker.maximum_coupled_rotation_residual_rad == pytest.approx( - math.radians(7.5) - ) - - -def test_side_joint_group_does_not_assume_tag_mounting_planes() -> None: - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - pnp_maximum_pose_jump_rad=math.radians(35.0), - pnp_maximum_translation_jump_m=0.04, - pnp_group_initialization_frames=8, - pnp_group_normal_alignment_scale_rad=math.radians(5.0), - pnp_group_maximum_normal_alignment_rad=math.radians(15.0), - pnp_tracker_reset_seconds=5.0, - thumb_ip_pnp_coupling_multiplier=1.03, - thumb_ip_pnp_coupling_scale_rad=math.radians(3.0), - thumb_ip_pnp_maximum_coupling_residual_rad=math.radians(7.5), - ) - - tracker = G20ThreeCameraCalibrationNode._make_group_pose_tracker( - node, - "side", - ("side_base", "middle_pip", "middle_dip"), - ) - - assert tracker.adjacent_pairs == ( - ("side_base", "middle_pip"), - ("middle_pip", "middle_dip"), - ) - assert tracker.normal_alignment_pairs == () - - -def test_fit_failure_preserves_main_progress_and_selects_retry_scope(tmp_path) -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) - node = SimpleNamespace( - sweep_items=_sweep_items(), - sweep_attempts={spec.motor_index: 1}, - retry_sweep_spec=None, - retry_resume_index=None, - fit_failure={}, - sweep_index=24, - raw_path=tmp_path / "raw_samples.jsonl", - repetitions=3, - ) - node._sweep_spec_start_index = lambda selected: ( - G20ThreeCameraCalibrationNode._sweep_spec_start_index(node, selected) - ) - node._pause = lambda reason: setattr(node, "paused_reason", reason) - - G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, - spec, - [ - { - "joint": "index_mcp_roll", - "metric": "arc_deg", - "actual": 3.98, - "limit": 15.0, - "comparison": "minimum", - } - ], - ) - - assert node.sweep_index == 24 - assert node.retry_sweep_spec == spec - assert node.retry_cycles == {0, 1, 2} - assert node.fit_failure["directions_to_rescan"] == 6 - assert node.paused_reason == "joint_fit_check_failed" - - -def test_zero_model_failure_never_schedules_an_automatic_rescan(tmp_path) -> None: - pauses: list[str] = [] - node = SimpleNamespace( - profile=RIGHT_HAND_PROFILE, - raw_path=tmp_path / "raw_samples.jsonl", - maximum_validation_p95_rad=math.radians(2.0), - zero_maximum_offset_rad=math.radians(20.0), - zero_joint_maximum_offsets_rad={}, - retry_sweep_spec=RIGHT_HAND_PROFILE.sweep_specs[2], - retry_resume_index=42, - retry_cycles={0, 1, 2}, - fit_failure={}, - _pause=lambda reason: pauses.append(reason), - ) - result = SimpleNamespace( - failure_reasons={ - "thumb_mcp": "zero_offset_exceeds_configured_limit" - }, - direct_offsets_rad={"thumb_mcp": math.radians(-39.81)}, - validation_error_by_joint_rad={ - "thumb_ip": math.radians(23.5) - }, - ) - - G20ThreeCameraCalibrationNode._pause_for_zero_model_failure(node, result) - - assert pauses == ["zero_model_validation_failed"] - assert node.retry_sweep_spec is None - assert node.retry_resume_index is None - assert node.retry_cycles == set() - assert node.fit_failure["recoverable_by_rescan"] is False - assert node.fit_failure["directions_to_rescan"] == 0 - assert node.fit_failure["failures"][0]["actual_deg"] == -39.81 - assert node.fit_failure["failures"][0]["limit_deg"] == 20.0 - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "zero_model_failure" - - -def test_resume_rejects_zero_model_failure_without_starting_motion() -> None: - node = SimpleNamespace( - state="PAUSED", - paused_reason="zero_model_validation_failed", - ) - response = SimpleNamespace(success=None, message="") - - result = G20ThreeCameraCalibrationNode._resume_callback( - node, SimpleNamespace(), response - ) - - assert result.success is False - assert "cannot repair" in result.message - - -def test_scan_completion_is_not_reported_as_calibration_completion() -> None: - assert _overall_progress("SWEEP", scan_progress=1.0) == 0.90 - assert _overall_progress("FITTING", scan_progress=1.0) == 0.92 - assert np.isclose( - _overall_progress( - "VALIDATION_CAPTURE", - scan_progress=1.0, - validation_index=5, - validation_total=10, - ), - 0.955, - ) - assert _overall_progress("COMPLETE", scan_progress=1.0) == 1.0 - - -def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) - reset_runtimes = [] - front_runtime = SimpleNamespace( - pnp_invalid_since=12.0, - pnp_reset_count=0, - ) - records = { - "thumb_mcp": [ - {"cycle": 0, "keep": "mcp"}, - {"cycle": 1, "old": "mcp"}, - ], - "thumb_ip": [ - {"cycle": 0, "keep": "ip"}, - {"cycle": 1, "old": "ip"}, - ], - "thumb_cmc_pitch": [{"keep": True}], - } - node = SimpleNamespace( - retry_sweep_spec=spec, - records_by_joint=records, - sweep_attempts={spec.motor_index: 1}, - raw_path=tmp_path / "raw_samples.jsonl", - paused_reason="joint_fit_check_failed", - fit_failure={"failures": ["old"]}, - retry_cycles={1}, - repetitions=3, - sweep_items=_sweep_items(), - views={"front": front_runtime}, - _reset_view_trackers=lambda runtime: reset_runtimes.append(runtime), - ) - - selected = G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) - - assert selected == spec - assert records["thumb_mcp"] == [{"cycle": 0, "keep": "mcp"}] - assert records["thumb_ip"] == [{"cycle": 0, "keep": "ip"}] - assert records["thumb_cmc_pitch"] == [{"keep": True}] - assert node.sweep_attempts[15] == 2 - assert node.retry_sweep_spec == spec - assert node.fit_failure == {} - assert reset_runtimes == [front_runtime] - assert front_runtime.pnp_invalid_since is None - assert front_runtime.pnp_reset_count == 1 - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event == { - "kind": "retry", - "view": "front", - "motor_index": 15, - "joints": ["thumb_mcp", "thumb_ip"], - "joints_to_rescan": ["thumb_ip", "thumb_mcp"], - "attempt": 2, - "reason": "joint_fit_check_failed", - "cycles": [2], - } - - -def test_thumb_yaw_retry_replaces_palm_axis_source_tasks_not_yaw_sweep( - tmp_path, -) -> None: - yaw_spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_yaw_top" - ) - source_specs = [ - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key - in {"thumb_cmc_pitch_front", "thumb_cmc_roll_front"} - ] - source_joints = { - joint for item in source_specs for joint in item.joints - } - records = { - **{ - joint: [ - {"cycle": 0, "keep": joint}, - {"cycle": 2, "old": joint}, - ] - for joint in source_joints - }, - "thumb_cmc_yaw": [{"cycle": 2, "keep": "yaw"}], - } - source_names = { - observer.source_name - for observer in RIGHT_19_HAND_PROFILE.palm_axis_observers - if observer.task_name in {item.key for item in source_specs} - } - palm_records = { - name: [ - {"cycle": 0, "keep": name}, - {"cycle": 2, "old": name}, - ] - for name in source_names - } - resets: list[str] = [] - - def runtime(name: str) -> SimpleNamespace: - return SimpleNamespace( - name=name, - pnp_invalid_since=1.0, - pnp_reset_count=0, - task_valid_frames=100, - task_total_frames=110, - ) - - views = {name: runtime(name) for name in ("front", "top")} - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - retry_sweep_spec=yaw_spec, - retry_joint_names={"thumb_cmc_yaw"}, - retry_source_failure_task_key=yaw_spec.key, - retry_source_task_keys=tuple(item.key for item in source_specs), - retry_cycles={2}, - records_by_joint=records, - baseline_records_by_joint={joint: [] for joint in source_joints}, - command_records_by_joint={joint: [] for joint in source_joints}, - palm_axis_records_by_source=palm_records, - sweep_attempts={yaw_spec.key: 1}, - sweep_retry_counts={}, - raw_path=tmp_path / "raw_samples.jsonl", - paused_reason="joint_fit_check_failed", - fit_failure={"failures": ["old"]}, - repetitions=4, - sweep_items=[ - SweepItem(item, cycle, direction) - for item in source_specs - for cycle in range(4) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ], - views=views, - _reset_view_trackers=( - lambda item, preserve_task_reference=False: resets.append( - item.name - ) - ), - ) - - G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) - - assert records["thumb_cmc_yaw"] == [ - {"cycle": 2, "keep": "yaw"} - ] - assert all( - records[joint] == [{"cycle": 0, "keep": joint}] - for joint in source_joints - ) - assert all( - palm_records[name] == [{"cycle": 0, "keep": name}] - for name in source_names - ) - assert [(item.spec.key, item.cycle) for item in node.retry_sweep_items] == [ - (item.key, 2) - for item in source_specs - for _direction in range(2) - ] - assert resets == [] - assert node.retry_preserve_pnp_continuity is True - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["source_task_names"] == [item.key for item in source_specs] - assert event["source_attempts"] == {item.key: 2 for item in source_specs} - assert event["joints_to_rescan"] == sorted(source_joints) - assert event["cycles"] == [3] - assert event["pnp_reference_policy"] == "preserve_existing_generation" - assert all(runtime.task_valid_frames == 0 for runtime in views.values()) - assert all(runtime.task_total_frames == 0 for runtime in views.values()) - - -def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "middle_roll_multiview" - ) - primary, validation = spec.joints - records = { - primary: [ - {"cycle": 0, "accepted": "front"}, - {"cycle": 1, "accepted": "front"}, - ], - validation: [ - {"cycle": 0, "old": "side"}, - {"cycle": 1, "accepted": "side"}, - ], - } - side_runtime = SimpleNamespace( - pnp_invalid_since=12.0, - pnp_reset_count=0, - task_valid_frames=4927, - task_total_frames=5212, - ) - resets: list[bool] = [] - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - retry_sweep_spec=spec, - retry_joint_names={validation}, - records_by_joint=records, - baseline_records_by_joint={primary: [], validation: []}, - command_records_by_joint={primary: [], validation: []}, - sweep_attempts={spec.key: 1}, - raw_path=tmp_path / "raw_samples.jsonl", - paused_reason="joint_fit_check_failed", - fit_failure={"failures": ["old"]}, - retry_cycles={0}, - repetitions=4, - sweep_items=[ - SweepItem(spec, cycle, direction) - for cycle in range(4) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ], - views={"side": side_runtime}, - _reset_view_trackers=( - lambda runtime, preserve_task_reference=False: resets.append( - preserve_task_reference - ) - ), - ) - - G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) - - assert records[primary] == [ - {"cycle": 0, "accepted": "front"}, - {"cycle": 1, "accepted": "front"}, - ] - assert records[validation] == [ - {"cycle": 1, "accepted": "side"} - ] - assert node.retry_joint_names == {validation} - assert side_runtime.task_valid_frames == 0 - assert side_runtime.task_total_frames == 0 - assert resets == [True] - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["joints_to_rescan"] == [validation] - assert event["cycles"] == [1] - - -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] = [] - pauses: list[str] = [] - holds: list[bool] = [] - node = SimpleNamespace( - active_sweep=item, - automatic_sweep_retry_limit=3, - sweep_retry_counts={}, - raw_path=tmp_path / "raw_samples.jsonl", - reason="", - _publish_hold_current=lambda: holds.append(True), - _begin_return_baseline=lambda after: transitions.append(after), - _pause=lambda reason: pauses.append(reason), - retry_speed_scales=(1.0,), - retry_endpoint_hold_seconds=(0.5,), - ) - - 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"] - assert pauses == ["sweep_bin_gap_too_large"] - assert holds == [] - 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] - - -def test_visibility_precheck_does_not_require_dense_feedback_bins(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 10 - ) - item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) - # Preserve the field observation: complete endpoints and midpoint, but a - # 17-u8 feedback gap between 99 and 116. - commands = [*range(0, 100), *range(116, 256)] - frames = [] - for command in commands: - state = [255.0] * 20 - state[10] = float(command) - frames.append(_frame("top", state)) - transitions: list[str] = [] - retries: list[str] = [] - node = SimpleNamespace( - active_sweep=item, - sweep_frames=frames, - minimum_sweep_bins=32, - maximum_bin_gap=16, - minimum_detection_rate=0.95, - views={"top": SimpleNamespace(valid_rate=1.0)}, - sweep_detection_total_frames=644, - sweep_detection_valid_frames=644, - raw_path=tmp_path / "raw_samples.jsonl", - active_sweep_is_fit_retry=False, - sweep_index=0, - sweep_items=[item], - profile=RIGHT_19_HAND_PROFILE, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - precheck_speed_metrics={}, - formal_speed_scales={}, - sweep_retry_counts={}, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 5.0, - _retry_active_sweep_or_pause=lambda reason: retries.append(reason), - _begin_return_baseline=lambda after: transitions.append(after), - ) - - G20ThreeCameraCalibrationNode._finish_active_sweep(node) - - assert retries == [] - assert transitions == ["fit"] - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "task_visibility_precheck" - assert event["passed"] is True - assert event["detection_rate"] == 1.0 - - -def test_visibility_precheck_accepts_interpolated_endpoint_fraction(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 1 - ) - item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) - # Field regression: the motor reached zero with the physical 2-u8 - # deadband, while the last image-timestamped feedback was 2.502 u8. - commands = [2.5021463960247776, *range(3, 255)] - frames = [] - for command in commands: - state = [255.0] * 20 - state[1] = float(command) - frames.append(_frame("side", state)) - transitions: list[str] = [] - retries: list[str] = [] - node = SimpleNamespace( - active_sweep=item, - sweep_frames=frames, - synchronised_endpoint_tolerance_margin_u8=1.0, - minimum_sweep_bins=32, - maximum_bin_gap=16, - minimum_detection_rate=0.95, - views={"side": SimpleNamespace(valid_rate=1.0)}, - sweep_detection_total_frames=500, - sweep_detection_valid_frames=382, - raw_path=tmp_path / "raw_samples.jsonl", - active_sweep_is_fit_retry=False, - sweep_index=0, - sweep_items=[item], - profile=RIGHT_19_HAND_PROFILE, - normal_calibration_speed=15, - index_roll_calibration_speed=5, - index_flex_calibration_speed=10, - precheck_speed_metrics={}, - formal_speed_scales={}, - sweep_retry_counts={}, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, - _retry_active_sweep_or_pause=lambda reason: retries.append(reason), - _begin_return_baseline=lambda after: transitions.append(after), - ) - - G20ThreeCameraCalibrationNode._finish_active_sweep(node) - - assert retries == [] - assert transitions == ["fit"] - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["detection_rate"] == 0.764 - assert event["detection_rate_below_threshold_views"] == ["side"] - assert event["trajectory_coverage_override_views"] == ["side"] - - -def test_low_precheck_rate_still_retries_when_trajectory_is_sparse(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 1 - ) - item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) - frames = [] - for command in (0, 127, 255): - state = [255.0] * 20 - state[1] = float(command) - frames.append(_frame("side", state)) - retries: list[str] = [] - node = SimpleNamespace( - active_sweep=item, - sweep_frames=frames, - minimum_sweep_bins=32, - maximum_bin_gap=16, - minimum_detection_rate=0.95, - sweep_detection_total_frames=100, - sweep_detection_valid_frames=3, - profile=RIGHT_19_HAND_PROFILE, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, - _retry_active_sweep_or_pause=lambda reason: retries.append(reason), - ) - - G20ThreeCameraCalibrationNode._finish_active_sweep(node) - - assert retries == ["task_precheck_detection_rate_too_low:side"] - - -def test_visibility_precheck_rejects_sample_outside_sync_endpoint_margin() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 1 - ) - item = SweepItem(spec, -1, DIRECTION_DECREASING, precheck=True) - frames = [] - for command in [3.01, *range(4, 255)]: - state = [255.0] * 20 - state[1] = float(command) - frames.append(_frame("side", state)) - retries: list[str] = [] - node = SimpleNamespace( - active_sweep=item, - sweep_frames=frames, - synchronised_endpoint_tolerance_margin_u8=1.0, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, - _retry_active_sweep_or_pause=lambda reason: retries.append(reason), - ) - - G20ThreeCameraCalibrationNode._finish_active_sweep(node) - - assert retries == ["sweep_missing_endpoint_bin"] - - -def test_synchronised_endpoint_margin_does_not_change_motion_deadband() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) - node = SimpleNamespace( - synchronised_endpoint_tolerance_margin_u8=1.0, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 2.0, - ) - - assert node._endpoint_tolerance_for_spec(spec, 0) == 2.0 - assert ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - node, spec, 0 - ) - == 3.0 - ) - - -def test_formal_sweep_still_rejects_17_u8_feedback_gap(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 10 - ) - item = SweepItem(spec, 0, DIRECTION_DECREASING) - commands = [*range(0, 100), *range(116, 256)] - frames = [] - for command in commands: - state = [255.0] * 20 - state[10] = float(command) - frames.append(_frame("top", state)) - retries: list[str] = [] - node = SimpleNamespace( - active_sweep=item, - sweep_frames=frames, - minimum_sweep_bins=32, - maximum_bin_gap=16, - _endpoint_tolerance_for_spec=lambda selected, endpoint: 5.0, - _retry_active_sweep_or_pause=lambda reason: retries.append(reason), - ) - - G20ThreeCameraCalibrationNode._finish_active_sweep(node) - - assert retries == ["sweep_bin_gap_too_large"] - - -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] = [] - prepared: list[bool] = [] - node = SimpleNamespace( - sweep_items=_sweep_items(), - sweep_attempts={spec.motor_index: 1}, - retry_sweep_spec=None, - retry_resume_index=None, - fit_failure={}, - sweep_index=18, - raw_path=tmp_path / "raw_samples.jsonl", - repetitions=3, - automatic_fit_retry_limit=1, - paused_reason="", - reason="", - _sweep_spec_start_index=lambda selected: ( - G20ThreeCameraCalibrationNode._sweep_spec_start_index(node, selected) - ), - _prepare_failed_sweep_retry=lambda: prepared.append(True), - _begin_return_baseline=lambda after: transitions.append(after), - _pause=lambda reason: pauses.append(reason), - ) - - G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, - spec, - [{"joint": "thumb_ip", "metric": "rotation_orthogonal_rms_deg"}], - ) - - assert prepared == [] - assert transitions == [] - assert pauses == ["joint_fit_check_failed"] - - -def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry( - tmp_path, -) -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) - failures = [ - { - "joint": "thumb_ip", - "metric": "rotation_circle_axis_difference_deg", - "cycle": cycle, - "actual": actual, - "limit": 1.0, - "comparison": "maximum", - } - for cycle, actual in enumerate((3.9, 4.1, 4.0), start=1) - ] - assert _fit_failure_is_systematic(failures, 3) - transitions: list[str] = [] - pauses: list[str] = [] - node = SimpleNamespace( - sweep_items=_sweep_items(), - sweep_attempts={spec.motor_index: 1}, - retry_sweep_spec=None, - retry_resume_index=None, - fit_failure={}, - sweep_index=18, - raw_path=tmp_path / "raw_samples.jsonl", - repetitions=3, - automatic_fit_retry_limit=2, - paused_reason="", - reason="", - _begin_return_baseline=lambda after: transitions.append(after), - _pause=lambda reason: pauses.append(reason), - ) - - G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, spec, failures - ) - - assert transitions == [] - assert pauses == ["joint_fit_systematic_failure"] - assert node.fit_failure["recoverable_by_rescan"] is False - assert node.fit_failure["directions_to_rescan"] == 0 - - -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", - provisional_warning_ratio=1.25, - reason="", - sweep_attempts={}, - retry_resume_index=None, - retry_sweep_spec=None, - retry_cycles=set(), - repetitions=3, - fit_failure={}, - sweep_index=0, - automatic_fit_retry_limit=2, - paused_reason="", - _prepare_failed_sweep_retry=lambda: None, - _begin_return_baseline=lambda after: None, - _pause=lambda reason: None, - ) - handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, - spec, - [ - { - "joint": "index_mcp_roll", - "metric": "hysteresis_deg", - "actual": 5.5, - "limit": 5.0, - "comparison": "maximum", - } - ], - allow_warning=True, - ) - - assert handled is True - events = [ - json.loads(line) - for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines() - ] - kinds = [event["kind"] for event in events] - assert "provisional_fit_warning_retry_exhausted" in kinds - assert "fit_failure" in kinds - - -def test_motion_timeout_pauses_without_republishing(tmp_path) -> None: - commands: list[list[int]] = [] - pauses: list[str] = [] - holds: list[bool] = [] - node = SimpleNamespace( - motion_retry_counts={}, - automatic_motion_retry_limit=2, - raw_path=tmp_path / "raw_samples.jsonl", - state="RETURN_BASELINE", - baseline_command=tuple([255] * 20), - position_hold_since=1.0, - reason="", - _publish_hold_current=lambda: holds.append(True), - _publish_speed_profile=lambda profile: None, - _normal_speed_profile=lambda: [15] * 5, - _publish_command=lambda command: commands.append(command), - _pause=lambda reason: pauses.append(reason), - _baseline_error_u8=lambda: 50.0, - _reset_motion_progress=lambda now, error: None, - ) - - for now in (10.0, 20.0, 30.0): - G20ThreeCameraCalibrationNode._retry_motion_or_pause( - node, "return_baseline_timeout", now - ) - - assert commands == [] - assert holds == [] - assert pauses == ["return_baseline_timeout"] * 3 - - -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 - if item.key == "ring_pitch_side" - ) - item = SweepItem(spec, 0, DIRECTION_DECREASING) - runtime = SimpleNamespace(pnp_invalid_since=10.0, pnp_reset_count=0) - 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, - active_sweep=item, - active_sweep_is_fit_retry=False, - views={"side": runtime}, - sweep_start_frames=start_frames, - motion_retry_counts={}, - automatic_motion_retry_limit=2, - raw_path=tmp_path / "raw_samples.jsonl", - state=STATE_PREPARE_SWEEP, - position_hold_since=1.0, - reason="", - preparation_command_u8=tuple([255] * 20), - baseline_command=tuple([255] * 20), - latest_state_u8=tuple([255] * 20), - _reset_view_trackers=lambda selected, preserve_task_reference=False: ( - resets.append((selected, preserve_task_reference)) - ), - _reset_view_pnp_diagnostics=lambda selected: ( - diagnostic_resets.append(selected) - ), - _command_vector_error_u8=lambda command: 0.0, - _normal_speed_profile=lambda: [15] * 5, - _publish_speed_profile=lambda profile: None, - _publish_command=lambda command: commands.append(command), - _reset_motion_progress=lambda now, error: None, - _pause=lambda reason: pauses.append(reason), - _begin_active_sweep=lambda now: starts.append(now), - ) - - G20ThreeCameraCalibrationNode._handle_sweep_start_timeout( - node, 20.0, True - ) - - assert starts == [20.0] - assert pauses == [] - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "sweep_start_vision_timeout_warning" - - -def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None: - pauses: list[str] = [] - node = SimpleNamespace( - raw_path=tmp_path / "raw_samples.jsonl", - latest_state_u8=tuple([18.0] + [255.0] * 19), - motion_progress_reference_error_u8=18.0, - motion_last_progress_at=10.0, - motor_stall_timeout_seconds=8.0, - motor_stall_minimum_progress_u8=1.0, - _pause=lambda reason: pauses.append(reason), - _reset_motion_progress=lambda now, error: None, - ) - - assert not G20ThreeCameraCalibrationNode._pause_if_motion_stalled( - node, now=17.9, error_u8=18.0, context="sweep_motor_0" - ) - assert G20ThreeCameraCalibrationNode._pause_if_motion_stalled( - node, now=18.0, error_u8=18.0, context="sweep_motor_0" - ) - - assert pauses == [ - "motor_state_stalled:sweep_motor_0:timeout_seconds=8.000:" - "error_u8=18.000" - ] - event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) - assert event["kind"] == "mechanical_motion_stall" - assert event["context"] == "sweep_motor_0" - - -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) - node = SimpleNamespace( - active_sweep=item, - sweep_timeout_seconds=90.0, - sweep_retry_counts={key: 1}, - retry_speed_scales=(0.8, 0.6, 0.5), - ) - - assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds( - node - ) == 90.0 - node.sweep_retry_counts[key] = 3 - assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds( - node - ) == 90.0 - - -def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) - node = _fit_check_node( - { - "thumb_cmc_pitch": _image_cycle_records( - [math.radians(47.0), math.radians(47.2), math.radians(55.0)] - ) - } - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - cycle_failure = next( - item - for item in failures - if item["metric"] == "cycle_travel_range_deg" - ) - assert cycle_failure["joint"] == "thumb_cmc_pitch" - assert cycle_failure["actual"] == 8.0 - assert cycle_failure["limit"] == 3.0 - - -def test_provisional_fit_accepts_constrained_circle_for_image_joint() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) - node = _fit_check_node( - { - "thumb_cmc_pitch": _image_cycle_records( - [math.radians(47.0)] * 3, - depth_slope=2.00, - ) - } - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] - in { - "rotation_circle_axis_difference_deg", - "axis_cycle_difference_deg", - "axis_plane_rms_mm", - "axis_radial_rms_mm", - } - ] - - -def test_axis_cycle_spread_localizes_one_outlier_for_four_rounds() -> None: - axes = [ - np.asarray([-0.05713560, -0.06623617, -0.99616680]), - np.asarray([-0.05620951, -0.05515776, -0.99689423]), - np.asarray([-0.05700579, -0.05346630, -0.99694117]), - np.asarray([-0.05589426, -0.05231845, -0.99706500]), - ] - - assert _isolated_axis_cycle_outliers( - axes, math.radians(0.75) - ) == {0} - - -def test_thumb_yaw_zero_spread_localizes_181154_second_cycle() -> None: - cycle_offsets = [ - math.radians(-2.518678), - math.radians(-3.098037), - math.radians(-2.544640), - ] - result = SimpleNamespace( - cycle_offsets_rad={"thumb_cmc_yaw": cycle_offsets}, - offset_confidence_half_width_rad={ - "thumb_cmc_yaw": math.radians(0.81295) - }, - ) - - assert _isolated_scalar_cycle_outlier( - cycle_offsets, math.radians(0.5) - ) == {1} - assert _thumb_yaw_zero_repeatability_failures( - result, - maximum_cycle_range_rad=math.radians(0.5), - maximum_confidence_half_width_rad=math.radians(0.75), - ) == [ - { - "joint": "thumb_cmc_yaw", - "metric": "zero_cycle_offset_range_deg", - "actual": 0.579359, - "limit": 0.5, - "comparison": "maximum", - "cycle_offset_deg": [-2.518678, -3.098037, -2.54464], - "cycle": 2, - "inlier_cycles": [1, 3], - } - ] - - -def test_thumb_yaw_zero_spread_localizes_142322_marginal_cycle() -> None: - cycle_offsets = [ - math.radians(-2.589022), - math.radians(-2.915619), - math.radians(-2.391267), - ] - result = SimpleNamespace( - cycle_offsets_rad={"thumb_cmc_yaw": cycle_offsets}, - offset_confidence_half_width_rad={ - "thumb_cmc_yaw": math.radians(0.658) - }, - ) - - assert _isolated_scalar_cycle_outlier( - cycle_offsets, math.radians(0.5) - ) == {1} - failures = _thumb_yaw_zero_repeatability_failures( - result, - maximum_cycle_range_rad=math.radians(0.5), - maximum_confidence_half_width_rad=math.radians(0.75), - ) - assert failures[0]["actual"] == 0.524352 - assert failures[0]["cycle"] == 2 - assert failures[0]["inlier_cycles"] == [1, 3] - - -def test_thumb_yaw_zero_spread_keeps_symmetric_drift_nonlocalized() -> None: - cycle_offsets = [ - math.radians(-2.76), - math.radians(-2.50), - math.radians(-2.24), - ] - - assert _isolated_scalar_cycle_outlier( - cycle_offsets, math.radians(0.5) - ) == set() - - -def test_thumb_yaw_zero_confidence_failure_requests_full_yaw_retry() -> None: - result = SimpleNamespace( - cycle_offsets_rad={ - "thumb_cmc_yaw": [ - math.radians(-2.50), - math.radians(-2.70), - math.radians(-2.30), - ] - }, - offset_confidence_half_width_rad={ - "thumb_cmc_yaw": math.radians(0.80) - }, - ) - - assert _thumb_yaw_zero_repeatability_failures( - result, - maximum_cycle_range_rad=math.radians(0.5), - maximum_confidence_half_width_rad=math.radians(0.75), - ) == [ - { - "joint": "thumb_cmc_yaw", - "metric": "zero_confidence_95_half_width_deg", - "actual": 0.8, - "limit": 0.75, - "comparison": "maximum", - } - ] - - -def test_thumb_yaw_zero_repeatability_accepts_171406_quality() -> None: - result = SimpleNamespace( - cycle_offsets_rad={ - "thumb_cmc_yaw": [ - math.radians(-2.53708), - math.radians(-2.69820), - math.radians(-2.29168), - ] - }, - offset_confidence_half_width_rad={ - "thumb_cmc_yaw": math.radians(0.50853) - }, - ) - - assert _thumb_yaw_zero_repeatability_failures( - result, - maximum_cycle_range_rad=math.radians(0.5), - maximum_confidence_half_width_rad=math.radians(0.75), - ) == [] - - -def test_thumb_yaw_zero_outlier_retries_only_localized_source_cycle( - tmp_path, -) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_yaw_top" - ) - failures = _thumb_yaw_zero_repeatability_failures( - SimpleNamespace( - cycle_offsets_rad={ - "thumb_cmc_yaw": [ - math.radians(-2.518678), - math.radians(-3.098037), - math.radians(-2.544640), - ] - }, - offset_confidence_half_width_rad={ - "thumb_cmc_yaw": math.radians(0.81295) - }, - ), - maximum_cycle_range_rad=math.radians(0.5), - maximum_confidence_half_width_rad=math.radians(0.75), - ) - calls: list[str] = [] - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - raw_path=tmp_path / "raw_samples.jsonl", - sweep_attempts={}, - retry_resume_index=None, - retry_sweep_spec=None, - retry_cycles=set(), - retry_joint_names=set(), - fit_failure={}, - sweep_index=96, - repetitions=3, - automatic_fit_retry_limit=2, - retry_source_failure_task_key=spec.key, - retry_source_task_keys=( - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - ), - retry_cycle_override=set(), - paused_reason="", - reason="", - _prepare_failed_sweep_retry=lambda: calls.append("prepare"), - _begin_return_baseline=lambda after: calls.append(after), - _pause=lambda reason: calls.append(f"pause:{reason}"), - ) - - handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, spec, failures - ) - - assert handled is True - assert node.retry_cycles == {1} - assert node.retry_joint_names == { - "thumb_cmc_pitch", - "thumb_cmc_roll", - } - assert node.fit_failure["cycles_to_rescan"] == [2] - assert node.fit_failure["directions_to_rescan"] == 4 - assert node.fit_failure["source_task_names"] == [ - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - ] - assert calls == ["pause:joint_fit_check_failed"] - - -def test_previous_passed_joint_zero_offset_reads_formal_pointer( - tmp_path, -) -> None: - root = tmp_path / "G20_RIGHT_001" - previous = root / "20260830_171406" - current = root / "20260831_100000" - previous.mkdir(parents=True) - current.mkdir() - payload = { - "joints": { - "thumb_cmc_yaw": { - "zero_angles": {"urdf_zero_offset_rad": -0.04736713} - } - } - } - (previous / "g20_right_G20_RIGHT_001_calibration.json").write_text( - json.dumps(payload), encoding="utf-8" - ) - (root / "latest_passed").symlink_to(previous, target_is_directory=True) - - assert _previous_passed_joint_zero_offset( - current, "G20_RIGHT_001", "thumb_cmc_yaw" - ) == (previous, -0.04736713) - - -def test_axis_line_spread_localizes_one_outlier_for_four_rounds() -> None: - axes = [ - [-0.043565, 0.036698, -0.998376], - [-0.043462, 0.047539, -0.997923], - [-0.045335, 0.041627, -0.998104], - [-0.043689, 0.044394, -0.998058], - ] - points_mm = [ - [-43.981, 52.532, 1101.515], - [-43.999, 50.684, 1101.530], - [-44.080, 49.366, 1101.310], - [-44.252, 50.672, 1101.511], - ] - measurements = [ - SimpleNamespace( - axis_common_xyz=axis, - point_common_xyz_m=[0.001 * value for value in point], - ) - for axis, point in zip(axes, points_mm) - ] - - assert _isolated_axis_line_cycle_outliers( - measurements, 0.001 - ) == {0} - - -def test_axis_line_spread_does_not_localize_ambiguous_drift() -> None: - measurements = [ - SimpleNamespace( - axis_common_xyz=[0.0, 0.0, 1.0], - point_common_xyz_m=[0.0, offset_m, 0.0], - ) - for offset_m in (0.0, 0.001, 0.002, 0.003) - ] - - assert not _isolated_axis_line_cycle_outliers( - measurements, 0.001 - ) - - -def test_fit_retry_preserves_first_cycle_pnp_task_reference() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) - - assert _preserve_pnp_task_reference_for_sweep( - SweepItem(spec, 0, DIRECTION_DECREASING), - is_fit_retry=True, - has_precheck_anchor=False, - ) - assert _preserve_pnp_task_reference_for_sweep( - SweepItem(spec, 0, DIRECTION_DECREASING), - is_fit_retry=False, - has_precheck_anchor=True, - ) - assert not _preserve_pnp_task_reference_for_sweep( - SweepItem(spec, 0, DIRECTION_DECREASING), - is_fit_retry=False, - has_precheck_anchor=False, - ) - assert _preserve_pnp_task_reference_for_sweep( - SweepItem(spec, 1, DIRECTION_DECREASING), - is_fit_retry=False, - has_precheck_anchor=False, - ) - - -def test_provisional_axis_spread_marks_only_the_isolated_round() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) - records = _image_cycle_records([math.radians(47.0)] * 4) - node = _fit_check_node({"thumb_cmc_pitch": records}) - node.repetitions = 4 - axes = [ - [-0.05713560, -0.06623617, -0.99616680], - [-0.05620951, -0.05515776, -0.99689423], - [-0.05700579, -0.05346630, -0.99694117], - [-0.05589426, -0.05231845, -0.99706500], - ] - node._fit_axis_measurement = lambda name, cycle: SimpleNamespace( - axis_common_xyz=axes[cycle], - axis_direction_source="pose_rotation_3d", - radial_rms_m=0.0001, - pose_axis_line_rms_m=0.0001, - plane_rms_m=0.0001, - rotation_circle_axis_difference_rad=0.0, - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - - spread = next( - failure - for failure in failures - if failure["metric"] == "axis_cycle_difference_deg" - ) - assert spread["cycle"] == 1 - assert spread["inlier_cycles"] == [2, 3, 4] - - -def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - name = "pinky_mcp_roll_side" - spec = replace(spec, joints=(name,)) - records = _image_cycle_records( - [math.radians(47.0)] * 3, - depth_slope=2.0, - ) - node = _fit_check_node({name: records}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {name: records} - node.minimum_detection_rate = 0.95 - node.views = { - "side": SimpleNamespace( - valid_rate=0.9453, - task_valid_frames=2653, - task_total_frames=2807, - ) - } - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] - in { - "rotation_circle_axis_difference_deg", - "axis_plane_rms_mm", - "tag_valid_rate_percent", - } - ] - - -def test_thumb_ip_axis_uses_same_cycle_thumb_mcp_direction() -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - node = _fit_check_node( - { - "thumb_cmc_pitch": records, - "thumb_mcp": records, - "thumb_ip": records, - } - ) - - upstream = node._fit_axis_measurement("thumb_mcp", 0) - passive = node._fit_axis_measurement("thumb_ip", 0) - - assert upstream.condition_command_u8 is not None - assert upstream.condition_command_u8[15] == 255.0 - assert abs( - float( - np.asarray(upstream.axis_common_xyz) - @ np.asarray(passive.axis_common_xyz) - ) - ) > math.cos(math.radians(0.01)) - - -def test_provisional_fit_uses_orientation_constrained_circle_for_yaw() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 10) - node = _fit_check_node( - { - "thumb_cmc_yaw": _image_cycle_records( - [math.radians(47.0)] * 3, - depth_slope=0.30, - ) - } - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] - in { - "rotation_circle_axis_difference_deg", - "axis_cycle_difference_deg", - "axis_plane_rms_mm", - "axis_radial_rms_mm", - } - ] - - -def test_right_19_side_flexion_uses_end_on_orientation_axis() -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_pitch_side" - ) - name = "pinky_mcp_pitch" - records = _image_cycle_records( - [math.radians(47.0)] * 3, - depth_slope=0.30, - ) - node = _fit_check_node({name: records}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - - measurement = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - node, name, 0 - ) - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert measurement.axis_direction_source == "rotation" - assert not [ - failure - for failure in failures - if failure["metric"] - in { - "rotation_circle_axis_difference_deg", - "axis_plane_rms_mm", - } - ] - - -def test_provisional_fit_uses_active_and_passive_axis_model_limits() -> None: - spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) - records = _image_cycle_records([math.radians(47.0)] * 3) - node = _fit_check_node( - { - "thumb_mcp": records, - "thumb_ip": records, - } - ) - residuals_deg = {"thumb_mcp": 2.0, "thumb_ip": 6.0} - - def fitted(name, selected, relaxed=False): - fit = G20ThreeCameraCalibrationNode._fit_joint_records( - node, name, selected, relaxed=relaxed - ) - quality = dict(fit.quality) - quality["rotation_orthogonal_rms_rad"] = math.radians( - residuals_deg[name] - ) - return replace(fit, quality=quality) - - node._fit_joint_records = fitted - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - assert not [ - failure - for failure in failures - if failure["metric"] == "rotation_orthogonal_rms_deg" - ] - - residuals_deg["thumb_mcp"] = 3.0 - residuals_deg["thumb_ip"] = 8.0 - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - axis_failures = [ - failure - for failure in failures - if failure["metric"] == "rotation_orthogonal_rms_deg" - ] - assert {(item["joint"], item["limit"]) for item in axis_failures} == { - ("thumb_mcp", 2.5), - ("thumb_ip", 7.5), - } - - -def _axis_cycle_records( - travel_rad: float, - axis_xyz: list[float], - *, - cycles: int = 3, - offset_xyz: list[float] | None = None, -) -> list[dict]: - """Synthetic sweeps rotating rigidly about an arbitrary axis.""" - commands = list(range(0, 256, 16)) + [255] - axis = np.asarray(axis_xyz, dtype=float) - axis = axis / np.linalg.norm(axis) - reference = ( - np.array([0.0, 0.0, 1.0]) - if abs(float(axis @ np.array([0.0, 0.0, 1.0]))) < 0.9 - else np.array([0.0, 1.0, 0.0]) - ) - u = np.cross(reference, axis) - u = u / np.linalg.norm(u) - v = np.cross(axis, u) - offset = ( - np.zeros(3) if offset_xyz is None else np.asarray(offset_xyz, float) - ) - records = [] - for cycle in range(cycles): - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): - for command in commands: - angle = travel_rad * (255.0 - command) / 255.0 - rotation = Rotation.from_rotvec(axis * angle) - centre = ( - offset - + 0.03 * (u * math.cos(angle) + v * math.sin(angle)) - ) - records.append( - { - "cycle": cycle, - "direction": direction, - "command_u8": command, - "relative_translation_xyz_m": centre.tolist(), - "image_relative_xy_px": [ - 100.0 * math.cos(angle), - 100.0 * math.sin(angle), - ], - "relative_quaternion_xyzw": ( - rotation.as_quat().tolist() - ), - "parent_pose_common": { - "translation_xyz_m": [0.0, 0.0, 0.0], - "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], - }, - "child_pose_common": { - "translation_xyz_m": centre.tolist(), - "quaternion_xyzw": ( - rotation.as_quat().tolist() - ), - }, - "state_u8": [255.0] * 9 - + [float(command)] - + [255.0] * 10, - } - ) - return records - - -def _cross_view_roll_node( - tmp_path, side_tilt_deg: float, side_offset_m: list[float] | None = None -) -> SimpleNamespace: - travel = math.radians(25.0) - front = _axis_cycle_records(travel, [0.0, 0.0, 1.0]) - tilt = math.radians(side_tilt_deg) - side = _axis_cycle_records( - travel, - [math.sin(tilt), 0.0, math.cos(tilt)], - offset_xyz=side_offset_m, - ) - node = _fit_check_node( - {"pinky_mcp_roll": front, "pinky_mcp_roll_side": side} - ) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = { - "pinky_mcp_roll": front, - "pinky_mcp_roll_side": side, - } - node.raw_path = tmp_path / "raw_samples.jsonl" - # The production node always owns validated camera extrinsics. Keep the - # synthetic camera centre away from the synthetic axis origin so the - # interpretation-plane geometry is well defined as it is on real data. - node.extrinsics = SimpleNamespace( - transform=lambda _view: np.asarray( - [ - [1.0, 0.0, 0.0, 0.1], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0], - ], - dtype=float, - ) - ) - return node - - -def test_cross_view_roll_axis_disagreement_skips_fusion(tmp_path) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) - - measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - - # 11.4 deg disagreement was the stable signature of the side-view IPPE - # bias in session 20260820_105535: it must keep the trusted front axis - # and record a diagnostic instead of failing the joint. - assert measurement.axis_direction_source != "cross_view_weighted_fusion" - assert measurement.pose_axis_line_source_joints == ( - "pinky_mcp_roll", - ) - assert measurement.axis_point_source == ( - "front_interpretation_plane_cross_view_validated" - ) - assert axis_line_uses_depth_free_interpretation_plane(measurement) - diagnostics = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert diagnostics[-1]["kind"] == "cross_view_roll_axis_diagnostic" - assert diagnostics[-1]["decision"] == ( - "diagnostic_only_pose_disagreement_use_primary" - ) - assert 10.0 < diagnostics[-1]["axis_difference_deg"] < 12.5 - - -def test_cross_view_depth_free_axis_group_is_not_radius_refitted(tmp_path) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) - measurements = [ - node._fit_axis_measurement("pinky_mcp_roll", cycle) - for cycle in range(node.repetitions) - ] - - refined = G20ThreeCameraCalibrationNode._refit_cross_view_axis_line_group( - node, "pinky_mcp_roll", measurements - ) - - assert refined == measurements - assert all( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in refined - ) - - -def test_cross_view_depth_free_axis_skips_point_repeatability_rescan( - tmp_path, monkeypatch -) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) - original_fit_axis = node._fit_axis_measurement - - def displaced_side_line(name, cycle): - measurement = original_fit_axis(name, cycle) - if name == "pinky_mcp_roll" and cycle == node.repetitions - 1: - point = np.asarray(measurement.point_common_xyz_m, dtype=float) - point[0] += 0.0024 - return replace( - measurement, - point_common_xyz_m=tuple(float(value) for value in point), - ) - return measurement - - node._fit_axis_measurement = displaced_side_line - # A depth-free interpretation plane does not define one Euclidean line - # point, so point-only cycle RMS must not trigger a misleading rescan. - monkeypatch.setitem( - G20ThreeCameraCalibrationNode._provisional_fit_failures.__globals__, - "_isolated_axis_line_cycle_outliers", - lambda measurements, limit_m: pytest.fail( - "depth-free axes must not enter the line-point outlier gate" - ), - ) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - line_failures = [ - failure - for failure in failures - if failure["metric"] == "axis_line_cycle_rms_mm" - ] - - assert not line_failures - - -def test_cross_view_front_pose_residual_remains_a_hard_gate( - tmp_path, -) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) - original_fit_axis = node._fit_axis_measurement - - def biased_but_repeatable_side_pose(name, cycle): - measurement = original_fit_axis(name, cycle) - if name == "pinky_mcp_roll": - return replace(measurement, pose_axis_line_rms_m=0.00125) - return measurement - - node._fit_axis_measurement = biased_but_repeatable_side_pose - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - - pose_failures = [ - failure - for failure in failures - if failure["metric"] == "axis_pose_line_rms_mm" - ] - assert pose_failures - assert all( - failure["joint"] == "pinky_mcp_roll" - and failure["quality_source_joints"] == ["pinky_mcp_roll"] - for failure in pose_failures - ) - assert not [ - failure - for failure in failures - if failure["metric"] == "axis_line_cycle_rms_mm" - ] - - -def test_cross_view_roll_systematic_line_offset_skips_fusion(tmp_path) -> None: - # Ring session 20260824_155323: the front screw-driven pseudo-line and - # side PIP-link physical line sit 36.6-38.2 mm apart while both views are - # individually clean. This finger-dependent offset must remain a - # diagnostic and must not reject the trusted-front/side-line fallback. - node = _cross_view_roll_node( - tmp_path, side_tilt_deg=11.0, side_offset_m=[0.038, 0.0, 0.0] - ) - - measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - - assert measurement.axis_direction_source != "cross_view_weighted_fusion" - diagnostics = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert diagnostics[-1]["kind"] == "cross_view_roll_axis_diagnostic" - assert 34.0 < diagnostics[-1]["line_distance_mm"] < 42.0 - assert 10.0 < diagnostics[-1]["axis_difference_deg"] < 12.5 - - -def test_cross_view_roll_diagnostic_uses_real_node_logger(tmp_path) -> None: - # The live node exposes get_logger as a method returning the logger; - # recording the diagnostic must call it instead of treating the bound - # method itself as the logger (regression: session 20260820_133434 - # turned this AttributeError into four axis_fit failures). - node = _cross_view_roll_node(tmp_path, side_tilt_deg=11.0) - warnings: list[str] = [] - node.get_logger = lambda: SimpleNamespace(warning=warnings.append) - - measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - - assert measurement.axis_direction_source != "cross_view_weighted_fusion" - assert any("disagree" in message for message in warnings) - - -def test_cross_view_roll_axis_gross_disagreement_is_diagnostic_only( - tmp_path, -) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=20.0) - - measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - - assert measurement.axis_direction_source == "rotation" - diagnostics = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert diagnostics[-1]["decision"] == ( - "diagnostic_only_gross_pose_disagreement_use_primary" - ) - - -def test_cross_view_roll_gross_disagreement_does_not_rescan(tmp_path) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=20.0) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - gross_failures = [ - failure - for failure in failures - if str(failure.get("reason", "")).startswith( - "cross_view_roll_axis_gross_disagreement:" - ) - ] - - assert not gross_failures - diagnostics = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert any( - row.get("decision") - == "diagnostic_only_gross_pose_disagreement_use_primary" - for row in diagnostics - ) - - -def test_cross_view_roll_axis_agreement_keeps_published_primary(tmp_path) -> None: - node = _cross_view_roll_node(tmp_path, side_tilt_deg=0.2) - - measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - - assert measurement.axis_direction_source == "rotation" - assert measurement.pose_axis_line_source_joints == ( - "pinky_mcp_roll", - ) - assert measurement.axis_point_source == ( - "front_interpretation_plane_cross_view_validated" - ) - - -def test_cross_view_roll_side_pose_never_replaces_primary_direction( - tmp_path, monkeypatch -) -> None: - # Regression for session 20260824_171302. Three side-view cone - # residuals were just inside 5 deg and one was 5.037 deg. Selecting the - # camera per cycle mixed three side axes with one front axis and created a - # false 3.24 deg cycle spread although each camera was internally stable. - names = ( - "middle_mcp_roll", - "middle_mcp_roll_side", - "middle_mcp_pitch", - ) - node = SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - zero_profile=get_zero_calibration_profile( - "right", "g20_right_19" - ), - repetitions=4, - records_by_joint={ - name: [{"cycle": cycle} for cycle in range(4)] - for name in names - }, - source_urdf_path=tmp_path / "unused.urdf", - raw_path=tmp_path / "raw_samples.jsonl", - zero_maximum_axis_cycle_difference_rad=math.radians(0.75), - zero_maximum_axis_cone_mismatch_rad=math.radians(5.0), - axis_maximum_pose_line_rms_m=0.001, - cross_view_roll_maximum_axis_difference_rad=math.radians(15.0), - extrinsics=SimpleNamespace( - transform=lambda _view: np.asarray( - [ - [1.0, 0.0, 0.0, 0.1], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0], - ], - dtype=float, - ) - ), - ) - primary_degrees = (8.0078, 8.1710, 8.0712, 8.0236) - secondary_degrees = (4.9043, 4.9136, 5.0374, 4.9389) - - def axis(degrees: float) -> tuple[float, float, float]: - angle = math.radians(degrees) - return (math.sin(angle), 0.0, math.cos(angle)) - - def measurement(name: str, cycle: int) -> JointAxisMeasurement: - if name == "middle_mcp_roll": - direction = axis(primary_degrees[cycle]) - elif name == "middle_mcp_roll_side": - direction = axis(secondary_degrees[cycle]) - else: - direction = (1.0, 0.0, 0.0) - return JointAxisMeasurement( - joint=name, - cycle=cycle, - axis_common_xyz=direction, - point_common_xyz_m=(0.0, 0.0, 0.0), - condition_state_u8=(255.0,) * 20, - plane_rms_m=0.0001, - radial_rms_m=0.0001, - rotation_circle_axis_difference_rad=0.0, - axis_direction_source="rotation", - pose_axis_line_rms_m=0.0001, - ) - - class Model: - def __init__(self, unused_path) -> None: - pass - - def axis_line(self, name, *, zero_offsets, joint_angles): - direction = ( - np.asarray([0.0, 0.0, 1.0]) - if name == "middle_mcp_roll" - else np.asarray([1.0, 0.0, 0.0]) - ) - return direction, np.zeros(3) - - monkeypatch.setattr( - G20ThreeCameraCalibrationNode, - "_fit_axis_measurement_raw", - lambda unused_self, name, cycle: measurement(name, cycle), - ) - monkeypatch.setitem( - G20ThreeCameraCalibrationNode._fit_axis_measurement.__globals__, - "UrdfKinematicModel", - Model, - ) - - measured = [ - G20ThreeCameraCalibrationNode._fit_axis_measurement( - node, "middle_mcp_roll", cycle - ) - for cycle in range(4) - ] - - assert { - item.axis_direction_source for item in measured - } == {"rotation"} - maximum_spread = max( - math.acos( - abs( - float( - np.clip( - np.asarray(left.axis_common_xyz) - @ np.asarray(right.axis_common_xyz), - -1.0, - 1.0, - ) - ) - ) - ) - for left in measured - for right in measured - ) - assert math.degrees(maximum_spread) < 0.75 - - -def test_side_alias_branch_gap_range_is_diagnostic_only( - tmp_path, monkeypatch -) -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - gaps_deg = [0.10, 0.81, 0.10] # range 0.71 deg, maximum below 2 deg - monkeypatch.setattr( - "linkerhand_calibration.three_camera_node" - ".baseline_hysteresis_by_cycle_rad", - lambda records, zero_command_u8, axis_xyz: [ - math.radians(value) for value in gaps_deg - ], - ) - multiview = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - - def node_for(joints): - node = _fit_check_node({name: records for name in joints}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {name: records for name in joints} - node.raw_path = tmp_path / f"raw_{joints[0]}.jsonl" - return node - - alias_failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node_for(("pinky_mcp_roll_side",)), - replace(multiview, joints=("pinky_mcp_roll_side",)), - ) - assert not [ - failure - for failure in alias_failures - if failure["metric"] == "baseline_directional_gap_range_deg" - ] - alias_diagnostics = [ - json.loads(line) - for line in (tmp_path / "raw_pinky_mcp_roll_side.jsonl") - .read_text() - .splitlines() - ] - assert any( - row.get("kind") == "validation_only_quality_diagnostic" - and row.get("metric") == "baseline_directional_gap_range_deg" - and row.get("actual") == 0.71 - and row.get("reference_limit") == 0.3 - for row in alias_diagnostics - ) - - canonical_failures = ( - G20ThreeCameraCalibrationNode._provisional_fit_failures( - node_for(("pinky_mcp_roll",)), - replace(multiview, joints=("pinky_mcp_roll",)), - ) - ) - range_failure = next( - failure - for failure in canonical_failures - if failure["metric"] == "baseline_directional_gap_range_deg" - ) - assert range_failure["limit"] == 0.3 - - -def test_side_alias_axis_cycle_spread_is_diagnostic_only(tmp_path) -> None: - records = _image_cycle_records([math.radians(47.0)] * 4) - multiview = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - name = "pinky_mcp_roll_side" - node = _fit_check_node({name: records}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.repetitions = 4 - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {name: records} - node.raw_path = tmp_path / "raw_axis_alias.jsonl" - wrapped = node._fit_axis_measurement - axes = [ - np.asarray([0.0, 0.0, 1.0]), - np.asarray( - [ - 0.0, - math.sin(math.radians(1.1)), - math.cos(math.radians(1.1)), - ] - ), - np.asarray([0.0, 0.0, 1.0]), - np.asarray([0.0, 0.0, 1.0]), - ] - - def spread_axis(joint_name, cycle): - return replace( - wrapped(joint_name, cycle), - axis_common_xyz=axes[cycle], - radial_rms_m=0.0001, - pose_axis_line_rms_m=0.0001, - plane_rms_m=0.0001, - rotation_circle_axis_difference_rad=0.0, - ) - - node._fit_axis_measurement = spread_axis - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, replace(multiview, joints=(name,)), include_view_validity=False - ) - - assert not [ - failure - for failure in failures - if failure["metric"] == "axis_cycle_difference_deg" - ] - diagnostics = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert any( - row.get("kind") == "validation_only_quality_diagnostic" - and row.get("metric") == "axis_cycle_difference_deg" - and row.get("actual") == 1.1 - and row.get("reference_limit") == 0.75 - for row in diagnostics - ) - - -def test_side_alias_skips_pose_line_rms_gate(tmp_path) -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - multiview = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_roll_multiview" - ) - - def node_for(joints): - node = _fit_check_node({name: records for name in joints}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {name: records for name in joints} - node.raw_path = tmp_path / f"raw_pose_{joints[0]}.jsonl" - wrapped = node._fit_axis_measurement - - def inflated(name, cycle): - measurement = wrapped(name, cycle) - if name in joints: - return replace(measurement, pose_axis_line_rms_m=0.002) - return measurement - - node._fit_axis_measurement = inflated - return node - - alias_failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node_for(("pinky_mcp_roll_side",)), - replace(multiview, joints=("pinky_mcp_roll_side",)), - ) - assert not [ - failure - for failure in alias_failures - if failure["metric"] == "axis_pose_line_rms_mm" - ] - - canonical_failures = ( - G20ThreeCameraCalibrationNode._provisional_fit_failures( - node_for(("pinky_mcp_roll",)), - replace(multiview, joints=("pinky_mcp_roll",)), - ) - ) - assert [ - failure - for failure in canonical_failures - if failure["metric"] == "axis_pose_line_rms_mm" - ] - - -def test_right_19_thumb_ip_pose_line_is_position_diagnostic_only( - tmp_path, -) -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_mcp_ip_front" - ) - node = _fit_check_node( - {"thumb_mcp": records, "thumb_ip": records} - ) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_19" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = { - "thumb_mcp": records, - "thumb_ip": records, - } - node.raw_path = tmp_path / "raw_thumb_position.jsonl" - - def axis_measurement(name, cycle): - return SimpleNamespace( - axis_common_xyz=(0.0, 0.0, 1.0), - axis_direction_source="upstream_constraint", - radial_rms_m=0.0002, - pose_axis_line_rms_m=( - 0.0014 if name == "thumb_ip" else 0.0002 - ), - plane_rms_m=0.0002, - rotation_circle_axis_difference_rad=0.0, - ) - - node._fit_axis_measurement = axis_measurement - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - - assert not [ - failure - for failure in failures - if failure["joint"] == "thumb_ip" - and failure["metric"] == "axis_pose_line_rms_mm" - ] - diagnostics = [ - json.loads(line) - for line in node.raw_path.read_text().splitlines() - ] - assert len(diagnostics) == 3 - assert all( - row["kind"] == "position_invariant_quality_diagnostic" - and row["joint"] == "thumb_ip" - and row["metric"] == "axis_pose_line_rms_mm" - and row["actual"] == 1.4 - and row["reference_limit"] == 1.0 - and row["decision"] == "diagnostic_only" - for row in diagnostics - ) - - -def test_right_19_passive_dip_requires_axis_point_for_pip_zero_phase() -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - node = _fit_check_node( - {"pinky_pip": records, "pinky_dip": records} - ) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_19" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = { - "pinky_pip": records, - "pinky_dip": records, - } - - def axis_measurement(name, cycle): - is_dip = name in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS - return SimpleNamespace( - axis_common_xyz=(0.0, 0.0, 1.0), - axis_direction_source="upstream_constraint", - radial_rms_m=0.0002, - pose_axis_line_rms_m=0.002 if is_dip else 0.0002, - plane_rms_m=0.006 if is_dip else 0.0002, - rotation_circle_axis_difference_rad=( - math.radians(16.0) if is_dip else math.radians(0.1) - ), - ) - - node._fit_axis_measurement = axis_measurement - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "pinky_pip_side" - ) - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - - pose_failures = [ - failure - for failure in failures - if failure["joint"] == "pinky_dip" - and failure["metric"] == "axis_pose_line_rms_mm" - ] - assert len(pose_failures) == 3 - assert not [ - failure - for failure in failures - if failure["joint"] == "pinky_dip" - and failure["metric"] - in {"axis_plane_rms_mm", "rotation_circle_axis_difference_deg"} - ] - - -def _ring_group_end_state() -> list[float]: - """Ring-group avoidance pose: pinky flexed, index/middle rolls parked.""" - state = [float(value) for value in THREE_CAMERA_BASELINE_COMMAND] - state[4] = 0.0 # pinky MCP pitch flexed - state[6] = 254.0 # index roll parked (feedback one count low) - state[7] = 253.0 # middle roll parked - state[19] = 0.0 # pinky PIP flexed - return state - - -def _cross_group_node() -> SimpleNamespace: - middle_roll = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "middle_roll_multiview" - ) - return SimpleNamespace( - profile=RIGHT_19_HAND_PROFILE, - baseline_command=list(THREE_CAMERA_BASELINE_COMMAND), - sweep_items=[SweepItem(middle_roll, 0, DIRECTION_DECREASING)], - sweep_index=0, - retry_sweep_items=[], - latest_state_u8=_ring_group_end_state(), - endpoint_tolerance_u8=2.0, - ) - - -def test_cross_group_return_keeps_next_group_clearance_motors() -> None: - node = _cross_group_node() - - target = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "next_sweep" - ) - - target_list = list(target) - assert target_list[4] == 0 - assert target_list[19] == 0 - assert target_list[6] == 255 - assert target_list[7] == 255 - # Motors the next group does not need still return to baseline. - assert target_list[8] == THREE_CAMERA_BASELINE_COMMAND[8] - assert target_list[3] == THREE_CAMERA_BASELINE_COMMAND[3] - assert target_list[18] == THREE_CAMERA_BASELINE_COMMAND[18] - - -def test_cross_group_transition_has_no_unfold_refold_redundancy() -> None: - node = _cross_group_node() - middle_roll = node.sweep_items[0].spec - - def moved_motors(waypoints, start): - moved: set[int] = set() - previous = list(start) - for waypoint in waypoints: - moved.update( - index - for index, (left, right) in enumerate(zip(previous, waypoint)) - if left != right - ) - previous = list(waypoint) - return moved - - for parallel in (True, False): - target = ( - G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "next_sweep" - ) - ) - returns = build_calibration_return_waypoints( - target, - current_command=node.latest_state_u8, - profile=RIGHT_19_HAND_PROFILE, - parallel=parallel, - ) - after_return = list(returns[-1]) if returns else node.latest_state_u8 - preparations = build_calibration_preparation_waypoints( - middle_roll, - 255, - current_command=after_return, - baseline=THREE_CAMERA_BASELINE_COMMAND, - profile=RIGHT_19_HAND_PROFILE, - parallel=parallel, - ) - return_moved = moved_motors(returns, node.latest_state_u8) - prep_moved = moved_motors(preparations, after_return) - assert not (return_moved & prep_moved), ( - f"parallel={parallel}: motors {sorted(return_moved & prep_moved)} " - "unfold to baseline and immediately re-flex" - ) - # The pinky stays flexed throughout the whole finger change. - assert 4 not in return_moved and 19 not in return_moved - assert 4 not in prep_moved and 19 not in prep_moved - - -def test_cross_group_return_falls_back_to_baseline_for_thumb() -> None: - thumb = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 0 - ) - node = _cross_group_node() - node.sweep_items = [SweepItem(thumb, 0, DIRECTION_DECREASING)] - - target = G20ThreeCameraCalibrationNode._return_command_for_transition( - node, "next_sweep" - ) - - assert list(target) == list(THREE_CAMERA_BASELINE_COMMAND) - - -def _task_validity_node(task_valid: int, task_total: int, window: float): - spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) - records = _image_cycle_records([math.radians(47.0)] * 3) - node = _fit_check_node({"thumb_cmc_pitch": records}) - node.minimum_detection_rate = 0.95 - node.views = { - "front": SimpleNamespace( - valid_rate=window, - task_valid_frames=task_valid, - task_total_frames=task_total, - ) - } - return node, spec - - -def test_task_validity_gate_ignores_post_task_window_pollution() -> None: - # Session 20260820_133904: the sweeps themselves were ~100% valid - # (28.9 Hz synchronised frames), but after the last sweep the - # required-role set flipped back to the full preflight set, the rolling - # window restarted on idle frames at 65%, and a healthy task failed. - node, spec = _task_validity_node( - task_valid=2990, task_total=3072, window=0.65 - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] == "tag_valid_rate_percent" - ] - - -def test_task_validity_gate_fails_on_bad_task_counters() -> None: - node, spec = _task_validity_node( - task_valid=650, task_total=1000, window=0.99 - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - validity_failure = next( - failure - for failure in failures - if failure["metric"] == "tag_valid_rate_percent" - ) - assert validity_failure["actual"] == 65.0 - assert validity_failure["task_valid_frames"] == 650 - assert validity_failure["task_total_frames"] == 1000 - - -def test_right_19_task_validity_is_diagnostic_after_complete_sweeps() -> None: - node, _legacy_spec = _task_validity_node( - task_valid=650, task_total=1000, window=0.65 - ) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_19" - ) - node.records_by_joint["thumb_cmc_pitch"] = _image_cycle_records( - [math.radians(47.0)] * 3, - command_step=1, - ) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.motor_index == 0 - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] == "tag_valid_rate_percent" - ] - - -def test_task_validity_gate_falls_back_to_window_without_counters() -> None: - node, spec = _task_validity_node( - task_valid=0, task_total=0, window=0.99 - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] == "tag_valid_rate_percent" - ] - - -def _import_revalidation_node(records_by_joint: dict) -> SimpleNamespace: - node = _fit_check_node(records_by_joint) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", "g20_right_15" - ) - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = { - name: records for name, records in records_by_joint.items() - } - node.command_records_by_joint = { - name: [ - dict(record) - for record in records - if int(record.get("cycle", 0)) == 0 - ] - for name, records in records_by_joint.items() - } - node.palm_axis_records_by_source = { - observer.source_name: [] - for observer in RIGHT_19_HAND_PROFILE.palm_axis_observers - } - node.command_maximum_direction_gap_rad = math.radians(2.0) - node.sweep_items = [ - SweepItem(spec, 0, DIRECTION_DECREASING) - for spec in RIGHT_19_HAND_PROFILE.sweep_specs - ] - return node - - -def test_import_revalidation_drops_warning_band_task_and_prefix() -> None: - # thumb_ip hysteresis 2.18 deg provisionally passed in its source - # session (warning band) but fails the final hard gate; importing it - # anyway made session 20260820_133904 go back to the thumb after the - # four fingers were finished. The failing prefix task and everything - # behind it must be dropped at import time with records cleared. - bad = _image_cycle_records( - [math.radians(47.0), math.radians(47.2), math.radians(55.0)] - ) - good = _image_cycle_records([math.radians(47.0)] * 3) - node = _import_revalidation_node( - {"thumb_cmc_pitch": bad, "thumb_cmc_roll": good} - ) - - accepted, dropped = ( - G20ThreeCameraCalibrationNode._revalidate_imported_tasks( - node, - ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"], - ) - ) - - assert accepted == [] - assert dropped[0]["task"] == "thumb_cmc_pitch_front" - assert dropped[0]["failures"][0]["metric"] == "cycle_travel_range_deg" - assert not node.records_by_joint["thumb_cmc_pitch"] - assert not node.records_by_joint["thumb_cmc_roll"] - - -def test_sparse_import_revalidation_keeps_clean_later_task() -> None: - bad = _image_cycle_records( - [math.radians(47.0), math.radians(47.2), math.radians(55.0)] - ) - good = _image_cycle_records([math.radians(47.0)] * 3) - node = _import_revalidation_node( - {"thumb_cmc_pitch": bad, "thumb_cmc_roll": good} - ) - - accepted, dropped = ( - G20ThreeCameraCalibrationNode._revalidate_imported_tasks( - node, - ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"], - allow_sparse=True, - ) - ) - - assert accepted == ["thumb_cmc_roll_front"] - assert [item["task"] for item in dropped] == [ - "thumb_cmc_pitch_front" - ] - assert not node.records_by_joint["thumb_cmc_pitch"] - assert node.records_by_joint["thumb_cmc_roll"] - - -def test_sparse_resume_queue_skips_revalidated_later_tasks() -> None: - specs = RIGHT_19_HAND_PROFILE.sweep_specs[:3] - items = [ - SweepItem(spec, cycle, direction) - for spec in specs - for cycle in range(4) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) - ] - node = SimpleNamespace( - sweep_items=items, - sweep_index=0, - resumed_task_keys=(specs[0].key, specs[2].key), - ) - - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(node) - assert node.sweep_items[node.sweep_index].spec == specs[1] - - node.sweep_index += 8 - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(node) - assert node.sweep_index == len(items) - - -def test_provisional_gate_catches_final_command_direction_gap() -> None: - records = _image_cycle_records([math.radians(47.0)] * 4) - command_records = [ - dict(record) for record in records if int(record["cycle"]) == 0 - ] - for record in command_records: - if record["direction"] != DIRECTION_INCREASING: - continue - rotation = Rotation.from_quat(record["relative_quaternion_xyzw"]) - command = float(record["command_u8"]) - shift = math.radians(3.0) * (255.0 - command) / 255.0 - shifted = Rotation.from_rotvec([0.0, 0.0, shift]) * rotation - record["relative_quaternion_xyzw"] = shifted.as_quat().tolist() - record["child_pose_common"] = dict(record["child_pose_common"]) - record["child_pose_common"]["quaternion_xyzw"] = ( - shifted.as_quat().tolist() - ) - node = _fit_check_node({"thumb_cmc_yaw": records}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", RIGHT_19_HAND_PROFILE.layout_id - ) - node.repetitions = 4 - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {"thumb_cmc_yaw": records} - node.command_records_by_joint = {"thumb_cmc_yaw": command_records} - node.command_maximum_direction_gap_rad = math.radians(2.0) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_yaw_top" - ) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - - command_failure = next( - item - for item in failures - if item["metric"] == "feedback_direction_gap_deg" - ) - assert command_failure["actual"] == pytest.approx(3.0, abs=0.05) - assert command_failure["limit"] == 2.0 - - -def test_product_gate_separates_firmware_deadband_from_backlash() -> None: - records = _image_cycle_records([math.radians(47.0)] * 4) - command_records = _image_cycle_records([math.radians(120.0)]) - for record in command_records: - requested = int(record["command_u8"]) - if requested in {0, 255}: - feedback = requested - elif record["direction"] == DIRECTION_DECREASING: - feedback = min(254, requested + 3) - else: - feedback = max(1, requested - 3) - angle = math.radians(120.0) * (255.0 - feedback) / 255.0 - rotation = Rotation.from_rotvec([0.0, 0.0, angle]) - record["requested_command_u8"] = requested - record["feedback_u8"] = float(feedback) - record["relative_quaternion_xyzw"] = rotation.as_quat().tolist() - record["child_pose_common"] = dict(record["child_pose_common"]) - record["child_pose_common"]["quaternion_xyzw"] = ( - rotation.as_quat().tolist() - ) - node = _fit_check_node({"thumb_cmc_yaw": records}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", RIGHT_19_HAND_PROFILE.layout_id - ) - node.repetitions = 4 - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {"thumb_cmc_yaw": records} - node.command_records_by_joint = { - "thumb_cmc_yaw": command_records - } - node.command_maximum_direction_gap_rad = math.radians(2.0) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_yaw_top" - ) - - requested_fit = G20ThreeCameraCalibrationNode._fit_joint_records( - node, "thumb_cmc_yaw", command_records - ) - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - - assert math.degrees(requested_fit.maximum_hysteresis_rad) > 2.0 - assert not [ - item - for item in failures - if item["metric"] == "feedback_direction_gap_deg" - ] - - -def test_product_gate_uses_settled_direction_gap_not_dynamic_sweep_lag() -> None: - records = _image_cycle_records([math.radians(70.0)] * 4) - for record in records: - if record["direction"] != DIRECTION_INCREASING: - continue - command = float(record["command_u8"]) - lag = math.radians(3.0) * (255.0 - command) / 255.0 - rotation = Rotation.from_quat(record["relative_quaternion_xyzw"]) - shifted = Rotation.from_rotvec([0.0, 0.0, lag]) * rotation - record["relative_quaternion_xyzw"] = shifted.as_quat().tolist() - record["child_pose_common"] = dict(record["child_pose_common"]) - record["child_pose_common"]["quaternion_xyzw"] = ( - shifted.as_quat().tolist() - ) - settled = _image_cycle_records([math.radians(70.0)])[0:] - node = _fit_check_node({"thumb_mcp": records}) - node.profile = RIGHT_19_HAND_PROFILE - node.zero_profile = get_zero_calibration_profile( - "right", RIGHT_19_HAND_PROFILE.layout_id - ) - node.repetitions = 4 - node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) - node.baseline_records_by_joint = {"thumb_mcp": records} - node.command_records_by_joint = {"thumb_mcp": settled} - node.command_maximum_direction_gap_rad = math.radians(2.0) - node.maximum_hysteresis_rad = math.radians(2.0) - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_mcp_ip_front" - ) - - dynamic_fit = G20ThreeCameraCalibrationNode._fit_joint_records( - node, "thumb_mcp", records - ) - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, replace(spec, joints=("thumb_mcp",)), - include_view_validity=False, - ) - - assert math.degrees(dynamic_fit.maximum_hysteresis_rad) > 2.5 - assert not [ - item - for item in failures - if item["metric"] - in {"hysteresis_deg", "command_direction_gap_deg"} - ] - - -def test_import_revalidation_accepts_clean_prefix() -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - node = _import_revalidation_node( - {"thumb_cmc_pitch": records, "thumb_cmc_roll": records} - ) - - accepted, dropped = ( - G20ThreeCameraCalibrationNode._revalidate_imported_tasks( - node, - ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"], - ) - ) - - assert accepted == ["thumb_cmc_pitch_front", "thumb_cmc_roll_front"] - assert dropped == [] - assert len(node.records_by_joint["thumb_cmc_pitch"]) == len(records) - - -def test_provisional_fit_can_skip_view_validity_for_imports() -> None: - records = _image_cycle_records([math.radians(47.0)] * 3) - node = _import_revalidation_node({"thumb_cmc_pitch": records}) - node.minimum_detection_rate = 0.95 - node.views = { - "front": SimpleNamespace( - valid_rate=0.0, task_valid_frames=0, task_total_frames=0 - ) - } - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_pitch_front" - ) - - without_views = ( - G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=False - ) - ) - with_views = ( - G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec, include_view_validity=True - ) - ) - - assert not [ - failure - for failure in without_views - if failure["metric"] == "tag_valid_rate_percent" - ] - assert [ - failure - for failure in with_views - if failure["metric"] == "tag_valid_rate_percent" - ] - - -def _warning_band_node(tmp_path, attempts: dict) -> SimpleNamespace: - calls: list[str] = [] - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_pitch_front" - ) - node = SimpleNamespace( - cross_view_roll_diagnostic_finger="", - raw_path=tmp_path / "raw.jsonl", - provisional_warning_ratio=1.25, - sweep_attempts=attempts, - retry_resume_index=None, - retry_sweep_spec=None, - retry_cycles=set(), - repetitions=4, - fit_failure={}, - sweep_index=3, - automatic_fit_retry_limit=2, - paused_reason="", - reason="", - _prepare_failed_sweep_retry=lambda: calls.append("prepare_retry"), - _begin_return_baseline=lambda after: calls.append(f"return_{after}"), - _pause=lambda reason: calls.append(f"pause_{reason}"), - _calls=calls, - _spec=spec, - ) - return node - - -def test_warning_band_failure_stops_in_place(tmp_path) -> None: - node = _warning_band_node(tmp_path, attempts={}) - failures = [ - { - "joint": "thumb_cmc_pitch", - "metric": "axis_cycle_difference_deg", - "actual": 0.779, - "limit": 0.75, - "comparison": "maximum", - } - ] - - paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, node._spec, failures, allow_warning=True - ) - - assert paused is True - 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_retry_exhausted" in kinds - assert "provisional_fit_warning" not in kinds - assert "fit_failure" in kinds - - -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 - if item.key == "thumb_cmc_pitch_front" - ) - node = _warning_band_node( - tmp_path, attempts={spec.key: 2} - ) - failures = [ - { - "joint": "thumb_cmc_pitch", - "metric": "axis_cycle_difference_deg", - "actual": 0.779, - "limit": 0.75, - "comparison": "maximum", - } - ] - - paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, node._spec, failures, allow_warning=True - ) - - assert paused is True - 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_retry_exhausted", - "fit_failure", - ] - assert rows[0]["attempt"] == 2 - assert rows[0]["attempt_limit"] == 1 - - -def test_third_warning_band_result_stops_at_current_joint(tmp_path) -> None: - spec = next( - item - for item in RIGHT_19_HAND_PROFILE.sweep_specs - if item.key == "thumb_cmc_pitch_front" - ) - node = _warning_band_node(tmp_path, attempts={spec.key: 3}) - failures = [ - { - "joint": "thumb_cmc_pitch", - "metric": "axis_cycle_difference_deg", - "actual": 0.751, - "limit": 0.75, - "comparison": "maximum", - } - ] - - paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, node._spec, failures, allow_warning=True - ) - - assert paused is True - 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_retry_exhausted", - "fit_failure", - ] - assert rows[0]["attempt"] == 3 - assert rows[0]["attempt_limit"] == 1 - - -def test_repeated_branch_clusters_stop_before_third_full_rescan(tmp_path) -> None: - node = _warning_band_node(tmp_path, attempts={}) - - def failures(travels, orthogonal): - return [ - { - "joint": "thumb_mcp", - "metric": "rotation_orthogonal_rms_deg", - "actual": orthogonal, - "limit": 2.5, - "comparison": "maximum", - }, - { - "joint": "thumb_ip", - "metric": "cycle_travel_range_deg", - "actual": max(travels) - min(travels), - "limit": 10.0, - "comparison": "maximum", - "cycle_travel_deg": travels, - }, - ] - - paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, - node._spec, - failures([74.52, 88.36, 74.58, 74.51], 4.04), - allow_warning=True, - ) - assert paused is True - assert node._calls == ["pause_joint_fit_check_failed"] - - node._calls.clear() - node.sweep_attempts[node._spec.key] = 2 - paused = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( - node, - node._spec, - failures([74.43, 88.26, 74.54, 74.45], 3.92), - allow_warning=True, - ) - - assert paused is True - assert node._calls == ["pause_joint_fit_repeated_branch_failure"] - assert node.fit_failure["recoverable_by_rescan"] is False - assert node.fit_failure["directions_to_rescan"] == 0 - assert node.fit_failure["repeated_branch_clusters"] is True - - -def test_imported_task_without_capture_skips_view_rate_gate() -> None: - # A fully resumed session (16/16 imported, no live sweeps) has no - # task-scoped view counters; the rolling window then holds idle - # preflight frames judged against the full role set and would fail - # every task's validity gate at the final fit. - node, spec = _task_validity_node( - task_valid=0, task_total=0, window=0.65 - ) - node.resumed_task_keys = (spec.key,) - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert not [ - failure - for failure in failures - if failure["metric"] == "tag_valid_rate_percent" - ] - - -def test_live_task_still_uses_window_when_not_resumed() -> None: - node, spec = _task_validity_node( - task_valid=0, task_total=0, window=0.65 - ) - node.resumed_task_keys = () - - failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( - node, spec - ) - - assert [ - failure - for failure in failures - if failure["metric"] == "tag_valid_rate_percent" - ] - - -def test_cross_view_skip_keeps_front_depth_free_geometry(tmp_path) -> None: - # The side PIP-link pose validates the observation, but its planar-pose - # depth and orientation bias must not replace the trusted front geometry. - node = _cross_view_roll_node( - tmp_path, - side_tilt_deg=11.0, - side_offset_m=[0.021, 0.0, 0.0], - ) - - measurement = node._fit_axis_measurement("pinky_mcp_roll", 0) - - assert measurement.axis_direction_source != "cross_view_weighted_fusion" - assert measurement.axis_point_source == ( - "front_interpretation_plane_cross_view_validated" - ) - assert measurement.pose_axis_line_source_joints == ("pinky_mcp_roll",) - assert axis_line_uses_depth_free_interpretation_plane(measurement) - front_direction = np.asarray( - measurement.axis_common_xyz, dtype=float - ) - assert float(np.dot(front_direction, [0.0, 0.0, 1.0])) > math.cos( - math.radians(2.0) - ) - assert measurement.axis_point_camera_center_common_xyz_m is not None - assert ( - measurement.axis_point_interpretation_plane_normal_common_xyz - is not None - ) diff --git a/src/linkerhand_calibration/test/test_trajectory_policy.py b/src/linkerhand_calibration/test/test_trajectory_policy.py new file mode 100644 index 0000000..9d67457 --- /dev/null +++ b/src/linkerhand_calibration/test/test_trajectory_policy.py @@ -0,0 +1,39 @@ +from linkerhand_calibration.runtime import avoidance_arrival, smoothstep_position + + +def test_return_sweep_prepares_its_own_start_not_the_forward_start(): + from linkerhand_calibration.profiles.loader import load_bundled_hand_profile + from linkerhand_calibration.runtime.engine import CalibrationEngine + from linkerhand_calibration.runtime.trajectory import build_calibration_preparation_waypoints + profile = load_bundled_hand_profile("o12_right_16") + task = profile.motion.tasks[0] + reverse = CalibrationEngine(profile).scan_units()[1] + waypoints = build_calibration_preparation_waypoints(task, profile=profile, start_value=reverse.start) + assert waypoints[-1][task.command_index] == task.end_value + + +def test_cosine_trajectory_has_exact_endpoints() -> None: + assert smoothstep_position(0.0, 1.0, 0.0) == 0.0 + assert smoothstep_position(0.0, 1.0, 1.0) == 1.0 + + +def test_avoidance_accepts_stable_eighty_percent_without_exact_tracking() -> None: + result = avoidance_arrival( + start_feedback=0.0, + requested_target=1.5, + feedback_history=(1.20, 1.205, 1.21, 1.205), + trajectory_complete=True, + ) + assert result.arrived + assert result.progress_01 >= 0.8 + + +def test_avoidance_does_not_accept_wrong_direction() -> None: + result = avoidance_arrival( + start_feedback=0.0, + requested_target=1.5, + feedback_history=(-0.3, -0.31, -0.30), + trajectory_complete=True, + ) + assert not result.arrived + assert not result.direction_correct diff --git a/src/linkerhand_calibration/test/test_unified_artifact_pair.py b/src/linkerhand_calibration/test/test_unified_artifact_pair.py new file mode 100644 index 0000000..73211d4 --- /dev/null +++ b/src/linkerhand_calibration/test/test_unified_artifact_pair.py @@ -0,0 +1,81 @@ +"""Both input domains must pass the actual serialized standard URDF gate.""" + +from dataclasses import replace +import copy +import json + +import numpy as np +import pytest + +from test_frozen_tag_replay import fixture +from linkerhand_calibration.core.fitting.tag_installation import matrix_tuple +from linkerhand_calibration.runtime.artifacts.publisher import ArtifactPublisher, FrozenTagArtifactValidator +from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper + + +def dual_fixture(tmp_path): + source, plan, base, _, mounts, feedback, legacy = fixture(tmp_path) + old = legacy["joints"]["drive"] + def mapping(kind): + knots = old["curve_input_knots_rad"] + if kind == "command": + knots = [2*v+.1 for v in knots] + return {"input_domain": kind+"_rad", "input_unit": "rad", "output_unit": "rad", + "channel_index": 0, "knots": knots, "valid_input_range": [knots[0], knots[-1]], + "interpolation": "piecewise_linear", "extrapolation": "reject", + **{key: old[key].copy() for key in ("angle_rad", "increasing_rad", "decreasing_rad")}} + parent, multiplier, offset = plan.mimic_output["passive"] + payload = {"format": "unified_calibration_v1", "schema_version": 1, + "profile_id": legacy["profile_id"], "model": "VIRTUAL", "side": "right", "serial_number": "TEST", + "command_unit": "rad", "command_names": ["vendor_channel"], "joints": { + "drive": {"urdf_joint": "drive", "passive": False, + "command_to_rad": mapping("command"), "feedback_to_rad": mapping("feedback")}, + "passive": {"urdf_joint": "passive", "passive": True, + "mimic": {"joint": parent, "multiplier": multiplier, "offset_rad": offset}}}} + commands = tuple(replace(r, sample_id="command:"+r.sample_id, sdk_values=(2*r.sdk_values[0]+.1,)) for r in feedback) + validator = FrozenTagArtifactValidator(payload["profile_id"], source, plan.source_sha256, + plan.authorized_fields, plan.zero_offsets_rad, matrix_tuple(base), mounts, + tuple(feedback), ("a", "b"), lambda _: None, command_observations=commands) + session = tmp_path/"session"; session.mkdir() + urdf = plan.write(source, session/"corrected.urdf") + path = session/"calibration.json" + path.write_text(json.dumps(payload)) + return session, path, urdf, payload, validator + + +def test_dual_final_file_gate_manifest_reader_and_standard_passive(tmp_path): + session, path, urdf, payload, validator = dual_fixture(tmp_path) + release = ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, + calibration_json=path, corrected_urdf=urdf, validate=validator) + command = load_unified_mapper(release.manifest) + feedback = load_unified_mapper(path, input_kind="feedback") + assert command.map_positions((1.1,)) == pytest.approx(feedback.map_positions((.5,)), abs=1e-9) + assert command.map_positions((1.1,))[0] == pytest.approx(.2+.8*.5-.07) + feedback.feedback_name_aliases = {"vendor_reported_name": "vendor_channel"} + assert feedback.map_positions((.5,), ("vendor_reported_name",)) == pytest.approx(command.map_positions((1.1,))) + # Unnamed vendor positions retain the fixed array contract; no G20/O12 registry. + with pytest.raises(ValueError, match="outside"): + command.map_positions((3.,)) + path.write_text(path.read_text()+" ") + with pytest.raises(ValueError, match="SHA256"): + load_unified_mapper(path) + + +@pytest.mark.parametrize("corruption", ["command_curve", "domain", "passive_bypass", "missing_command_holdout"]) +def test_good_feedback_cannot_hide_bad_command_mapping(tmp_path, corruption): + session, path, urdf, payload, validator = dual_fixture(tmp_path) + if corruption == "command_curve": + for key in ("angle_rad", "increasing_rad", "decreasing_rad"): + values = payload["joints"]["drive"]["command_to_rad"][key] + payload["joints"]["drive"]["command_to_rad"][key] = (np.asarray(values)+.07*np.sin(np.linspace(0, np.pi, len(values)))).tolist() + elif corruption == "domain": + payload["joints"]["drive"]["command_to_rad"]["input_domain"] = "feedback_rad" + elif corruption == "passive_bypass": + payload["joints"]["passive"]["command_to_rad"] = copy.deepcopy(payload["joints"]["drive"]["command_to_rad"]) + else: + validator = replace(validator, command_observations=()) + path.write_text(json.dumps(payload)) + with pytest.raises(ValueError): + ArtifactPublisher(tmp_path, "latest").publish(session_directory=session, + calibration_json=path, corrected_urdf=urdf, validate=validator) + assert not (tmp_path/"latest").exists() diff --git a/src/linkerhand_calibration/test/test_unified_engine.py b/src/linkerhand_calibration/test/test_unified_engine.py index cdc7e4b..58e22f6 100644 --- a/src/linkerhand_calibration/test/test_unified_engine.py +++ b/src/linkerhand_calibration/test/test_unified_engine.py @@ -2,12 +2,11 @@ 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.compat.legacy_diagnostic_tools.models import get_default_registry from linkerhand_calibration.core import ( ArtifactPolicy, CalibrationProfile, @@ -35,8 +34,8 @@ from linkerhand_calibration.runtime import ( ) 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 +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.profile import JointCurveFit +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.l6.fitting import fit_coupling_model def test_all_product_profiles_use_unified_engine_and_sdk_adapter() -> None: @@ -144,29 +143,6 @@ def test_rotation_axis_uses_observed_endpoints_for_physical_feedback() -> None: ) -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: @@ -232,10 +208,10 @@ def test_old_checkpoint_policy_is_intentionally_incompatible() -> None: def test_removed_soft_stop_gates_are_not_in_live_nodes() -> None: - package = Path(__file__).resolve().parents[1] / "linkerhand_calibration/models" + package = Path(__file__).resolve().parents[1] / "linkerhand_calibration/runtime" live = "\n".join( (package / relative).read_text(encoding="utf-8") - for relative in ("l6/node.py", "o12/node.py") + for relative in ("ros/calibration_node.py", "ros/io.py", "coordinator.py", "safety.py") ) for removed in ( "non_target_motor_moved", diff --git a/src/linkerhand_calibration/test/test_unified_launch.py b/src/linkerhand_calibration/test/test_unified_launch.py new file mode 100644 index 0000000..176040a --- /dev/null +++ b/src/linkerhand_calibration/test/test_unified_launch.py @@ -0,0 +1,78 @@ +"""Build real launch actions without executing a process or opening hardware.""" + +import hashlib +import importlib.util +from pathlib import Path + +import pytest +import yaml +from launch import LaunchContext +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from ros2launch.api.api import parse_launch_arguments + +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.runtime.runner_support import launch_command + + +PACKAGE = Path(__file__).resolve().parents[1] + + +@pytest.fixture(autouse=True) +def isolated_launch_logs(tmp_path, monkeypatch): + monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path / "ros_logs")) + + +def launch_module(): + spec = importlib.util.spec_from_file_location("test_calibration_launch", + PACKAGE / "launch/three_camera_calibration.launch.py") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +@pytest.mark.parametrize("renamed_views", [False, True]) +def test_actual_launch_builds_from_protected_profile(tmp_path, model, renamed_views): + config = load_product_config(PACKAGE / f"config/{model}_right_product.yaml", + workspace=PACKAGE.parents[1], check_can=False) + arguments = dict(parse_launch_arguments(launch_command(config, tmp_path / "session", + record_bag=False, commands_enabled=False)[4:])) + if renamed_views: + payload = yaml.safe_load(config.profile_config.read_text()) + rename = {view.name: f"inspection_{i}" for i, view in enumerate(config.calibration_contract.typed_profile.vision.views)} + for view in payload["vision"]["views"]: + view["name"] = rename[view["name"]] + payload["vision"]["extrinsic_reference_view"] = rename[payload["vision"]["extrinsic_reference_view"]] + for task in payload["motion"]["tasks"]: + task["view"] = rename[task["view"]] + for measurement in payload["measurement"]["measurements"].values(): + measurement["view"] = rename[measurement["view"]] + path = tmp_path / "renamed.yaml" + path.write_text(yaml.safe_dump(payload)) + arguments["profile_config"] = str(path) + arguments["profile_config_expected_sha256"] = hashlib.sha256(path.read_bytes()).hexdigest() + for old, new in rename.items(): + for suffix in ("camera_serial", "camera_name", "camera_info_url"): + arguments[f"{new}_{suffix}"] = arguments.pop(f"{old}_{suffix}") + context = LaunchContext() + context.launch_configurations.update(arguments) + module = launch_module() + for action in module.generate_launch_description().entities: + if isinstance(action, DeclareLaunchArgument): + action.execute(context) + actions = module._launch_stack(context) + # Cameras, component container, SDK, calibration owner; none is executed. + assert sum(isinstance(action, Node) for action in actions) == 6 + assert not (tmp_path / "session/raw_samples.jsonl").exists() + + +def test_launch_rejects_missing_profile_before_creating_processes(): + context = LaunchContext() + context.launch_configurations.update(model="O6", hand_type="right", tag_layout="o6_right_8") + module = launch_module() + for action in module.generate_launch_description().entities: + if isinstance(action, DeclareLaunchArgument): + action.execute(context) + with pytest.raises(RuntimeError, match="protected YAML Profile"): + module._launch_stack(context) diff --git a/src/linkerhand_calibration/test/test_unified_ros_host.py b/src/linkerhand_calibration/test/test_unified_ros_host.py new file mode 100644 index 0000000..bf5aa75 --- /dev/null +++ b/src/linkerhand_calibration/test/test_unified_ros_host.py @@ -0,0 +1,83 @@ +"""Actual ROS host, isolated DDS domain; no SDK, camera or hand is launched.""" + +from pathlib import Path + +import numpy as np +import pytest + + +@pytest.mark.parametrize("model", ["l6", "o6", "g20", "o12"]) +def test_ros_host_constructs_and_start_discards_preview(tmp_path, monkeypatch, model): + rclpy = pytest.importorskip("rclpy") + from sensor_msgs.msg import JointState + from std_srvs.srv import Trigger + from linkerhand_calibration.product import load_product_config + from linkerhand_calibration.runtime.runner_support import protected_inputs + from linkerhand_calibration.runtime.ros.calibration_node import UnifiedCalibrationNode + from linkerhand_calibration.runtime.session import CalibrationPhase as Phase + package = Path(__file__).resolve().parents[1] + config = load_product_config(package / f"config/{model}_right_product.yaml", workspace=package.parents[1], check_can=False) + profile = config.calibration_contract.typed_profile + params = {"serial_number": "OFFLINE_HOST", "session_dir": str(tmp_path / model), + "source_urdf_path": str(config.source_urdf), "camera_extrinsics_file": str(config.camera_extrinsics), + **{key.replace("_sha256", "_expected_sha256"): value for key, value in protected_inputs(config).items()}} + args = ["--ros-args", "--params-file", str(config.calibration_config)] + for key, value in params.items(): + args += ["-p", f"{key}:={value}"] + monkeypatch.setenv("ROS_DOMAIN_ID", "221") + monkeypatch.setenv("ROS_LOCALHOST_ONLY", "1") + monkeypatch.setenv("ROS_LOG_DIR", str(tmp_path / "ros_logs")) + rclpy.init(args=args) + node = None + try: + node = UnifiedCalibrationNode(profile) + host = node.coordinator + assert host.parameters.tracking.reprojection_tie_px == 0.03 + assert all(tracker.reprojection_tie_px == 0.03 for tracker in host.capture.trackers.values()) + if profile.sdk_adapter == "o12_hcan_sdk": + from linkerhand_calibration.runtime.adapters import HardwareHealth + monkeypatch.setattr(host.sdk_adapter, "_health", lambda: HardwareHealth(True, True)) + writes = [] + monkeypatch.setattr(node.io.command_publisher, "publish", writes.append) + message = JointState() + message.position = list(map(float, profile.command.baseline_values)) + message.name = list(profile.command.names) + message.header.stamp = node.get_clock().now().to_msg() + node._state_callback(message) + host.cameras.matrices = {view: np.eye(3) for view in profile.vision.view_names} + import time + host.cameras.info_received_at = {view: time.monotonic() for view in profile.vision.view_names} + host.cameras.detections_received_at = dict(host.cameras.info_received_at) + node._tick() + assert host.execution.session.phase == Phase.READY + assert not writes # Preview never sends an open/baseline command. + assert host.snapshot().as_dict()["overall_progress_01"] == 0 + preview_capture = host.capture + preview_reference = host.reference_lock + # A once-ready camera going silent must revoke readiness, including + # when Start arrives before the next control timer callback. + host.cameras.detections_received_at.clear() + response = node._start(Trigger.Request(), Trigger.Response()) + assert not response.success + assert host.execution.session.phase == Phase.WAIT_DEVICE + assert not writes + assert host.capture is preview_capture + host.cameras.detections_received_at = {view: time.monotonic() for view in profile.vision.view_names} + node._tick() + response = node._start(Trigger.Request(), Trigger.Response()) + assert response.success + assert host.capture is not preview_capture + assert host.reference_lock is not preview_reference + assert all(tracker.reprojection_tie_px == 0.03 for tracker in host.capture.trackers.values()) + assert not host.state_history + node._tick() + assert not writes # Still waits for a fresh, post-Start feedback frame. + message.header.stamp = node.get_clock().now().to_msg() + node._state_callback(message) + node._tick() + assert host.execution.session.phase == Phase.BASELINE + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + diff --git a/src/linkerhand_calibration/test/test_unified_runner.py b/src/linkerhand_calibration/test/test_unified_runner.py new file mode 100644 index 0000000..471bb77 --- /dev/null +++ b/src/linkerhand_calibration/test/test_unified_runner.py @@ -0,0 +1,362 @@ +from __future__ import annotations + +import json +from dataclasses import replace +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from linkerhand_calibration.profiles import load_hand_profile +from linkerhand_calibration.product import load_product_config +from linkerhand_calibration.runtime import runner +from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION +from linkerhand_calibration.runtime.resume import discover_resume_candidate +from linkerhand_calibration.runtime.status import normalize_status, render_status_zh + + +PACKAGE = Path(__file__).resolve().parents[1] +WORKSPACE = PACKAGE.parents[1] + + +class LifecycleHarness: + def __init__(self, messages, *, step=1.0, publishers=1, response=True): + self.messages = iter(messages) + self.now = 0.0 + self.step = step + self.publisher_count = publishers + self.status = {} + self.last_status_at = 0.0 + self.status_topic = "/virtual/status" + self.start_client = SimpleNamespace(service_is_ready=lambda: True) + self.returncode = None + self.starts = 0 + self.response = response + + def spin(self): + self.now += self.step + value = next(self.messages) + if value is not None: + self.status = value + self.last_status_at = self.now + + def count_publishers(self, _topic): + return self.publisher_count + + def poll(self): + return self.returncode + + def request(self): + self.starts += 1 + return SimpleNamespace(done=lambda: True, + result=lambda: SimpleNamespace(success=self.response, message="fixture response")) + + def run(self, **options): + return runner.drive_session(self, self, spin=self.spin, ok=lambda: True, + request_start=self.request, clock=lambda: self.now, **options) + + +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +def test_actual_cli_uses_common_runner_not_model_dispatch(monkeypatch, model): + calls = [] + monkeypatch.setattr(runner, "run_online", lambda config, **kw: calls.append((config, kw)) or 0) + # Failure of any old Python CLI strategy must not affect the real entry. + import linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry as registry + monkeypatch.setattr(registry, "get_default_registry", lambda: pytest.fail("model runner dispatch")) + with pytest.raises(SystemExit) as error: + runner.main(["--config", str(PACKAGE / "config" / f"{model}_right_product.yaml"), + "--workspace", str(WORKSPACE), "--commands-disabled", "--no-resume"]) + assert error.value.code == 0 + assert calls[0][0].model == model.upper() + assert calls[0][1] == dict(record_bag=False, commands_enabled=False, allow_resume=False) + + +@pytest.mark.parametrize("ready,terminal", [("WAIT_START", "COMPLETE"), ("READY", "PASSED"), ("READY", "COMPLETE")]) +def test_ready_sent_once_even_when_node_repeats_ready(ready, terminal): + harness = LifecycleHarness([{"state": ready}, {"state": ready}, {"state": "SWEEP"}, {"state": terminal}]) + assert normalize_status(harness.run())["state"] == "COMPLETE" + assert harness.starts == 1 + + +def test_reference_wait_and_slow_fit_do_not_become_false_communication_faults(): + harness = LifecycleHarness([ + {"state": "READY"}, {"state": "REFERENCE_LOCKING"}, None, None, + {"state": "FITTING"}, None, None, {"state": "COMPLETE"}, + ], step=300.0) + assert harness.run()["state"] == "COMPLETE" + assert harness.starts == 1 + + +def test_missing_status_owner_is_not_confused_with_slow_status(): + harness = LifecycleHarness([{"state": "READY"}, None], step=20.0, publishers=0) + status = harness.run() + assert status["state"] == "FAILED" + assert status["reason"] == "calibration_node_process_exited" + + +def test_rejected_start_preserves_exact_reason(): + harness = LifecycleHarness([{"state": "READY"}], response=False) + assert harness.run()["reason"] == "calibration_start_rejected:fixture response" + assert harness.starts == 1 + + +def test_preview_never_calls_start(): + harness = LifecycleHarness([{"state": "READY"}, {"state": "READY"}, {"state": "ABORTED"}]) + assert harness.run(auto_start=False)["state"] == "ABORTED" + assert harness.starts == 0 + + +def test_start_service_response_has_its_own_deadline(): + harness = LifecycleHarness([{"state": "READY"}, None], step=20.0) + harness.request = lambda: SimpleNamespace(done=lambda: False) + assert harness.run()["reason"] == "calibration_start_response_timeout" + + +def test_progress_uses_profile_tags_including_secondary_view(): + profile = load_hand_profile(PACKAGE / "config/profiles/o12_right_16.yaml") + status = normalize_status({"state": "RUNNING", "task_name": "middle_roll_front", + "phase": "sweep", "cycle": 0, "attempt": 1, + "reference_locked": True}, profile=profile) + assert status["task"]["cycle"] == 1 + assert status["task"]["required_tag_ids_by_view"] == {"front": [0, 12], "side": [4, 8]} + text = render_status_zh(status) + assert "ID0/ID12" in text and "ID4/ID8" in text + assert "请勿移动" in text + + +def test_empty_preview_fields_do_not_break_status_rendering(): + status = normalize_status({"state": "WAIT_DEVICES", "attempt": None, + "cycle": None, "step_index": None}) + assert "等待设备" in render_status_zh(status) + + +def test_unified_progress_keeps_sdk_fault_diagnostics(): + text = render_status_zh(normalize_status({"state": "PAUSED", + "reason": "o12_active_motor_fault:channel=1", "error_faults": ["motor_1:overcurrent"]})) + assert "活动电机故障" in text + assert "motor_1:overcurrent" in text + assert "未分类" not in text + + +def _journal(path, *, version=ACQUISITION_POLICY_VERSION, digest="a" * 64, complete=True): + path.mkdir() + rows = [{"kind": "session_start", "profile_id": "VIRTUAL/right/layout/v1", + "serial_number": "TEST_001", "acquisition_policy_version": version, + "protected_hashes": {"source": digest}}, + {"kind": "fixed_base_reference_locked"}] + if complete: + rows.append({"kind": "scan_unit_complete", "passed": True}) + (path / "raw_samples.jsonl").write_text("\n".join(json.dumps(row) for row in rows)) + + +def test_discovery_keeps_prior_evidence_and_ignores_new_empty_incompatible_sessions(tmp_path): + _journal(tmp_path / "01") + _journal(tmp_path / "02", complete=False) + _journal(tmp_path / "03", digest="b" * 64) + _journal(tmp_path / "04", version="unified_engine_v1") + _journal(tmp_path / "05", version="unified_engine_v2") + before = (tmp_path / "01/raw_samples.jsonl").read_bytes() + found = discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", + serial_number="TEST_001", protected_hashes={"source": "a" * 64}) + assert found == tmp_path / "01" + assert found.joinpath("raw_samples.jsonl").read_bytes() == before + + +def test_discovery_never_reuses_released_session_or_external_symlink(tmp_path): + _journal(tmp_path / "01") + (tmp_path / "latest_passed").symlink_to(tmp_path / "01", target_is_directory=True) + (tmp_path / "99").symlink_to(tmp_path / "01", target_is_directory=True) + assert discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", + serial_number="TEST_001", protected_hashes={"source": "a" * 64}) is None + + +def test_passed_capture_cannot_be_reused_to_fake_an_independent_session(tmp_path): + _journal(tmp_path / "01") + (tmp_path / "01/calibration_summary_zh.json").write_text(json.dumps({"result": "PASS"})) + assert discover_resume_candidate(tmp_path, profile_id="VIRTUAL/right/layout/v1", + serial_number="TEST_001", protected_hashes={"source": "a" * 64}) is None + + +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +@pytest.mark.parametrize("terminal", ["COMPLETE", "PAUSED"]) +def test_common_online_process_chain_with_virtual_ros_and_no_hardware(monkeypatch, tmp_path, model, terminal): + """Use run_online itself, not just its selector or a schedule list.""" + import rclpy + from linkerhand_calibration.runtime import runner_support + from linkerhand_calibration.runtime.artifacts import completion as online_artifacts + import linkerhand_calibration.hikrobot_camera as camera + + config = load_product_config(PACKAGE / "config" / f"{model}_right_product.yaml", + workspace=WORKSPACE, check_can=False) + config = replace(config, output_root=tmp_path) + harness = LifecycleHarness([ + {"state": "READY"}, {"state": "REFERENCE_LOCKING"}, + {"state": "SWEEP"}, {"state": terminal, "reason": "synthetic_terminal"}, + ]) + harness.abort_client = SimpleNamespace(service_is_ready=lambda: True) + harness.start_client.call_async = lambda _request: harness.request() + harness.destroy_node = lambda: calls.append("destroy_monitor") + calls = [] + launched = [] + ros_active = [False] + def init(): + ros_active[0] = True + def shutdown(): + ros_active[0] = False + calls.append("shutdown") + def popen(command, **kwargs): + launched.append((command, kwargs)) + return harness + def spin(_node, **_kwargs): + if launched: + harness.spin() + else: + harness.now += 0.5 + harness.count_publishers = lambda _topic: 1 if launched else 0 + monkeypatch.setattr(rclpy, "init", init) + monkeypatch.setattr(rclpy, "shutdown", shutdown) + monkeypatch.setattr(rclpy, "ok", lambda: ros_active[0]) + monkeypatch.setattr(rclpy, "spin_once", spin) + monkeypatch.setattr(runner.time, "monotonic", lambda: harness.now) + monkeypatch.setattr(runner.subprocess, "Popen", popen) + monkeypatch.setattr(runner_support, "CalibrationMonitor", lambda **kw: harness) + monkeypatch.setattr(runner_support, "stop_stack", lambda process: calls.append("stop_owned_stack")) + monkeypatch.setattr(runner_support, "overlay_environment", lambda path: {"TEST_OVERLAY": str(path)}) + monkeypatch.setattr(camera, "configure_fastdds_large_image_transport", lambda: None) + def finish(product, session, status): + assert product == config and status["state"] == "COMPLETE" + assert (session / "node_status.json").is_file() + calls.append("artifact_validation") + return {"JSON": "virtual_only"} + monkeypatch.setattr(online_artifacts, "finish_online_artifacts", finish) + + code = runner.run_online(config, allow_resume=False) + assert code == (0 if terminal == "COMPLETE" else 3) + assert harness.starts == 1 + assert ("artifact_validation" in calls) == (terminal == "COMPLETE") + assert calls[-3:] == ["stop_owned_stack", "destroy_monitor", "shutdown"] + assert len(launched) == 1 + command, kwargs = launched[0] + assert "unified_calibration.launch.py" in command + assert f"profile_config:={config.profile_config}" in command + assert f"profile_config_expected_sha256:={config.profile_config_sha256}" in command + assert kwargs["start_new_session"] is True + if config.sdk_setup: + assert kwargs["env"]["TEST_OVERLAY"] == str(config.sdk_setup) + + +def test_session_directory_collision_never_sleeps_or_overwrites(monkeypatch, tmp_path): + from linkerhand_calibration.runtime import runner_support + monkeypatch.setattr(runner_support.time, "sleep", lambda _: pytest.fail("blocking collision sleep")) + first = runner_support.create_session_directory(tmp_path) + (first / "keep.txt").write_text("user evidence") + second = runner_support.create_session_directory(tmp_path) + assert first != second + assert (first / "keep.txt").read_text() == "user evidence" + + +@pytest.mark.parametrize("model", ["g20", "l6", "o6", "o12"]) +def test_launch_arguments_parse_with_ros_cli_and_preserve_sdk_config(model, tmp_path): + from ros2launch.api.api import parse_launch_arguments + from linkerhand_calibration.runtime.runner_support import launch_command + + config = load_product_config(PACKAGE / "config" / f"{model}_right_product.yaml", + workspace=WORKSPACE, check_can=False) + session = tmp_path / "session with spaces" + command = launch_command(config, session, record_bag=False, commands_enabled=False) + arguments = dict(parse_launch_arguments(command[4:])) + assert arguments["session_dir"] == str(session) + assert arguments["commands_enabled"] == "false" + for name, value in { + "vendor_sdk_config": config.sdk_config, + "sdk_config_expected_sha256": config.sdk_config_sha256, + "vendor_sdk_python_package": config.sdk_python_package, + "sdk_package_expected_sha256": config.sdk_package_sha256, + }.items(): + if value: + assert arguments[name] == str(value) + else: + assert name not in arguments + + +def test_runner_does_not_override_reviewed_speed_defaults(): + from linkerhand_calibration.runtime.runner_support import launch_command + for model in ("g20", "o6", "l6", "o12"): + config = load_product_config(PACKAGE / "config" / f"{model}_right_product.yaml", + workspace=WORKSPACE, check_can=False) + command = launch_command(config, Path("/tmp/virtual_session"), record_bag=False, commands_enabled=False) + speed = config.calibration_contract.typed_profile.motion.speed_parameters.get("baseline_u8") + if speed is not None: + assert f"calibration_speed:={int(speed)}" in command + else: + assert not any(arg.startswith("calibration_speed:=") for arg in command) + assert "index_roll_calibration_speed:=1" not in command + assert "index_flex_calibration_speed:=1" not in command + + +@pytest.mark.parametrize("model,layout", [("g20", "g20_right_19"), ("l6", "l6_right_8"), + ("o6", "o6_right_8"), ("o12", "o12_right_16")]) +def test_node_profile_handoff_checks_actual_yaml_and_hash(monkeypatch, model, layout): + import hashlib + from linkerhand_calibration.compat.legacy_diagnostic_tools.models import get_default_registry + from linkerhand_calibration.runtime.ros import entrypoint as calibration + path = PACKAGE / "config/profiles" / f"{layout}.yaml" + profile = load_hand_profile(path) + registered = get_default_registry().get(profile.key) + assert registered.profile == profile + forwarded = [] + monkeypatch.setattr(calibration, "run_profile_node", lambda declared, args: forwarded.append(args)) + assert not hasattr(calibration, "get_default_registry") + args = ["--profile-id", profile.key.profile_id, "--profile-config", str(path)] + with pytest.raises(ValueError, match="Profile changed"): + calibration.main(args + ["--profile-sha256", "0" * 64]) + assert not forwarded + calibration.main(args + ["--profile-sha256", hashlib.sha256(path.read_bytes()).hexdigest(), "--ros-args"]) + assert forwarded == [["--ros-args"]] + + +def test_owned_child_group_receives_shutdown_even_if_launch_parent_exited(monkeypatch): + from linkerhand_calibration.runtime import runner_support + import signal + signals = [] + process = SimpleNamespace(pid=12345, poll=lambda: 2, wait=lambda timeout: 2) + monkeypatch.setattr(runner_support.os, "killpg", lambda pid, sig: signals.append((pid, sig))) + runner_support.stop_stack(process) + assert signals == [(12345, signal.SIGINT)] + + +def test_native_profile_node_handoff_does_not_require_a_model_registration(tmp_path, monkeypatch): + import hashlib + import yaml + from linkerhand_calibration.runtime.ros import entrypoint as calibration + data = yaml.safe_load((PACKAGE / "config/profiles/l6_right_8.yaml").read_text()) + data.update(profile_id="VIRTUAL/right/no_python_model/v1", namespace="/virtual_calibration") + path = tmp_path / "new_profile.yaml" + path.write_text(yaml.safe_dump(data)) + def forbidden(): + raise AssertionError("new Profile must not look up a Python model") + import linkerhand_calibration.compat.legacy_diagnostic_tools.models.registry as registry + monkeypatch.setattr(registry, "get_default_registry", forbidden) + calls = [] + monkeypatch.setattr(calibration, "run_profile_node", lambda profile, args: calls.append((profile, args))) + calibration.main(["--profile-id", data["profile_id"], "--profile-config", str(path), + "--profile-sha256", hashlib.sha256(path.read_bytes()).hexdigest(), "--ros-args"]) + assert calls[0][0].key.model == "VIRTUAL" + assert calls[0][1] == ["--ros-args"] + + +@pytest.mark.parametrize("model", ["o6", "o12"]) +def test_abort_never_commands_baseline_return_for_byte_or_rad(tmp_path, model): + from runtime_host_fixture import coordinator_fixture + host, clock = coordinator_fixture(tmp_path, model=model) + try: + safe = tuple((lo+hi)/2 for lo, hi in zip(host.profile.command.minimum_values, + host.profile.command.maximum_values)) + if host.profile.command.unit == "u8": + safe = tuple(float(round(value)) for value in safe) + host._publish_command(safe) + response = host.abort() + assert host.state == "ABORTED" and response.success + assert clock.positions == [safe, safe] + finally: + host.close() diff --git a/src/linkerhand_calibration/test/test_urdf_validation.py b/src/linkerhand_calibration/test/test_urdf_validation.py new file mode 100644 index 0000000..5c88b08 --- /dev/null +++ b/src/linkerhand_calibration/test/test_urdf_validation.py @@ -0,0 +1,53 @@ +from __future__ import annotations + +from pathlib import Path + +import pytest + +from linkerhand_calibration.core.urdf import ( + validate_runtime_curve_limits, + validate_structural_urdf_diff, +) + + +SOURCE = """ + + + + +""" + + +def test_structural_diff_accepts_only_declared_joint_attribute(tmp_path: Path) -> None: + source = tmp_path / "source.urdf" + corrected = tmp_path / "corrected.urdf" + source.write_text(SOURCE, encoding="utf-8") + corrected.write_text(SOURCE.replace('rpy="0 0 0"', 'rpy="0 0 0.1"'), encoding="utf-8") + changes = validate_structural_urdf_diff( + source, corrected, authorized_fields={"finger": ("origin.rpy",)} + ) + assert changes == ("finger.origin.rpy",) + validate_runtime_curve_limits(corrected, {"finger": (0.0, 0.5, 1.0)}) + + +def test_structural_diff_rejects_mesh_inertial_or_undeclared_limit(tmp_path: Path) -> None: + source = tmp_path / "source.urdf" + source.write_text(SOURCE, encoding="utf-8") + for name, changed in { + "mesh": SOURCE.replace("meshes/tip.stl", "meshes/other.stl"), + "mass": SOURCE.replace('mass value="1"', 'mass value="2"'), + "limit": SOURCE.replace('upper="1"', 'upper="1.1"'), + }.items(): + output = tmp_path / f"{name}.urdf" + output.write_text(changed, encoding="utf-8") + with pytest.raises(ValueError, match="unauthorized"): + validate_structural_urdf_diff( + source, output, authorized_fields={"finger": ("origin.rpy",)} + ) + + +def test_runtime_curve_cannot_exceed_corrected_limit(tmp_path: Path) -> None: + urdf = tmp_path / "source.urdf" + urdf.write_text(SOURCE, encoding="utf-8") + with pytest.raises(ValueError, match="exceeds"): + validate_runtime_curve_limits(urdf, {"finger": (0.0, 1.01)}) diff --git a/src/linkerhand_calibration/test/test_urdf_zero.py b/src/linkerhand_calibration/test/test_urdf_zero.py index 8830c16..1731717 100644 --- a/src/linkerhand_calibration/test/test_urdf_zero.py +++ b/src/linkerhand_calibration/test/test_urdf_zero.py @@ -40,11 +40,10 @@ from linkerhand_calibration.full_hand import ( JOINT_SPECS, MEASURED_JOINTS, PASSIVE_JOINTS, - RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, JointCurveFit, get_hand_calibration_profile, ) -from linkerhand_calibration.models.g20.urdf_input import ( +from linkerhand_calibration.compat.legacy_diagnostic_tools.models.g20.urdf_input import ( build_g20_urdf_input_payload, load_g20_urdf_input, ) @@ -657,10 +656,9 @@ def test_urdf_writer_changes_only_the_16_active_zero_origins( corrected, corrected_axis = _joint_origin(destination, name) assert np.allclose(corrected, original, atol=1.0e-12) assert np.allclose(corrected_axis, axis, atol=1.0e-12) - # A zero calibration must not silently expand mechanical/CAD safety - # limits. Dynamic measured ranges remain in the calibration JSON. + # Numeric limits move with their coordinate zero; physical limits do not. for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS): - assert _joint_limit(destination, name) == pytest.approx( + assert np.asarray(_joint_limit(destination, name)) + offsets.get(name, 0.0) == pytest.approx( _joint_limit(SOURCE_URDF, name) ) @@ -1833,47 +1831,17 @@ def test_right_19_palm_pose_and_16_zero_observation_jacobian_is_full_rank() -> N assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count -def test_right_19_urdf_writer_changes_only_the_16_static_targets( - tmp_path: Path, -) -> None: +def test_right_19_urdf_writer_rejects_inconsistent_source_mimic_limits(tmp_path: Path) -> None: zero = get_zero_calibration_profile("right", "g20_right_19") offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints} - destination = write_zero_corrected_urdf( - source_urdf=RIGHT_SOURCE_URDF, - output_directory=tmp_path, - serial_number="G20_RIGHT_019", - offsets_rad=offsets, - timestamp="20260818_120000", - ) - - original = { - str(joint.get("name")): joint - for joint in ET.parse(RIGHT_SOURCE_URDF).getroot().findall("joint") - } - corrected = { - str(joint.get("name")): joint - for joint in ET.parse(destination).getroot().findall("joint") - } - for mesh in ET.parse(destination).getroot().findall(".//mesh"): - relative = Path(mesh.get("filename")) - copied = destination.parent / relative - source = RIGHT_SOURCE_URDF.parent / relative - assert copied.is_file() - assert copied.stat().st_size == source.stat().st_size - changed = set() - for name in original: - original_origin = original[name].find("origin") - corrected_origin = corrected[name].find("origin") - if original_origin is None or corrected_origin is None: - continue - if original_origin.get("rpy") != corrected_origin.get("rpy"): - changed.add(name) - assert original_origin.get("xyz") == corrected_origin.get("xyz") - assert changed == set(zero.direct_zero_joints) - assert "thumb_mcp" in changed - assert not set(get_hand_calibration_profile( - "right", "g20_right_19" - ).passive_joints) & changed + before = RIGHT_SOURCE_URDF.read_bytes() + with pytest.raises(ValueError, match="mimic reachable range exceeds limits"): + write_zero_corrected_urdf( + source_urdf=RIGHT_SOURCE_URDF, output_directory=tmp_path, + serial_number="G20_RIGHT_019", offsets_rad=offsets, + timestamp="20260818_120000") + assert RIGHT_SOURCE_URDF.read_bytes() == before + assert not list(tmp_path.glob("*.urdf")) def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None: diff --git a/src/linkerhand_calibration/test/test_virtual_control_contract.py b/src/linkerhand_calibration/test/test_virtual_control_contract.py new file mode 100644 index 0000000..f964df3 --- /dev/null +++ b/src/linkerhand_calibration/test/test_virtual_control_contract.py @@ -0,0 +1,84 @@ +"""A renamed rig and reordered SDK still use the shared capture/artifact chain.""" + +from dataclasses import replace +import hashlib + +import pytest + +from linkerhand_calibration.core.domain.profile import ProfileKey +from linkerhand_calibration.core.urdf.kinematics import UrdfKinematicModel +from linkerhand_calibration.profiles import dump_hand_profile, load_hand_profile +from linkerhand_calibration.runtime.adapters.base import HardwareHealth +from linkerhand_calibration.runtime.adapters.legacy_byte_sdk import LegacyByteSdkAdapter +from linkerhand_calibration.runtime.artifacts.finalization import finalize_profile_session +from linkerhand_calibration.runtime.artifacts.reader import load_unified_mapper +from engine_capture_fixture import collect_with_engine +from test_profile_finalization import virtual_capture + + +def test_reordered_channels_views_tasks_and_same_sdk_command_to_exported_fk(tmp_path): + original, source, rows, _ = virtual_capture(tmp_path, "l6_right_8") + order = (4, 2, 0, 5, 1, 3) + channel = {old: new for new, old in enumerate(order)} + views = dict(zip(original.vision.view_names, ("inspection", "oblique", "overhead"))) + def vector(values): + return tuple(values[index] for index in order) if values else () + def groups(values): + return tuple(tuple(channel[index] for index in group) for group in values) + def commands(values): + return tuple((channel[index], value) for index, value in values) + layout = original.command + layout = replace(layout, + **{name: vector(getattr(layout, name)) for name in ( + "names", "baseline_u8", "baseline", "lower_bounds", "upper_bounds", + "feedback_lower_bounds", "feedback_upper_bounds", "sdk_to_joint_direction", "maximum_velocity")}, + command_index_by_joint={joint: channel[index] for joint, index in layout.command_index_by_joint.items()}, + disabled_indices=frozenset(channel[index] for index in layout.disabled_indices), + speed_slot_by_command_index={channel[index]: slot for index, slot in layout.speed_slot_by_command_index.items()}) + profile = replace(original, key=ProfileKey("VIRTUAL", "right", "reordered_interfaces"), + namespace="/virtual_interfaces", command=layout, + vision=replace(original.vision, + views=tuple(replace(view, name=views[view.name]) for view in reversed(original.vision.views)), + extrinsic_reference_view=views[original.vision.extrinsic_reference_view]), + measurement=replace(original.measurement, measurements={name: replace(spec, + view=None if spec.view is None else views[spec.view]) for name, spec in original.measurement.measurements.items()}), + motion=replace(original.motion, tasks=tuple(replace(task, + command_index=channel[task.command_index], view=views[task.view], + auxiliary_commands=commands(task.auxiliary_commands), preparation_groups=groups(task.preparation_groups), + entry_waypoints=tuple(commands(waypoint) for waypoint in task.entry_waypoints)) + for task in reversed(original.motion.tasks)), return_groups=groups(original.motion.return_groups), + resume_verification_waypoints=tuple(replace(waypoint, command=vector(waypoint.command), + tag_ids_by_view={views[view]: ids for view, ids in waypoint.tag_ids_by_view.items()}) + for waypoint in original.motion.resume_verification_waypoints))) + path = tmp_path/"virtual_profile.yaml" + path.write_text(dump_hand_profile(profile)) + profile = load_hand_profile(path) + observations = [{**row, "view": views[row["view"]], + "state_u8": vector(row["state_u8"]), + "command_vector_u8": vector(row["command_vector_u8"])} for row in rows] + _, records = collect_with_engine(profile, observations) + hashes = {name: "a"*64 for name in profile.artifacts.protected_input_fields} + hashes["source_urdf_sha256"] = hashlib.sha256(source.read_bytes()).hexdigest() + payload, _, correction = finalize_profile_session(profile=profile, session_dir=tmp_path/"session", + serial_number="VIRTUAL_ONLY", source_urdf=source, protected_inputs=hashes, + records=records, standard_loader=lambda _: None) + mapper = load_unified_mapper(tmp_path/"session/release_manifest.json") + model = UrdfKinematicModel(correction.path) + writes = [] + sdk = LegacyByteSdkAdapter(profile.command, publish=writes.append, + set_speed_callback=lambda *_: None, health_callback=lambda: HardwareHealth(True, True)) + for values in ((31, 63, 95, 127, 159, 191), (191, 159, 127, 95, 63, 31)): + sdk.publish_position(values) + assert writes[-1] == tuple(values) + feedback = sdk.parse_feedback(tuple(reversed(profile.command.names)), tuple(reversed(values))) + assert feedback == tuple(values) + angles = dict(zip(mapper.urdf_joint_names, mapper.map_positions(values))) + # The identical native SDK vector indexes the certified JSON and FK. + direct = {joint: table["angle_rad"][values[table["sdk_channel"]]] + for joint, table in payload["joints"].items()} + assert angles == pytest.approx(direct, abs=1e-9) + active = {joint: value for joint, value in direct.items() if model.joints[joint].mimic_joint is None} + resolved = model.resolve_angles(active) + assert {joint: resolved[joint] for joint in direct} == pytest.approx(direct, abs=1e-9) + for joint, index in profile.command.command_index_by_joint.items(): + assert payload["joints"][joint]["sdk_channel"] == index 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_left/linkerhand_o12_t3_left_urdf-0703.urdf similarity index 60% rename from src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_141642.urdf rename to src/linkerhand_calibration/urdf/o12_left/linkerhand_o12_t3_left_urdf-0703.urdf index a668f76..bca7a82 100644 --- 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_left/linkerhand_o12_t3_left_urdf-0703.urdf @@ -1,1234 +1,1231 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/base_link.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/base_link.STL new file mode 100644 index 0000000..e00e9e0 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/base_link.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/index_distal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/index_distal.STL new file mode 100644 index 0000000..110c1f8 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/index_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/index_metacarpals.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/index_metacarpals.STL new file mode 100644 index 0000000..091350d Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/index_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/index_middle.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/index_middle.STL new file mode 100644 index 0000000..1c6cf46 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/index_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/index_proximal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/index_proximal.STL new file mode 100644 index 0000000..b09c29e Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/index_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/middle_distal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_distal.STL new file mode 100644 index 0000000..2759f69 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/middle_metacarpals.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_metacarpals.STL new file mode 100644 index 0000000..7779af0 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/middle_middle.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_middle.STL new file mode 100644 index 0000000..cccbf5e Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/middle_proximal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_proximal.STL new file mode 100644 index 0000000..2398b8b Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/middle_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_distal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_distal.STL new file mode 100644 index 0000000..4c6b56c Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_middle.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_middle.STL new file mode 100644 index 0000000..a7b4a5d Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_proximal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_proximal.STL new file mode 100644 index 0000000..4977b8f Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/pinky_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/ring_distal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/ring_distal.STL new file mode 100644 index 0000000..f9b958b Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/ring_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/ring_middle.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/ring_middle.STL new file mode 100644 index 0000000..b58f4af Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/ring_middle.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/ring_proximal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/ring_proximal.STL new file mode 100644 index 0000000..2647d64 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/ring_proximal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_distal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_distal.STL new file mode 100644 index 0000000..2b53c56 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_distal.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals.STL new file mode 100644 index 0000000..bd3c424 Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals_base1.STL new file mode 100644 index 0000000..21bd7eb Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals_base1.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals_base2.STL new file mode 100644 index 0000000..5cd144a Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_metacarpals_base2.STL differ diff --git a/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_proximal.STL b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_proximal.STL new file mode 100644 index 0000000..90aca3e Binary files /dev/null and b/src/linkerhand_calibration/urdf/o12_left/meshes/thumb_proximal.STL differ 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 deleted file mode 100644 index 4b7887c..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_134319.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index 6239942..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_170656.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index eb235c8..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_174116.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index 3dbf097..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_181314.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index 980b36d..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260908_183332.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index 36a9e32..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_20260909_144123.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index b27a069..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260908_195044.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index b6b95dd..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_105543.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ 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 deleted file mode 100644 index 543f403..0000000 --- a/src/linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703_calibrated_O12_RIGHT_001_REVIEW_ONLY_20260909_112041.urdf +++ /dev/null @@ -1,1234 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file