重构
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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",
|
||||
),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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。
|
||||
|
||||
本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。
|
||||
@@ -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 的 `<mimic>` 只能表达：
|
||||
|
||||
```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/<model>/`：声明式 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、不运行硬件、不参与零位拟合、不改变发布门，也不是
|
||||
实机接触精度认证。参考模型不是必须复现的固定参数；同数据回放一致性与独立重采
|
||||
精度重复性必须分别验收。
|
||||
+205
-1310
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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}
|
||||
@@ -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}
|
||||
@@ -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
|
||||
|
||||
@@ -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),
|
||||
]
|
||||
)
|
||||
@@ -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),
|
||||
]
|
||||
)
|
||||
@@ -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),
|
||||
]
|
||||
)
|
||||
@@ -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_<side>_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",
|
||||
|
||||
@@ -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()
|
||||
+19
-13
@@ -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
|
||||
|
||||
+14
@@ -0,0 +1,14 @@
|
||||
# 历史兼容区
|
||||
|
||||
此目录保留旧格式读写、旧布局、历史数据诊断及必要的离线工具，不参与正式在线调度。
|
||||
四型号的 runner/node/pipeline 已由 `runtime/runner.py`、`runtime/session.py` 和
|
||||
`runtime/artifacts/finalization.py` 替代。只保留仍被历史工具调用的入口；
|
||||
L6/O6 无调用的 pipeline 包装已删除。
|
||||
|
||||
旧独立断点实现、低速预检和在线节点已移除。旧发布函数已拒绝更新正式发布指针。
|
||||
这里生成的离线诊断文件不能作为标准 URDF 已验收的证据；正式回放使用
|
||||
`calibrate_hand --config <product.yaml> --offline-raw <raw_samples.jsonl>`。
|
||||
|
||||
不要在此目录增加新型号。新型号提供 Profile、产品 YAML、原始 CAD/mesh；仅新 SDK 协议增加 Adapter。
|
||||
需要恢复已删除的历史实现时，使用工作区
|
||||
`calibration_output/refactor_backup.4WRWNn/` 中的归档，不要重新接入生产入口。
|
||||
+1
@@ -0,0 +1 @@
|
||||
"""Read-only compatibility for archived sessions and diagnostic tools."""
|
||||
+2
-2
@@ -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",
|
||||
+15
@@ -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"]
|
||||
+35
-9
@@ -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(
|
||||
+2
-1
@@ -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),
|
||||
+1
-1
@@ -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)
|
||||
+5
-5
@@ -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)
|
||||
+1
-1
@@ -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,
|
||||
+10
-10
@@ -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,
|
||||
+6
-63
@@ -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)
|
||||
+9
-236
@@ -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")
|
||||
+7
-5
@@ -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),
|
||||
)
|
||||
+10
@@ -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()
|
||||
+2
-2
@@ -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,
|
||||
+931
@@ -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)
|
||||
+14
@@ -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"]
|
||||
+18
-22
@@ -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]:
|
||||
+33
@@ -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)
|
||||
+10
-4
@@ -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,
|
||||
+18
@@ -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",
|
||||
]
|
||||
+78
@@ -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",
|
||||
]
|
||||
+7
@@ -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,
|
||||
)
|
||||
+7
-33
@@ -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,
|
||||
+11
@@ -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"]
|
||||
+59
-179
@@ -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:
|
||||
+38
@@ -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)
|
||||
+64
@@ -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",
|
||||
]
|
||||
+17
@@ -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)
|
||||
+31
@@ -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)
|
||||
+1
-1
@@ -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')
|
||||
+285
@@ -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",
|
||||
]
|
||||
+75
@@ -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",
|
||||
]
|
||||
+5
@@ -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
|
||||
+94
@@ -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"]
|
||||
+18
-52
@@ -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",
|
||||
+14
@@ -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"]
|
||||
+21
-12
@@ -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,
|
||||
+25
@@ -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)
|
||||
+10
-4
@@ -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,
|
||||
+86
@@ -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",
|
||||
]
|
||||
+4
@@ -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
|
||||
+10
-4
@@ -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,
|
||||
+7
-7
@@ -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)
|
||||
+3
-3
@@ -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
|
||||
+251
@@ -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"
|
||||
)
|
||||
)
|
||||
+283
@@ -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
|
||||
@@ -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",
|
||||
|
||||
@@ -1,5 +1 @@
|
||||
"""Artifact schema and release validation contracts."""
|
||||
|
||||
from .release import ReleaseValidation, ReleaseValidator
|
||||
|
||||
__all__ = ["ReleaseValidation", "ReleaseValidator"]
|
||||
"""Storage helpers shared by calibration artifact writers."""
|
||||
|
||||
@@ -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: ...
|
||||
@@ -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",
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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
|
||||
@@ -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")
|
||||
@@ -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",
|
||||
]
|
||||
@@ -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",
|
||||
]
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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)
|
||||
@@ -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"]
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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
|
||||
+1
@@ -0,0 +1 @@
|
||||
"""Shared spatial mathematics; import the stable spatial facade for public APIs."""
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user