O12重构一版提交
This commit is contained in:
@@ -261,7 +261,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"叁": [0, 39, 255, 255, 255, 0],
|
||||
"肆": [0, 0, 255, 255, 255, 255],
|
||||
"伍": [255, 255, 255, 255, 255, 255],
|
||||
"OK": [74, 13, 153, 255, 255, 255],
|
||||
"OK": [62, 5, 151, 255, 255, 255],
|
||||
"点赞": [255, 255, 0, 0, 0, 0],
|
||||
"握拳": [79, 11, 0, 0, 0, 0],
|
||||
"序列动作1": [250, 250, 250, 250, 250, 250],
|
||||
@@ -277,7 +277,12 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"拇指压感准备1": [139, 18, 130, 0, 0, 0],
|
||||
"拇指压感测试": [39, 30, 122, 250, 250, 250],
|
||||
"拇指压感准备2": [139, 18, 130, 0, 0, 0]
|
||||
}
|
||||
},
|
||||
preset_action_overrides={
|
||||
"right": {
|
||||
"OK": [58, 6, 153, 255, 255, 255],
|
||||
},
|
||||
},
|
||||
),
|
||||
"O12": HandConfig(
|
||||
# O12 active-angle API order. Keep this independent from the
|
||||
|
||||
@@ -10,3 +10,14 @@ def test_l6_gui_uses_the_sdk_channel_order() -> None:
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
]
|
||||
|
||||
|
||||
def test_l6_ok_preset_is_hand_specific() -> None:
|
||||
config = HAND_CONFIGS["L6"]
|
||||
|
||||
assert config.get_preset_actions("left")["OK"] == [
|
||||
62, 5, 151, 255, 255, 255,
|
||||
]
|
||||
assert config.get_preset_actions("right")["OK"] == [
|
||||
58, 6, 153, 255, 255, 255,
|
||||
]
|
||||
|
||||
@@ -0,0 +1,238 @@
|
||||
# LinkerHand 整体标定流程与 URDF 修正原理
|
||||
|
||||
本文描述 `linkerhand_calibration` 当前的多型号统一架构。具体的通道数、运动单位、
|
||||
Tag 布局、任务、零位策略和输出格式由产品 Profile 决定，通用流程不再假定某一种手型。
|
||||
|
||||
## 1. 整体流程
|
||||
|
||||
```mermaid
|
||||
flowchart TD
|
||||
A[加载产品配置与 Profile]
|
||||
A --> B[设备、视觉与安全预检]
|
||||
B --> C[按任务执行三轮训练和一轮留出采集]
|
||||
C --> D[拟合动态曲线、零位、行程和被动耦合]
|
||||
D --> E{质量与留出验证通过?}
|
||||
E -->|数据不足且未重扫| C
|
||||
E -->|不可恢复| X[保持/暂停，不发布]
|
||||
E -->|通过| P[形成最终标定结果]
|
||||
P --> J[生成标定 JSON]
|
||||
P --> U[从原始 CAD 生成修正后的 URDF]
|
||||
```
|
||||
|
||||
对应的通用状态可以概括为：
|
||||
|
||||
```text
|
||||
配置/Profile → 预检 → 运动与视觉采集 → 参数拟合 → 质量验证
|
||||
├─→ 标定 JSON
|
||||
└─→ 修正后的 URDF
|
||||
```
|
||||
|
||||
图中只展示业务流程和最终产物。实现内部，G20/L6/O6 会额外生成并回读
|
||||
`*_urdf_correction_input.json`，用于审计和校验 URDF 修正参数；O12 直接使用已验证的
|
||||
拟合结果。该交接细节不改变最终交付物仍是标定 JSON 和修正 URDF。
|
||||
|
||||
## 2. Profile 负责什么
|
||||
|
||||
统一入口 `calibrate_hand --config <产品配置>` 先根据产品配置选择
|
||||
`MODEL/side/layout/vREVISION` Profile。Profile 是硬件运动和数据解释的边界，声明：
|
||||
|
||||
- SDK 命令顺序、单位（u8 或 rad）、反馈映射和安全 baseline；
|
||||
- 相机视角、Tag ID、父子连杆角色和外参要求；
|
||||
- 每项运动任务、起止点、辅助避障姿态和速度；
|
||||
- 哪些关节实测、迁移、保留 CAD，哪些关节是主动或被动；
|
||||
- 静态零位、机械端点和 mimic/非线性耦合的求解策略；
|
||||
- 训练轮、独立留出轮、质量门限、输出 schema 和发布指针。
|
||||
|
||||
因此通用层只执行“Profile 声明的任务”，不会自行猜测关节顺序、单位、Tag 数量或
|
||||
左右手镜像关系。
|
||||
|
||||
## 3. 标定原理
|
||||
|
||||
### 3.1 数据采集
|
||||
|
||||
每项任务只驱动一个目标通道，其余通道保持 baseline 或进入 Profile 规定的避障姿态。
|
||||
G20、L6、O6 不再执行逐任务全行程运动预检；O12 只对每个主动通道执行一次不超过
|
||||
3° 的映射点动。正式采集同步保存：
|
||||
|
||||
- 请求命令和真实反馈；
|
||||
- 父、子 AprilTag 的相对旋转与相对平移；
|
||||
- 相机时间戳、内外参身份、重投影误差及全手状态；
|
||||
- 扫描方向、轮次、任务和重试编号。
|
||||
|
||||
请求命令和真实反馈是两个不同的数据域，必须由对应型号的 schema 明确标识，不能混用。
|
||||
|
||||
### 3.2 视觉几何
|
||||
|
||||
关节运动由父、子刚性件上 Tag 的相对 SE(3) 轨迹得到。相对观测可以消除相机在世界中的
|
||||
绝对位置和固定 Tag 安装变换对动态角度的影响。
|
||||
|
||||
- 转轴方向由整段相对旋转轨迹拟合；
|
||||
- 轴线上一点可由刚体旋转关系 `(I-R)p=t` 稳健拟合；
|
||||
- 接近沿轴观察时，弱可观的单目深度分量会被降级为诊断或投影到图像平面；
|
||||
- 多视角结果先转换到公共坐标系，再按 Profile 规定用于主测量、交叉检查或零位约束。
|
||||
|
||||
### 3.3 三类标定结果
|
||||
|
||||
一次会话通常同时求三类参数：
|
||||
|
||||
1. 动态曲线：控制量/反馈量到关节角的单调映射，并保留或检查正反方向回差。
|
||||
2. 静态零位与行程：实物基准姿态相对 CAD 关节坐标系的偏差，以及实测安全范围。
|
||||
3. 被动耦合：主动关节与被动关节之间的线性或二次关系。
|
||||
|
||||
对启用 `isolated_holdout` 的当前产品 Profile，训练轮用于拟合，独立留出轮不参与最终参数
|
||||
重拟合，只检验泛化误差。短时 Tag/PnP/同步丢失只丢弃无效帧，运动继续完成。一个方向
|
||||
只有在有效同步样本少于 40、行程覆盖不足、少于 32 个分箱或连续盲区超过全行程 1/16
|
||||
时，才同速重扫一次。检测率与反馈频率低于理想值只作诊断，只要有效数据足够就不停机。
|
||||
拟合或 holdout 失败会拒绝发布，但不自动反复运动。
|
||||
|
||||
实时运动仅在人工中止/重复控制器、SDK 活动故障或失联、控制模式错误、物理越限、
|
||||
明确要求运动后连续两秒无推进，以及固定基准连续 10 帧漂移超过 5 px 时停止。非目标轴
|
||||
小幅运动、正常跟随滞后、机构固有耦合和辅助避让轴误差不是停机条件。
|
||||
|
||||
## 4. URDF 修正原理
|
||||
|
||||
### 4.1 基本原则
|
||||
|
||||
修正始终从经过哈希确认的原始 CAD URDF 生成，不在上一份标定 URDF 上叠加，也不覆盖
|
||||
源文件。型号适配层只生成声明式 Patch，公共 patch engine 负责字段级修改、禁止覆盖、
|
||||
mesh 安全复制和原子写入。
|
||||
|
||||
允许修改哪些字段由 Profile 和型号 writer 决定，可能包括：
|
||||
|
||||
| 参数 | 作用 |
|
||||
|---|---|
|
||||
| `origin.rpy` | 写入可观测且被授权的主动关节静态零偏 |
|
||||
| `limit.lower/upper` | 把实测行程或端点转换到修正后的关节坐标系 |
|
||||
| `mimic.multiplier/offset` | 为普通 URDF 使用者提供线性被动联动 |
|
||||
| MuJoCo equality `polycoef` | 表示 Profile 授权的非线性被动耦合 |
|
||||
|
||||
`origin.xyz`、关节轴、mesh、惯量、连杆长度和拓扑默认保持 CAD；只有型号 Profile 明确
|
||||
授权的字段才能变化。
|
||||
|
||||
### 4.2 静态零位
|
||||
|
||||
若型号允许修正某主动关节的静态零位，源关节变换为 `T_cad`、源关节轴为 `a`、
|
||||
零偏为 `δ`，则：
|
||||
|
||||
```text
|
||||
T_corrected = T_cad × Rot(a, δ)
|
||||
```
|
||||
|
||||
实现上将结果重新表达为 `origin.rpy`。动态曲线描述的是相对该新零位的运动量，所以运行时
|
||||
不能再把 `δ` 加到曲线输出中。
|
||||
|
||||
并非所有型号都修改 origin：如果视觉无法把固定 Tag 安装角与绝对零位可靠分离，Profile
|
||||
会保留 CAD origin，只发布动态曲线和实测行程。
|
||||
|
||||
### 4.3 机械端点和限位
|
||||
|
||||
零位和限位必须作为同一个坐标变换问题处理。Profile 会为不同机构选择经过确认的锚点策略，
|
||||
例如 `lower_at_start`、`upper_at_end` 或 `cad_range_center`，而不是统一假定命令 0/255
|
||||
一定对应某个 CAD 端点。
|
||||
|
||||
若某物理上限由 CAD 确认，零位移动 `δ` 后，坐标限位也要反向移动，保证：
|
||||
|
||||
```text
|
||||
静态零偏 + 修正后的坐标端点 = 原 CAD 物理端点
|
||||
```
|
||||
|
||||
其他型号则直接把实测安全行程写成新的 `[lower, upper]`。无论采用哪种策略，发布前都会
|
||||
检查运行曲线和被动耦合不越过修正后的 URDF 限位。
|
||||
|
||||
### 4.4 被动关节和非线性耦合
|
||||
|
||||
普通 URDF 的 `<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、不运行硬件、不参与零位拟合、不改变发布门，也不是
|
||||
实机接触精度认证。参考模型不是必须复现的固定参数；同数据回放一致性与独立重采
|
||||
精度重复性必须分别验收。
|
||||
@@ -1,5 +1,47 @@
|
||||
# LinkerHand 专业标定包
|
||||
|
||||
## 全型号安装与重复标定约定
|
||||
|
||||
O12 的 `thumb_mcp_dip_front` 使用 ID1/ID2/ID3 联合 IPPE 候选选择，不再逐 Tag
|
||||
独立决定分支。复用通用组跟踪器，以实测平行转轴方向辅助消歧；不指定 Tag 安装角，
|
||||
不将 CAD mimic 比例或 SDK 被动公式替换为视觉测量。方向证据不足时退回视觉连续性，
|
||||
不新增实时停机条件；最终几何质量验证保持不变。其他型号的选解策略及断点逻辑不变。
|
||||
在线选择仅作暂定观测。最终求解用前三轮完整轨迹重新比较拇指候选分支，第四轮
|
||||
不参与分支评分；结果写入 `pose_selection_diagnostics.json`。不会把关节轨迹强行
|
||||
投影成无残差的刚性运动，也不会降低原几何门限。
|
||||
|
||||
三相机检测来自 `image_rect`，PnP 必须使用 `CameraInfo.P[:3,:3]`，不能使用原始
|
||||
`K` 并把畸变设为零。L6/O6/O12 共用取参路径已修正，G20 原本即使用 P。
|
||||
新会话保存 `rectified_camera_model`（含原 K/D/R/P）；O12 各任务保存角点、候选
|
||||
和选解记录 `o12_pnp_candidate_frame`，锁定的基准明确标注 `locked_reference`，
|
||||
不伪造像素。拇指还保留初始化未选出位姿的帧。
|
||||
|
||||
旧帧若未记录正确投影来源，最终拟合不会把旧 K 静默当成 P；仍走旧观测路径并
|
||||
在诊断中标注 `legacy_projection_unverified`。只保存最终位姿的任务无法可靠重算；
|
||||
外部相机文件仅可显式用于局部诊断，其混合观测不允许生成整手正式产物。
|
||||
当前真实数据修正内参、重选拇指候选后仍有 MCP 残差未通过；不能承诺已解决实机精度。
|
||||
|
||||
标定前允许移动机械手底座、重新安装 Tag；ID 与所属刚性连杆必须正确，Tag 尺寸和
|
||||
平面观测条件必须满足配置。开始后底座固定，Tag 相对各自连杆固定，关节仍正常运动。
|
||||
相机相互位置及内参不变时，移动手不要求重标相机外参。
|
||||
|
||||
断点恢复保持原有默认行为，前提是底座、相机和 Tag 安装未变，不增加确认参数。
|
||||
G20/O12 在移动底座或重贴 Tag 后开始全新标定时，使用已有 `--no-resume` 选项，
|
||||
不混用旧安装的采样。原有哈希/数据兼容性检查保持不变；哈希不能检测物理安装变化。
|
||||
L6/O6 产品启动入口行为不变。
|
||||
|
||||
可用只读工具比较不同结果的模型姿态：
|
||||
|
||||
```bash
|
||||
ros2 run linkerhand_calibration compare_calibration_urdfs \
|
||||
--reference <参考.urdf> --candidate <本次.urdf> --output <新建报告.json>
|
||||
```
|
||||
|
||||
它检查相同 **URDF 关节角** 下的连杆原点位置、方向及主动行程，不修改文件、不控制
|
||||
硬件、不影响标定拟合或发布。坐标不是原始 SDK 弧度，连杆原点也不是指尖接触点；
|
||||
离线探测姿态不可直接发送给实机。报告不等于精度 PASS。参考结果只作对照，不作为
|
||||
固定零位输入，也不强制新数据拟合成旧参数。
|
||||
|
||||
## O12 右手 16-Tag 完整标定
|
||||
|
||||
O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口，固定订阅
|
||||
@@ -18,11 +60,101 @@ ros2 run linkerhand_calibration calibrate_hand --config \
|
||||
|
||||
runner 会自动加载仓库内 vendor Jazzy overlay，启动 HCAN device 0/channel 0
|
||||
节点、三相机、AprilTag 与标定节点；READY 后自动开始。启动前会确认 12 路
|
||||
POSITION 模式、错误码、温度和反馈，并以不超过 3° 的低速点动执行固定通道预检。
|
||||
正式扫描以 20 Hz 发布端点速度为零的弧度余弦轨迹；中止、堵转或质量失败时保持
|
||||
当前位置。结果发布到 `calibration_output/<O12串号>/latest_passed`，其中 JSON
|
||||
POSITION 模式、错误码和实时反馈，并以不超过 3° 的低速点动执行固定通道预检。
|
||||
这版 O12 固件的温度查询会阻塞后返回空，因此默认不发送该无效请求，而以独立错误
|
||||
查询中的 bit1 持续提供过热保护。bit0–bit3 或未知错误位始终立即停止；SDK 明确可能
|
||||
由历史超时留下的 bit4 `commu_except`，只有在至少三次相同报告、至少 1.5 秒观察且
|
||||
命令触发反馈持续新鲜并达到最低频率后才标记为历史锁存。标定运行中出现新的 bit4
|
||||
组合，或反馈流中断超过一秒，仍会立即保持当前位置并停止。
|
||||
正式扫描以 50 Hz 发布端点速度为零的平滑限速弧度轨迹；中止、堵转或质量失败时保持
|
||||
当前位置。小指完成后，小指与无名指会同步弯至各自安全上限；进入食指标定前，
|
||||
中指 MCP/PIP 也弯至各自安全上限且中指侧摆保持 0 rad。专用避让航点确认这些轴
|
||||
到位后才继续，为中指和食指留出完整空间。SDK 的 MCP→PIP 回读联动、其他非目标轴小幅运动以及避让轴
|
||||
跟随滞后全部作为采样/诊断保留，不再套用理论 vendor 包络触发停机。O12 的扫描质量
|
||||
由统一引擎按有效同步样本、实测行程、归一化分箱和连续未观测区判断；单 Tag 检测率、
|
||||
联合帧率和反馈频率低于理想目标只记告警。短时局部遮挡只丢弃无效帧，不中断运动；
|
||||
数据不足时当前方向只同速重扫一次。结果发布到
|
||||
`calibration_output/<O12串号>/latest_passed`，其中 JSON
|
||||
为 schema v7 弧度 knots，不生成 256 点 u8 表。
|
||||
|
||||
O12 与 G20 共用 Tag 几何拟合、3+1 holdout、URDF 写回和发布门，但输入域不同：G20
|
||||
使用 u8，O12 使用连续 SDK 弧度。O12 的 12 路 SDK 坐标包含 tendon/vendor solver
|
||||
坐标，不能直接当作 19 路 URDF 关节角；必须在完整 SDK 物理范围上由 Tag 相对旋转
|
||||
拟合 SDK→URDF 曲线。源 CAD 限位是待修正输出，不能反向截断采集范围。拇指
|
||||
roll/yaw 和其余实测主动关节的静态零位复用 G20 空间求解核：非平行轴用方向，
|
||||
平行弯曲轴用相邻轴线的位置相位。生产发布要求 11 个实测主动零位通过 3+1 验证，
|
||||
无名指继承小指修正；失败时保存 `spatial_zero_diagnostics.json`，不退回 CAD 报 PASS。
|
||||
被动关节的 JSON 仍使用 SDK 非线性公式，标准 URDF 则保留 CAD 端点等价线性
|
||||
mimic 近似，两者不能视为任意姿态下完全相同。发布前同时验证曲线、
|
||||
静态 origin、URDF 限位和递归 mimic 链；指尖接触精度还需独立组合姿态验收。
|
||||
|
||||
O12 空间零位策略 `o12_full_hand_spatial_v3_mount_invariant_phase` 正式采用
|
||||
复核版的轴向残差分离：求解旋转轴线时只使用可观测的横向方程，同时记录原始、轴向、
|
||||
横向残差，不能把轴向误差混作轴线位置误差。每次从本次数据重新求解，不包含某一只手
|
||||
的固定修正角度；G20/L6/O6 的默认拟合策略不变。三轮训练、第四轮独立验证及横向几何
|
||||
质量门仍保留，不能仅凭模型看起来更像就宣布精度通过。
|
||||
|
||||
侧面平行关节相位先消除轴线点的轴向自由分量，再投影到图像平面，不能重新使用未经
|
||||
处理的两点位移：轴线上的“最近点”随父 Tag 安装原点改变，并不是唯一的物理轴承中心。
|
||||
此前这种混用会在略斜的侧面视角下产生安装相关的零位偏差，即使无噪声且四轮重复也
|
||||
可能错误通过。回归测试覆盖近轴向机位、移动底座、重新安装父/子 Tag 及两者同时变化。
|
||||
这些理想几何测试不替代实测质量验证；单目位姿误差仍可能使数据无法通过。
|
||||
O12 的相邻平行轴图同时约束主动和被动弯曲轴的方向，而不只约束被动 DIP；各轴仍
|
||||
使用自己的实测旋转行程和轴线位置。配对任务须保持非平行上游轴在相同姿态，不能把
|
||||
其他姿态采到的轴方向直接当作当前方向。该约束不适用于 roll/yaw 等非平行轴。
|
||||
|
||||
如果动态拟合完成、空间零位存在解但验证未通过，会在会话的 `review_only/` 下保存
|
||||
文件名带 `REVIEW_ONLY` 的 URDF 和 `review_manifest.json`，方便检查；终端仍明确报告
|
||||
失败，不生成可用于控制的标定 JSON，也不更新 `latest_passed`。缺少几何数据、求解
|
||||
不可观测或修正超出允许范围时不生成复核模型。验证通过则正常发布，无需人工替换算法。
|
||||
标准 URDF 的滑块使用拟合后的关节角，不是原始 SDK 弧度；静态零位已写入 origin，
|
||||
不要再次加到滑块上。
|
||||
|
||||
重新完整采集时在原启动命令后加 `--no-resume`；仅验证算法则使用
|
||||
`--offline-raw <raw_samples.jsonl>`，不必重采。外参或 Tag 安装改变后，新旧观测不可混用。
|
||||
|
||||
O12 的命令端点与反馈端点不要求数值相等：完整命令轨迹负责驱动机械全行程，第一轮
|
||||
反馈的两个实测端点建立该方向的归一化输入域，后续轮次只需复现首轮实测行程的 90%。
|
||||
连续空洞门也只检查这段实测行程内部，不会把反馈比例或零位差形成的命令域末端区间
|
||||
误判成 Tag 遮挡。最终曲线和 URDF 端点同样使用该实测输入域，禁止向 SDK 命令端点
|
||||
进行未观测外推。
|
||||
|
||||
四指避让前会锁定无遮挡状态下的正面掌心 ID0 位姿；小指、无名指弯到上限遮住 ID0
|
||||
后，中指和食指的正面任务复用该会话固定基准，但 ID12/ID13 等运动 Tag 仍使用实时
|
||||
观测。运动 Tag 短时丢失只丢弃对应帧，恢复识别后继续采集，不会中断轨迹或误报映射
|
||||
失败；最终仍必须满足统一的有效样本与覆盖率质量门。
|
||||
|
||||
O12 的主动轴与避让轴共用同一条多轴平滑轨迹。正式扫描期间，避让轴的正常跟随滞后
|
||||
只作诊断；进入或退出避让的专用航点则要求侧摆进入 0.03 rad 到位带、每路屈曲反馈
|
||||
沿正确方向完成至少 90% 的首轮实测行程并稳定后才继续。避让命令仍发送 SDK 最大值，
|
||||
但完成判断不再把反馈弧度除以命令弧度；同时屈曲时 vendor solver 的反馈端点可以小于
|
||||
单轴命令上限。反馈弧度是连续拟合输入，而 Tag 相对旋转才是目标 URDF 关节角。
|
||||
真正的物理越限、活动硬件故障、通信失联
|
||||
和要求运动的轴连续两秒无推进仍会停止。
|
||||
O12 单独出现的 SDK `commu_except` bit4 只触发连续错误查询并记录诊断；只要完整
|
||||
12 路反馈仍然新鲜且目标轴正常推进，就不会把历史/瞬时通信位误判为失联。反馈流
|
||||
超时、目标轴无推进，或 bit0--bit3 堵转/过热/过流/电机异常仍会立即安全停止。
|
||||
|
||||
O12 的 FRONT/TOP 和 FRONT/SIDE 光轴都接近正交，允许最终外参批次 RMS 不超过
|
||||
2.0 px，同时继续要求三折旋转稳定性不超过 0.3°、平移稳定性不超过 1.5 mm。
|
||||
采集候选时将单相机和候选配对上限设为 2.5 px，以便斜视棋盘进入整批联合拟合；
|
||||
这两个候选上限不会替代最终的 2.0 px 批次门：
|
||||
|
||||
```bash
|
||||
ros2 launch linkerhand_calibration three_camera_extrinsics.launch.py \
|
||||
output_file:=$PWD/config/o12_three_camera_extrinsics.yaml \
|
||||
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027 \
|
||||
maximum_reprojection_rms_px:=2.0 \
|
||||
maximum_candidate_pair_reprojection_rms_px:=2.5 \
|
||||
maximum_single_camera_reprojection_rms_px:=2.5
|
||||
```
|
||||
|
||||
完整 SDK 范围策略不会复用旧 CAD 截断数据，必须重新完整采集。runner 默认
|
||||
寻找最新同版本兼容失败会话，使用 `--no-resume` 可禁用恢复。只有通过
|
||||
质量门的连续“任务/轮次/方向”单元会被复用；启动后仍先恢复全手安全基准，并执行
|
||||
恢复点所需的映射预检和避让。序列号、profile、schema 或任一受保护输入哈希变化
|
||||
时拒绝恢复。断点恢复默认安装未变；安装改变后应使用 `--no-resume` 从头采集。
|
||||
|
||||
只验证配置、16 张 16 mm Tag、相机/外参、源 URDF 和 SDK 配置哈希而不运动：
|
||||
|
||||
```bash
|
||||
@@ -262,23 +394,23 @@ URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll
|
||||
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记；
|
||||
恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。
|
||||
因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因
|
||||
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
|
||||
独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
|
||||
轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次
|
||||
采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面
|
||||
校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值，
|
||||
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面
|
||||
主测量和侧面校验测量独立保留。侧面校验视角的任务级有效率只记录为诊断；逐帧
|
||||
识别率低于标称值，
|
||||
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整
|
||||
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量
|
||||
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
|
||||
采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
|
||||
门限保持不变。`unified_engine_v1`取消每任务低速全行程预检；相邻方向共享已验证
|
||||
端点和任务级PnP参考。采样不足只按原速度重扫当前方向一次，拟合或第四轮留出
|
||||
失败立即锁定发布并保持当前位置，不再通过反复运动碰门限。G20右手正式扫描固定使用
|
||||
产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态
|
||||
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查：前三轮
|
||||
极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
|
||||
紧密簇、仅另一轮越界，自动补采该轮两个方向；无法明确定位时只重采完整yaw任务，
|
||||
不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
|
||||
紧密簇、仅另一轮越界，也只输出诊断并停止发布，不自动补采。与`latest_passed`
|
||||
中上一正式结果相差超过0.75°时另写入
|
||||
`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装，但该历史差值
|
||||
不直接否决当前会话，也不会用旧结果约束新零位。需要强制从第一个关节
|
||||
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||
不直接否决当前会话，也不会用旧结果约束新零位。默认恢复兼容断点，前提是安装未变；
|
||||
使用 `--no-resume` 可从第一个关节重新采集。
|
||||
升级前已经分别完成的正面/侧面roll也会合并为
|
||||
一个完整同步任务断点；只有两边数据都完整时才复用。
|
||||
|
||||
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
||||
@@ -327,8 +459,7 @@ ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
|
||||
侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过
|
||||
各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、
|
||||
用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉
|
||||
拟合和质量门限。每项正式四轮之前自动低速往返预检
|
||||
0/127/255可见性；
|
||||
拟合和质量门限。任务直接执行四轮双向正式扫描；
|
||||
|
||||
基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
|
||||
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中
|
||||
@@ -355,23 +486,22 @@ ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
|
||||
运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||
等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
|
||||
`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。
|
||||
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出，
|
||||
同一任务的四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出，
|
||||
不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且
|
||||
当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机
|
||||
退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的
|
||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
||||
不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值，
|
||||
不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。
|
||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。G20右手四轮正式速度始终使用
|
||||
产品配置的固定值，不会因本次帧率或识别密度而改变。
|
||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理
|
||||
零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||
四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。
|
||||
预检、正式四轮和拟合重扫始终使用速度5。
|
||||
正式四轮使用产品审定速度；唯一一次采样重扫保持相同速度。
|
||||
正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈；
|
||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再
|
||||
替代静态保持姿态。
|
||||
前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式
|
||||
扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||
每个任务在首个正式方向起点执行PnP静态初始化；四轮正式扫描连续复用同一帧间
|
||||
分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||
已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的
|
||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中
|
||||
|
||||
@@ -26,7 +26,7 @@ artifacts:
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
|
||||
calibration_config_sha256: afb323494140c88ab6368a061332fd80724a86831f44505dcfab147234e3c4be
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||
|
||||
|
||||
@@ -28,7 +28,7 @@ artifacts:
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
|
||||
calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
|
||||
calibration_config_sha256: e25e7ab27f4fd918f7cab70717e070d52a52fff17266a367c136388384ad4baa
|
||||
tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
|
||||
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
|
||||
|
||||
|
||||
@@ -61,7 +61,7 @@ l6_calibration:
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
automatic_sweep_retry_limit: 2
|
||||
automatic_sweep_retry_limit: 1
|
||||
non_target_motion_tolerance_u8: 3.0
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
|
||||
@@ -12,7 +12,7 @@ sdk:
|
||||
transport: hcan
|
||||
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
|
||||
config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
|
||||
config_sha256: ca1791c822bf1e99f25db8806397c3f877100c9252fcce6c53a7a6b8cef7694a
|
||||
config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c
|
||||
|
||||
cameras:
|
||||
front:
|
||||
@@ -31,10 +31,10 @@ cameras:
|
||||
artifacts:
|
||||
source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf
|
||||
source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
camera_extrinsics: config/o12_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: 5a515d0706f4e67d5e26bfddb348b519817bd72e885ea9e43997e41016176e53
|
||||
calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
|
||||
calibration_config_sha256: 9defe39a3f4b31c3764cbdcd9ee5b570da70e810215844515e8cc01e56414728
|
||||
calibration_config_sha256: 962a16ce3a21fd2d7886caab17c39a9441ff4eb856c9bf8e86c5c303c2291553
|
||||
tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml
|
||||
tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
|
||||
|
||||
|
||||
@@ -9,39 +9,43 @@ o12_calibration:
|
||||
top_camera_info_topic: /o12_calibration/top/camera/camera_info
|
||||
top_detections_topic: /o12_calibration/top/apriltag/detections
|
||||
|
||||
# O12 standard interface is radians. Raw 0..2000 mixed control is forbidden.
|
||||
command_rate_hz: 20.0
|
||||
# O12 standard interface is radians. Scan the complete reviewed SDK range;
|
||||
# the source CAD limits are outputs to correct, not acquisition limits.
|
||||
# Raw 0..2000 mixed control is forbidden.
|
||||
command_rate_hz: 50.0
|
||||
repetitions: 4
|
||||
tag_size_m: 0.016
|
||||
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_joint_frame_rate: 0.85
|
||||
minimum_feedback_hz: 15.0
|
||||
minimum_feedback_hz: 35.0
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_fraction: 0.90
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 2
|
||||
endpoint_tolerance_rad: 0.01
|
||||
endpoint_hold_seconds: 1.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 60.0
|
||||
sweep_timeout_seconds: 180.0
|
||||
automatic_sweep_retry_limit: 2
|
||||
automatic_sweep_retry_limit: 1
|
||||
non_target_motion_tolerance_rad: 0.015
|
||||
maximum_temperature_c: 70
|
||||
# 当前 O12 固件返回空温度报告。仍发起查询；5秒无结果后依赖已验证的
|
||||
# joint_error_states bit1 过热保护，并在会话记录中明确标注降级。
|
||||
temperature_report_required: false
|
||||
temperature_fallback_after_seconds: 5.0
|
||||
# 现场确认初始保守速度过慢；正式/点动速度提高到2倍，避让另有硬上限。
|
||||
motion_speed_scale: 2.0
|
||||
# 快速标定档：各任务按4倍请求，但拇指pitch、侧摆、屈曲及避让均有独立硬上限。
|
||||
motion_speed_scale: 4.0
|
||||
# 正弦速度加减速时间；中段保持关节限速，避免位置余弦全程低速。
|
||||
trajectory_ramp_seconds: 0.4
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_reprojection_tie_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
|
||||
@@ -28,7 +28,7 @@ artifacts:
|
||||
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
|
||||
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
|
||||
calibration_config_sha256: ce20d998a4342dfaacb14568513aa9af5063df48566fabd42180acc8da47e4a6
|
||||
calibration_config_sha256: a7fe124a195fa0f491535e96bdc728d25ea3efe298cf905e499917d329e9026f
|
||||
tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
|
||||
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
|
||||
|
||||
|
||||
@@ -58,7 +58,7 @@ o6_calibration:
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
automatic_sweep_retry_limit: 2
|
||||
automatic_sweep_retry_limit: 1
|
||||
non_target_motion_tolerance_u8: 3.0
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
|
||||
@@ -57,8 +57,8 @@ g20_calibration:
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见；
|
||||
# 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
fixed_base_maximum_corner_drift_px: 5.0
|
||||
fixed_base_movement_confirmation_frames: 10
|
||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
|
||||
# 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
|
||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||
@@ -110,13 +110,12 @@ g20_calibration:
|
||||
# roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
|
||||
baseline_hold_seconds: 0.5
|
||||
minimum_baseline_hold_frames: 10
|
||||
# 19-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。
|
||||
# unified_engine_v1 不执行每任务全行程预检；保留参数仅兼容旧配置读取。
|
||||
task_precheck_hold_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
|
||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
||||
# 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
|
||||
# 这类硬故障不自动重试。
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
motor_stall_startup_grace_seconds: 1.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
@@ -126,18 +125,18 @@ g20_calibration:
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 2
|
||||
automatic_sweep_retry_limit: 1
|
||||
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 2
|
||||
automatic_motion_retry_limit: 2
|
||||
automatic_fit_retry_limit: 0
|
||||
automatic_motion_retry_limit: 0
|
||||
# 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
|
||||
# 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
|
||||
cross_view_roll_diagnostic_finger: ""
|
||||
# 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [0.8, 0.6]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0]
|
||||
retry_speed_scales: [1.0]
|
||||
retry_endpoint_hold_seconds: [0.5]
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
"""Hardware- and model-independent calibration kernel."""
|
||||
|
||||
from .domain import (
|
||||
AcquisitionPolicy,
|
||||
ArtifactPolicy,
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
@@ -40,6 +41,7 @@ from .geometry import (
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"AcquisitionPolicy",
|
||||
"ArtifactPolicy",
|
||||
"CalibrationProfile",
|
||||
"CommandLayout",
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
"""Calibration domain types."""
|
||||
|
||||
from .profile import (
|
||||
AcquisitionPolicy,
|
||||
ArtifactPolicy,
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
@@ -21,6 +22,7 @@ from .profile import (
|
||||
from .sample import SampleRecord
|
||||
|
||||
__all__ = [
|
||||
"AcquisitionPolicy",
|
||||
"ArtifactPolicy",
|
||||
"CalibrationProfile",
|
||||
"CommandLayout",
|
||||
|
||||
@@ -66,6 +66,13 @@ class CommandLayout:
|
||||
baseline: tuple[float, ...] = ()
|
||||
lower_bounds: tuple[float, ...] = ()
|
||||
upper_bounds: tuple[float, ...] = ()
|
||||
# Position feedback is an observation, not a command. Servo tracking,
|
||||
# encoder quantisation, and the vendor's zero offset may put a valid
|
||||
# observation just outside the safe command envelope. Physical-angle
|
||||
# profiles can therefore register a separate feedback envelope; legacy
|
||||
# profiles default to the command envelope.
|
||||
feedback_lower_bounds: tuple[float, ...] = ()
|
||||
feedback_upper_bounds: tuple[float, ...] = ()
|
||||
feedback_by_index: bool = False
|
||||
|
||||
@property
|
||||
@@ -93,6 +100,22 @@ class CommandLayout:
|
||||
else (255.0,) * self.command_count
|
||||
)
|
||||
|
||||
@property
|
||||
def minimum_feedback_values(self) -> tuple[float, ...]:
|
||||
return (
|
||||
tuple(float(value) for value in self.feedback_lower_bounds)
|
||||
if self.feedback_lower_bounds
|
||||
else self.minimum_values
|
||||
)
|
||||
|
||||
@property
|
||||
def maximum_feedback_values(self) -> tuple[float, ...]:
|
||||
return (
|
||||
tuple(float(value) for value in self.feedback_upper_bounds)
|
||||
if self.feedback_upper_bounds
|
||||
else self.maximum_values
|
||||
)
|
||||
|
||||
def normalize(self, index: int, value: float) -> float:
|
||||
lower = self.minimum_values[int(index)]
|
||||
upper = self.maximum_values[int(index)]
|
||||
@@ -237,6 +260,24 @@ class QualityPolicy:
|
||||
isolated_holdout: bool = False
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AcquisitionPolicy:
|
||||
"""Cross-model runtime safety and retained-data acceptance contract."""
|
||||
|
||||
policy_version: str = "unified_engine_v1"
|
||||
mapping_probe_maximum_rad: float = 0.0
|
||||
automatic_rescan_limit: int = 1
|
||||
minimum_valid_samples: int = 40
|
||||
minimum_bins: int = 32
|
||||
maximum_unobserved_fraction: float = 1.0 / 16.0
|
||||
legacy_minimum_span_01: float = 240.0 / 255.0
|
||||
physical_first_cycle_minimum_span_01: float = 0.85
|
||||
physical_repeat_minimum_fraction: float = 0.90
|
||||
stall_timeout_seconds: float = 2.0
|
||||
fixed_reference_maximum_drift_px: float = 5.0
|
||||
fixed_reference_confirmation_frames: int = 10
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScopePolicy:
|
||||
calibrate_joints: Mapping[str, frozenset[str]]
|
||||
@@ -273,6 +314,7 @@ class CalibrationProfile:
|
||||
quality: QualityPolicy
|
||||
scope: ScopePolicy
|
||||
artifacts: ArtifactPolicy
|
||||
acquisition: AcquisitionPolicy = field(default_factory=AcquisitionPolicy)
|
||||
# Per-URDF-joint provenance used by partial calibration artifacts.
|
||||
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
|
||||
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
|
||||
@@ -312,6 +354,27 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
||||
)
|
||||
):
|
||||
errors.append("command bounds or baseline values are invalid")
|
||||
if (
|
||||
len(command.minimum_feedback_values) != command.command_count
|
||||
or len(command.maximum_feedback_values) != command.command_count
|
||||
):
|
||||
errors.append("feedback bounds must align with command names")
|
||||
elif any(
|
||||
not math.isfinite(feedback_lower)
|
||||
or not math.isfinite(feedback_upper)
|
||||
or feedback_lower >= feedback_upper
|
||||
or feedback_lower > command_lower
|
||||
or feedback_upper < command_upper
|
||||
for feedback_lower, feedback_upper, command_lower, command_upper in zip(
|
||||
command.minimum_feedback_values,
|
||||
command.maximum_feedback_values,
|
||||
command.minimum_values,
|
||||
command.maximum_values,
|
||||
)
|
||||
):
|
||||
errors.append(
|
||||
"feedback bounds must be finite and contain the command domain"
|
||||
)
|
||||
indices = set(range(command.command_count))
|
||||
if not set(command.disabled_indices).issubset(indices):
|
||||
errors.append("disabled command index is out of range")
|
||||
@@ -430,7 +493,12 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
||||
):
|
||||
errors.append("coupling model target has no source mapping")
|
||||
if not set(zero.coupling_model_by_joint.values()).issubset(
|
||||
{"linear_mimic", "quadratic_runtime"}
|
||||
{
|
||||
"linear_mimic",
|
||||
"quadratic_runtime",
|
||||
"direction_aware_knots",
|
||||
"vendor_o12_polynomial",
|
||||
}
|
||||
):
|
||||
errors.append("coupling model policy is unsupported")
|
||||
|
||||
@@ -457,6 +525,19 @@ def validate_profile(profile: CalibrationProfile) -> None:
|
||||
errors.append(f"{label} filename must not contain a directory")
|
||||
if not profile.namespace.startswith("/"):
|
||||
errors.append("runtime namespace must be absolute")
|
||||
acquisition = profile.acquisition
|
||||
if acquisition.policy_version != "unified_engine_v1":
|
||||
errors.append("unsupported acquisition policy version")
|
||||
if acquisition.automatic_rescan_limit != 1:
|
||||
errors.append("unified engine requires exactly one automatic rescan")
|
||||
if acquisition.minimum_valid_samples < 40 or acquisition.minimum_bins < 32:
|
||||
errors.append("acquisition sample and bin minima are too small")
|
||||
if not 0.0 < acquisition.maximum_unobserved_fraction <= 1.0 / 16.0:
|
||||
errors.append("maximum unobserved travel must not exceed 1/16")
|
||||
if command.unit == "rad" and not 0.0 < acquisition.mapping_probe_maximum_rad <= math.radians(3.0):
|
||||
errors.append("physical-angle profiles require a <=3 degree mapping probe")
|
||||
if command.unit == "u8" and acquisition.mapping_probe_maximum_rad != 0.0:
|
||||
errors.append("legacy profiles must not request a radian mapping probe")
|
||||
if profile.joint_coverage:
|
||||
valid_coverage = {
|
||||
"measured_static_dynamic",
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Projection contract for detections on image_proc's rectified images."""
|
||||
import numpy as np
|
||||
|
||||
|
||||
def rectified_camera_matrix(projection):
|
||||
"""CameraInfo.P, not raw K, is the intrinsic matrix of image_rect.
|
||||
|
||||
Never silently fall back to K: that fits a different pixel coordinate
|
||||
system and can produce smooth but biased tag poses.
|
||||
"""
|
||||
values = np.asarray(projection, dtype=float)
|
||||
if values.size != 12:
|
||||
raise ValueError('rectified detections require a 3x4 CameraInfo.P')
|
||||
matrix = values.reshape(3, 4)[:, :3].copy()
|
||||
if (not np.all(np.isfinite(matrix)) or matrix[0, 0] <= 0
|
||||
or matrix[1, 1] <= 0 or not np.allclose(matrix[2], [0, 0, 1])):
|
||||
raise ValueError('invalid rectified CameraInfo.P')
|
||||
return matrix
|
||||
@@ -152,6 +152,28 @@ def fit_rotation_axis(
|
||||
projections = matrix @ axis
|
||||
low = projections[command_values <= 16]
|
||||
high = projections[command_values >= 239]
|
||||
# Physical-angle adapters normalize their measured feedback into the
|
||||
# legacy 0..255 fitting coordinate. Tracking lag and mechanical travel
|
||||
# mean those records need not reach the historical <=16 endpoint even
|
||||
# when the observed physical stroke is complete. SVD axis sign is
|
||||
# arbitrary, so fall back to the ends of the *observed* command domain
|
||||
# instead of allowing that arbitrary sign to reverse an otherwise valid
|
||||
# trajectory. Byte-feedback products that observe the canonical end
|
||||
# bands retain their existing behaviour.
|
||||
if not low.size or not high.size:
|
||||
observed = command_values[useful]
|
||||
lower = int(np.min(observed))
|
||||
upper = int(np.max(observed))
|
||||
span = upper - lower
|
||||
if span <= 0:
|
||||
raise ValueError("commands do not span a rotation trajectory")
|
||||
band = max(1, int(math.ceil(0.1 * span)))
|
||||
low = projections[
|
||||
useful & (command_values <= lower + band)
|
||||
]
|
||||
high = projections[
|
||||
useful & (command_values >= upper - band)
|
||||
]
|
||||
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
|
||||
axis = -axis
|
||||
return axis / np.linalg.norm(axis)
|
||||
|
||||
@@ -7,6 +7,7 @@ from .patch import (
|
||||
UrdfPatchSet,
|
||||
apply_urdf_patch_text,
|
||||
materialize_relative_mesh_assets,
|
||||
validate_urdf_mimic_ranges,
|
||||
write_urdf_patches,
|
||||
)
|
||||
|
||||
@@ -18,5 +19,6 @@ __all__ = [
|
||||
"apply_urdf_patch_text",
|
||||
"build_correction_plan",
|
||||
"materialize_relative_mesh_assets",
|
||||
"validate_urdf_mimic_ranges",
|
||||
"write_urdf_patches",
|
||||
]
|
||||
|
||||
@@ -9,6 +9,7 @@ publishing a new file.
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
@@ -191,6 +192,99 @@ def _files_have_identical_contents(left: Path, right: Path) -> bool:
|
||||
return True
|
||||
|
||||
|
||||
def _mimic_reachable_ranges(
|
||||
root: ET.Element,
|
||||
) -> tuple[dict[str, tuple[float, float]], dict[str, float]]:
|
||||
"""Resolve full mimic chains and report limit excess for each joint."""
|
||||
nodes = {
|
||||
str(node.get("name")): node
|
||||
for node in root.findall("joint")
|
||||
if node.get("type") in {"revolute", "prismatic"}
|
||||
}
|
||||
resolved: dict[str, tuple[float, float]] = {}
|
||||
resolving: set[str] = set()
|
||||
|
||||
def resolve(name: str) -> tuple[float, float]:
|
||||
if name in resolved:
|
||||
return resolved[name]
|
||||
if name in resolving:
|
||||
raise ValueError(f"URDF mimic cycle contains joint: {name}")
|
||||
node = nodes.get(name)
|
||||
if node is None:
|
||||
raise ValueError(f"URDF mimic source joint does not exist: {name}")
|
||||
limit = node.find("limit")
|
||||
if limit is None:
|
||||
raise ValueError(f"bounded URDF joint has no limit: {name}")
|
||||
lower = float(limit.get("lower", "nan"))
|
||||
upper = float(limit.get("upper", "nan"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper:
|
||||
raise ValueError(f"URDF joint has invalid limits: {name}")
|
||||
resolving.add(name)
|
||||
mimic = node.find("mimic")
|
||||
if mimic is None:
|
||||
reachable = (lower, upper)
|
||||
else:
|
||||
source = str(mimic.get("joint", ""))
|
||||
multiplier = float(mimic.get("multiplier", "1"))
|
||||
offset = float(mimic.get("offset", "0"))
|
||||
if not math.isfinite(multiplier) or not math.isfinite(offset):
|
||||
raise ValueError(f"URDF joint has invalid mimic values: {name}")
|
||||
source_range = resolve(source)
|
||||
endpoints = (
|
||||
offset + multiplier * source_range[0],
|
||||
offset + multiplier * source_range[1],
|
||||
)
|
||||
reachable = (min(endpoints), max(endpoints))
|
||||
resolving.remove(name)
|
||||
resolved[name] = reachable
|
||||
return reachable
|
||||
|
||||
for joint_name in nodes:
|
||||
resolve(joint_name)
|
||||
excess: dict[str, float] = {}
|
||||
for name, reachable in resolved.items():
|
||||
node = nodes[name]
|
||||
if node.find("mimic") is None:
|
||||
continue
|
||||
limit = node.find("limit")
|
||||
assert limit is not None
|
||||
lower = float(limit.get("lower", "nan"))
|
||||
upper = float(limit.get("upper", "nan"))
|
||||
excess[name] = max(0.0, lower - reachable[0], reachable[1] - upper)
|
||||
return resolved, excess
|
||||
|
||||
|
||||
def validate_urdf_mimic_ranges(
|
||||
urdf: str | Path,
|
||||
*,
|
||||
reference_urdf: str | Path | None = None,
|
||||
tolerance: float = 1.0e-9,
|
||||
) -> Mapping[str, tuple[float, float]]:
|
||||
"""Reject new physical-limit violations anywhere in a mimic chain.
|
||||
|
||||
``reference_urdf`` permits only an already-present CAD rounding excess of
|
||||
the same joint. Calibration may never enlarge that excess.
|
||||
"""
|
||||
root = ET.parse(Path(urdf)).getroot()
|
||||
ranges, excess = _mimic_reachable_ranges(root)
|
||||
reference_excess: Mapping[str, float] = {}
|
||||
if reference_urdf is not None:
|
||||
_, reference_excess = _mimic_reachable_ranges(
|
||||
ET.parse(Path(reference_urdf)).getroot()
|
||||
)
|
||||
failures = {
|
||||
name: value
|
||||
for name, value in excess.items()
|
||||
if value > float(reference_excess.get(name, 0.0)) + float(tolerance)
|
||||
}
|
||||
if failures:
|
||||
details = ",".join(
|
||||
f"{name}={value:.9f}rad" for name, value in sorted(failures.items())
|
||||
)
|
||||
raise ValueError(f"URDF mimic reachable range exceeds limits: {details}")
|
||||
return ranges
|
||||
|
||||
|
||||
def materialize_relative_mesh_assets(
|
||||
*, source: Path, output: Path, urdf_root: ET.Element
|
||||
) -> tuple[Path, ...]:
|
||||
@@ -311,5 +405,6 @@ __all__ = [
|
||||
"UrdfPatchSet",
|
||||
"apply_urdf_patch_text",
|
||||
"materialize_relative_mesh_assets",
|
||||
"validate_urdf_mimic_ranges",
|
||||
"write_urdf_patches",
|
||||
]
|
||||
|
||||
@@ -33,7 +33,6 @@ _HARD_THRESHOLD_KEYS = frozenset(
|
||||
"maximum_reprojection_error_px",
|
||||
"maximum_axis_cycle_difference_rad",
|
||||
"maximum_pose_line_rms_m",
|
||||
"maximum_hysteresis_rad",
|
||||
"maximum_validation_error_rad",
|
||||
}
|
||||
)
|
||||
|
||||
@@ -40,6 +40,8 @@ from ...core import (
|
||||
robust_rotation_summary,
|
||||
)
|
||||
from ...core.urdf import build_correction_plan
|
||||
from ...runtime import ACQUISITION_POLICY_VERSION, CalibrationEngine
|
||||
from ...runtime.adapters import ProfileSdkAdapter
|
||||
from ...extrinsics import (
|
||||
ThreeCameraExtrinsics,
|
||||
camera_info_fingerprint,
|
||||
@@ -2628,8 +2630,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.declare_parameter("pnp_group_initialization_frames", 8)
|
||||
self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0)
|
||||
self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0)
|
||||
self.declare_parameter("fixed_base_maximum_corner_drift_px", 2.0)
|
||||
self.declare_parameter("fixed_base_movement_confirmation_frames", 5)
|
||||
self.declare_parameter("fixed_base_maximum_corner_drift_px", 5.0)
|
||||
self.declare_parameter("fixed_base_movement_confirmation_frames", 10)
|
||||
self.declare_parameter("thumb_ip_pnp_coupling_multiplier", 1.03)
|
||||
self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0)
|
||||
self.declare_parameter(
|
||||
@@ -2686,7 +2688,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.declare_parameter("task_precheck_hold_seconds", 2.0)
|
||||
self.declare_parameter("position_timeout_seconds", 30.0)
|
||||
self.declare_parameter("sweep_timeout_seconds", 90.0)
|
||||
self.declare_parameter("motor_stall_timeout_seconds", 5.0)
|
||||
self.declare_parameter("motor_stall_timeout_seconds", 2.0)
|
||||
self.declare_parameter("motor_stall_startup_grace_seconds", 1.0)
|
||||
self.declare_parameter("motor_stall_minimum_progress_u8", 1.0)
|
||||
self.declare_parameter("invalid_timeout_seconds", 3.0)
|
||||
@@ -2694,15 +2696,15 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.declare_parameter("minimum_state_span_u8", 240.0)
|
||||
self.declare_parameter("minimum_sweep_bins", 32)
|
||||
self.declare_parameter("maximum_bin_gap", 16)
|
||||
self.declare_parameter("automatic_sweep_retry_limit", 3)
|
||||
self.declare_parameter("automatic_fit_retry_limit", 2)
|
||||
self.declare_parameter("automatic_motion_retry_limit", 2)
|
||||
self.declare_parameter("automatic_sweep_retry_limit", 1)
|
||||
self.declare_parameter("automatic_fit_retry_limit", 0)
|
||||
self.declare_parameter("automatic_motion_retry_limit", 0)
|
||||
self.declare_parameter("cross_view_roll_diagnostic_finger", "")
|
||||
self.declare_parameter("provisional_warning_ratio", 1.25)
|
||||
self.declare_parameter("retry_minimum_speed", 3)
|
||||
self.declare_parameter("retry_speed_scales", [0.8, 0.6, 0.5])
|
||||
self.declare_parameter("retry_speed_scales", [1.0])
|
||||
self.declare_parameter(
|
||||
"retry_endpoint_hold_seconds", [0.75, 1.0, 1.25]
|
||||
"retry_endpoint_hold_seconds", [0.5]
|
||||
)
|
||||
self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004)
|
||||
self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004)
|
||||
@@ -2779,6 +2781,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.profile = product_contract.profile
|
||||
self.zero_profile = product_contract.zero_profile
|
||||
self.calibration_profile = product_contract.typed_profile
|
||||
self.calibration_engine = CalibrationEngine(self.calibration_profile)
|
||||
self.sdk_adapter = ProfileSdkAdapter(self.calibration_profile.command)
|
||||
self.serial_number = str(value("serial_number"))
|
||||
if self.serial_number == "UNSET":
|
||||
raise ValueError("serial_number is required")
|
||||
@@ -4066,16 +4070,34 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
|
||||
def _state_callback(self, message: JointState) -> None:
|
||||
command_names = _command_names(self)
|
||||
if len(message.position) != len(command_names):
|
||||
named_feedback = (
|
||||
tuple(message.name)
|
||||
if (
|
||||
len(message.name) == len(command_names)
|
||||
and set(message.name) == set(command_names)
|
||||
)
|
||||
else ()
|
||||
)
|
||||
adapter = getattr(self, "sdk_adapter", None)
|
||||
typed_profile = getattr(self, "calibration_profile", None)
|
||||
if adapter is None and typed_profile is not None:
|
||||
adapter = ProfileSdkAdapter(self.calibration_profile.command)
|
||||
state = (
|
||||
adapter.parse_feedback(named_feedback, message.position)
|
||||
if adapter is not None
|
||||
else (
|
||||
tuple(
|
||||
float(dict(zip(message.name, message.position))[name])
|
||||
for name in command_names
|
||||
)
|
||||
if named_feedback
|
||||
else tuple(float(value) for value in message.position)
|
||||
if len(message.position) == len(command_names)
|
||||
else None
|
||||
)
|
||||
)
|
||||
if state is None:
|
||||
return
|
||||
if (
|
||||
len(message.name) == len(command_names)
|
||||
and set(message.name) == set(command_names)
|
||||
):
|
||||
lookup = dict(zip(message.name, message.position))
|
||||
state = tuple(float(lookup[name]) for name in command_names)
|
||||
else:
|
||||
state = tuple(float(value) for value in message.position)
|
||||
stamp = _stamp_ns(message.header.stamp)
|
||||
if stamp <= 0:
|
||||
stamp = int(self.get_clock().now().nanoseconds)
|
||||
@@ -5291,6 +5313,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
if len(starts) != 1:
|
||||
raise RuntimeError("resume raw must contain exactly one session_start")
|
||||
start = starts[0]
|
||||
if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION:
|
||||
raise RuntimeError(
|
||||
"resume acquisition policy differs; unified_engine_v1 "
|
||||
"requires a new full capture"
|
||||
)
|
||||
previous_tag_sizes = {
|
||||
int(tag_id): float(size)
|
||||
for tag_id, size in dict(
|
||||
@@ -5800,6 +5827,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.raw_path,
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"model": self.model,
|
||||
"hand_type": self.hand_type,
|
||||
"tag_layout": self.profile.layout_id,
|
||||
@@ -6149,7 +6177,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
retry = self.sweep_retry_counts.get(key, 0)
|
||||
if retry:
|
||||
scale = self.retry_speed_scales[min(retry, len(self.retry_speed_scales)) - 1]
|
||||
scale = self.calibration_engine.retry_speed(1.0, 2)
|
||||
speeds = [
|
||||
max(self.retry_minimum_speed, int(round(speed * scale)))
|
||||
for speed in speeds
|
||||
@@ -7002,10 +7030,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
retry = self.sweep_retry_counts.get(key, 0)
|
||||
if retry <= 0:
|
||||
return float(self.sweep_timeout_seconds)
|
||||
scale = self.retry_speed_scales[
|
||||
min(retry, len(self.retry_speed_scales)) - 1
|
||||
]
|
||||
return float(self.sweep_timeout_seconds) / float(scale)
|
||||
# unified_engine_v1 retries at the original speed.
|
||||
return float(self.sweep_timeout_seconds)
|
||||
|
||||
def _endpoint_tolerance_for_spec(
|
||||
self, spec: SweepSpec, endpoint_u8: int
|
||||
@@ -9238,9 +9264,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
band_attempt = int(
|
||||
self.sweep_attempts.get(_sweep_storage_key(spec), 1)
|
||||
)
|
||||
retry_limit = int(
|
||||
getattr(self, "automatic_fit_retry_limit", 0)
|
||||
)
|
||||
retry_limit = 0
|
||||
# The final fit re-applies the unmodified hard thresholds to the
|
||||
# same records. Letting even a tiny overrun continue would make
|
||||
# the final fit recall this task after every later joint has been
|
||||
@@ -9336,9 +9360,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
_fit_failure_is_systematic(failures, self.repetitions)
|
||||
or repeated_branch_clusters
|
||||
)
|
||||
fit_retry_limit = int(
|
||||
getattr(self, "automatic_fit_retry_limit", 0)
|
||||
)
|
||||
fit_retry_limit = 0
|
||||
self.fit_failure = {
|
||||
"kind": "fit_failure",
|
||||
"view": spec.view,
|
||||
@@ -9559,8 +9581,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
_sweep_storage_key(item.spec), item.cycle, item.direction
|
||||
)
|
||||
retries = self.sweep_retry_counts.get(key, 0)
|
||||
retry_limit = getattr(self, "automatic_sweep_retry_limit", 0)
|
||||
if retries >= retry_limit:
|
||||
retry_limit = 1
|
||||
if not CalibrationEngine.permits_retry("sweep_acquisition", retries):
|
||||
self._pause(reason)
|
||||
return
|
||||
retries += 1
|
||||
@@ -9583,10 +9605,10 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self._reset_view_trackers(runtime)
|
||||
runtime.pnp_reset_count += 1
|
||||
speed_scales = getattr(
|
||||
self, "retry_speed_scales", (0.8, 0.6, 0.5)
|
||||
self, "retry_speed_scales", (1.0,)
|
||||
)
|
||||
endpoint_holds = getattr(
|
||||
self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25)
|
||||
self, "retry_endpoint_hold_seconds", (0.5,)
|
||||
)
|
||||
# Write the invalidation before mutating the authoritative in-memory
|
||||
# stores. Restart replays this event as a tombstone, so a crash can
|
||||
@@ -10000,7 +10022,73 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
return
|
||||
|
||||
total_by_view = getattr(self, "sweep_detection_total_by_view", {})
|
||||
valid_by_view = getattr(self, "sweep_detection_valid_by_view", {})
|
||||
rates = [
|
||||
float(valid_by_view.get(view, 0)) / float(total_by_view[view])
|
||||
for view in _sweep_views(_node_profile(self), item.spec)
|
||||
if int(total_by_view.get(view, 0)) > 0
|
||||
]
|
||||
detection_rate = min(rates, default=0.0)
|
||||
for joint_name, bins in joint_bins.items():
|
||||
feedback = [
|
||||
float(frame.state_u8[motor])
|
||||
for frame in _frames_for_joint(self.sweep_frames, joint_name)
|
||||
]
|
||||
engine = getattr(self, "calibration_engine", None)
|
||||
if engine is None:
|
||||
typed_profile = getattr(self, "calibration_profile", None)
|
||||
if typed_profile is None:
|
||||
legacy_profile = getattr(
|
||||
self,
|
||||
"profile",
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
)
|
||||
typed_profile = get_product_calibration_contract(
|
||||
str(getattr(self, "model", "G20")),
|
||||
legacy_profile.side,
|
||||
legacy_profile.layout_id,
|
||||
).typed_profile
|
||||
engine = CalibrationEngine(typed_profile)
|
||||
receive_times = getattr(self, "state_receive_times", ())
|
||||
quality = engine.evaluate_sweep(
|
||||
[value / 255.0 for value in feedback],
|
||||
minimum_span=getattr(self, "minimum_state_span_u8", 240.0) / 255.0,
|
||||
total_frames=max(total_by_view.values(), default=0),
|
||||
joint_frame_rate=(
|
||||
0.0
|
||||
if max(total_by_view.values(), default=0) == 0
|
||||
else len(feedback) / max(total_by_view.values())
|
||||
),
|
||||
feedback_hz=(
|
||||
0.0
|
||||
if len(receive_times) < 2
|
||||
or receive_times[-1] <= receive_times[0]
|
||||
else (len(receive_times) - 1)
|
||||
/ (receive_times[-1] - receive_times[0])
|
||||
),
|
||||
detection_rate=detection_rate,
|
||||
bin_count=256,
|
||||
)
|
||||
if getattr(self, "raw_path", None) is not None:
|
||||
append_jsonl(self.raw_path, {
|
||||
"kind": "unified_sweep_observation_quality",
|
||||
"task_name": item.spec.key,
|
||||
"joint": joint_name,
|
||||
"cycle": item.cycle,
|
||||
"direction": item.direction,
|
||||
**quality.metrics,
|
||||
"warnings": list(quality.warnings),
|
||||
"failures": list(quality.failures),
|
||||
"passed": quality.passed,
|
||||
})
|
||||
if not quality.passed:
|
||||
self._retry_active_sweep_or_pause(
|
||||
_joint_failure_reason(
|
||||
"sweep_observability_failed", item.spec, joint_name
|
||||
) + ":" + ",".join(quality.failures)
|
||||
)
|
||||
return
|
||||
commands = sorted(bins)
|
||||
if len(commands) < self.minimum_sweep_bins:
|
||||
self._retry_active_sweep_or_pause(
|
||||
@@ -11579,6 +11667,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
def _retry_combination_validation_or_pause(
|
||||
self, reason: str, now: float
|
||||
) -> None:
|
||||
if not CalibrationEngine.permits_retry("holdout", 0):
|
||||
self._pause(reason)
|
||||
return
|
||||
item = self.active_combination_validation
|
||||
if item is None:
|
||||
self._pause(reason)
|
||||
@@ -11771,6 +11862,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
name: round(float(value), 8)
|
||||
for name, value in self.zero_result.all_active_offsets_rad.items()
|
||||
}
|
||||
# All models cross the same internal 3+1 result gate before either
|
||||
# their deployed JSON serializer or the URDF patcher may run.
|
||||
engine = getattr(self, "calibration_engine", None)
|
||||
if engine is not None:
|
||||
self.unified_result = engine.result_from_fit(
|
||||
self.zero_result,
|
||||
curves=self.measured_fits,
|
||||
zero_offsets_rad=published_zero_offsets,
|
||||
holdout_errors_rad={
|
||||
"random_validation": tuple(self.validation_errors_rad)
|
||||
},
|
||||
metadata={"layout_id": self.profile.layout_id},
|
||||
)
|
||||
urdf_offsets = published_zero_offsets
|
||||
if self.profile.layout_id == G20_RIGHT_19_LAYOUT:
|
||||
urdf_offsets = {
|
||||
@@ -11962,7 +12066,30 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.reason = str(reason)
|
||||
self.position_hold_since = None
|
||||
|
||||
def _handle_sweep_start_timeout(self, now: float, reached: bool) -> None:
|
||||
if not reached:
|
||||
self._pause("sweep_start_position_timeout")
|
||||
return
|
||||
assert self.active_sweep is not None
|
||||
# Vision is not a live motion interlock. Start the sweep and let the
|
||||
# retained-data gate decide whether this direction needs its single
|
||||
# same-speed rescan.
|
||||
append_jsonl(
|
||||
self.raw_path,
|
||||
{
|
||||
"kind": "sweep_start_vision_timeout_warning",
|
||||
"task_name": self.active_sweep.spec.key,
|
||||
"cycle": self.active_sweep.cycle,
|
||||
"direction": self.active_sweep.direction,
|
||||
},
|
||||
)
|
||||
self.position_hold_since = None
|
||||
self._begin_active_sweep(now)
|
||||
|
||||
def _retry_motion_or_pause(self, reason: str, now: float) -> None:
|
||||
if not CalibrationEngine.permits_retry("motion", 0):
|
||||
self._pause(reason)
|
||||
return
|
||||
retries = self.motion_retry_counts.get(reason, 0)
|
||||
if retries >= self.automatic_motion_retry_limit:
|
||||
self._pause(reason)
|
||||
@@ -12064,6 +12191,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.reason = f"automatic_retry_{reason}"
|
||||
|
||||
def _retry_validation_or_pause(self, reason: str, now: float) -> None:
|
||||
if not CalibrationEngine.permits_retry("holdout", 0):
|
||||
self._pause(reason)
|
||||
return
|
||||
if self.active_validation is None:
|
||||
self._pause(reason)
|
||||
return
|
||||
@@ -12170,9 +12300,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
):
|
||||
return
|
||||
if now - self.motion_stage_started_at > self.position_timeout_seconds:
|
||||
self._retry_motion_or_pause("return_baseline_timeout", now)
|
||||
return
|
||||
if baseline_reached:
|
||||
return_waypoints = getattr(
|
||||
self, "return_waypoints", deque()
|
||||
@@ -12249,15 +12376,12 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
),
|
||||
)
|
||||
reached = self._command_vector_reached(preparation_command)
|
||||
if now - self.motion_stage_started_at > self.position_timeout_seconds:
|
||||
self._retry_motion_or_pause(
|
||||
(
|
||||
"sweep_start_tag_timeout"
|
||||
if reached
|
||||
else "sweep_start_position_timeout"
|
||||
),
|
||||
now,
|
||||
)
|
||||
if (
|
||||
reached
|
||||
and now - self.motion_stage_started_at
|
||||
> self.position_timeout_seconds
|
||||
):
|
||||
self._handle_sweep_start_timeout(now, reached)
|
||||
return
|
||||
prepare_context = (
|
||||
f"prepare_motor_{self.active_sweep.spec.motor_index}"
|
||||
@@ -12350,26 +12474,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
if self.state == STATE_SWEEP:
|
||||
assert self.active_sweep is not None
|
||||
motor = self.active_sweep.spec.motor_index
|
||||
if now - self.sweep_started_at > self._active_sweep_timeout_seconds():
|
||||
self._retry_active_sweep_or_pause("sweep_timeout")
|
||||
return
|
||||
stale_views = [
|
||||
view
|
||||
for view in _sweep_views(
|
||||
self.profile, self.active_sweep.spec
|
||||
)
|
||||
if now
|
||||
- getattr(self, "sweep_last_valid_at_by_view", {}).get(
|
||||
view, self.sweep_started_at
|
||||
)
|
||||
> self.invalid_timeout_seconds
|
||||
]
|
||||
if stale_views:
|
||||
self._retry_active_sweep_or_pause(
|
||||
"synchronised_tag_state_timeout:"
|
||||
+ ",".join(stale_views)
|
||||
)
|
||||
return
|
||||
# Temporary vision/synchronisation loss only drops those frames.
|
||||
# Complete the motion and let retained-data quality request the
|
||||
# single same-speed rescan when necessary.
|
||||
checkpoint_target_value = getattr(
|
||||
self, "sweep_checkpoint_target_u8", None
|
||||
)
|
||||
@@ -12532,13 +12639,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.sweep_endpoint_since is not None
|
||||
and now - self.sweep_endpoint_since
|
||||
>= self._active_endpoint_hold_seconds()
|
||||
and _frames_cover_sweep_motion(
|
||||
self.sweep_frames,
|
||||
self.active_sweep.spec,
|
||||
motor_index=motor,
|
||||
minimum_per_joint=self.minimum_sweep_frames,
|
||||
minimum_span_u8=self.minimum_state_span_u8,
|
||||
)
|
||||
):
|
||||
self._finish_active_sweep()
|
||||
return
|
||||
@@ -12575,10 +12675,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
else:
|
||||
self.position_hold_since = None
|
||||
if now - self.validation_stage_started_at > self.validation_timeout_seconds:
|
||||
self._retry_combination_validation_or_pause(
|
||||
"combination_validation_move_timeout", now
|
||||
)
|
||||
return
|
||||
assert self.active_validation is not None
|
||||
reached = self._motion_command_reached(
|
||||
@@ -12619,8 +12715,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.reason = "capturing_random_validation_pose"
|
||||
else:
|
||||
self.position_hold_since = None
|
||||
if now - self.validation_stage_started_at > self.validation_timeout_seconds:
|
||||
self._retry_validation_or_pause("validation_move_timeout", now)
|
||||
return
|
||||
if self.state == STATE_VALIDATION_CAPTURE:
|
||||
if self.active_combination_validation is not None:
|
||||
|
||||
@@ -710,7 +710,6 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
palm_orientation_maximum_command_distance_u8=64,
|
||||
capabilities=frozenset(
|
||||
{
|
||||
"precheck_sweeps",
|
||||
"steady_command_checkpoints",
|
||||
"directional_zero",
|
||||
"isolated_holdout",
|
||||
@@ -721,7 +720,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
"palm_axis_relative_motion_v3",
|
||||
}
|
||||
),
|
||||
precheck_sweeps=True,
|
||||
precheck_sweeps=False,
|
||||
steady_command_checkpoints=True,
|
||||
directional_zero=True,
|
||||
isolated_holdout=True,
|
||||
|
||||
@@ -29,6 +29,7 @@ from ...operator_report import (
|
||||
from ...product import ProductConfig, load_product_config, sha256_file
|
||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||
from ...storage import atomic_write_json
|
||||
from ...runtime import ACQUISITION_POLICY_VERSION
|
||||
|
||||
|
||||
EXIT_PASS = 0
|
||||
@@ -131,7 +132,9 @@ class ProgressConsole:
|
||||
"\n".join(
|
||||
[
|
||||
f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}",
|
||||
f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）",
|
||||
"系统处理：只按原速度重扫当前方向"
|
||||
f"（第 {active.get('automatic_retry_count', 1)}/"
|
||||
f"{active.get('automatic_retry_limit', 1)} 次）",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
@@ -476,6 +479,8 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
continue
|
||||
if (
|
||||
start is None
|
||||
or start.get("acquisition_policy_version")
|
||||
!= ACQUISITION_POLICY_VERSION
|
||||
or start.get("hand_type") != config.side
|
||||
or start.get("tag_layout") != config.tag_layout
|
||||
or start.get("source_urdf_sha256")
|
||||
|
||||
@@ -80,6 +80,14 @@ class ZeroCalibrationProfile:
|
||||
# signed rotation axes to disambiguate the otherwise mirrored palm-frame
|
||||
# branches. The default remains undirected for legacy G20/L6 profiles.
|
||||
directed_base_axis_joints: frozenset[str] = frozenset()
|
||||
# A profile may retain zero when a *bounded training confidence interval*
|
||||
# contains it. The frozen zero still faces every geometry/cycle/holdout
|
||||
# check below. Default False preserves existing G20/L6/O6 decisions.
|
||||
accept_validated_zero_in_confidence_interval: bool = False
|
||||
# Axis-line points have no unique coordinate along the axis. Remove that
|
||||
# gauge before projecting a parallel-axis phase into the camera plane.
|
||||
# Opt in explicitly while legacy profiles retain their reviewed policy.
|
||||
project_axis_gauge_before_image: bool = False
|
||||
|
||||
@property
|
||||
def reference_finger(self) -> str:
|
||||
@@ -815,7 +823,9 @@ def _canonical_reference_records(
|
||||
|
||||
|
||||
def _interpolate_reference_rotation(
|
||||
records: Sequence[Mapping[str, Any]], zero_command_u8: int
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
zero_command_u8: int,
|
||||
maximum_distance_u8: int | None = ZERO_REFERENCE_MAXIMUM_DISTANCE_U8,
|
||||
) -> Rotation | None:
|
||||
by_command: dict[int, list[np.ndarray]] = {}
|
||||
for record in records:
|
||||
@@ -839,7 +849,9 @@ def _interpolate_reference_rotation(
|
||||
if lower_command is not None and upper_command is not None:
|
||||
lower_distance = zero - lower_command
|
||||
upper_distance = upper_command - zero
|
||||
if max(lower_distance, upper_distance) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8:
|
||||
if maximum_distance_u8 is None or max(
|
||||
lower_distance, upper_distance
|
||||
) <= int(maximum_distance_u8):
|
||||
lower_rotation = rotations[lower_command]
|
||||
upper_rotation = rotations[upper_command]
|
||||
fraction = lower_distance / (upper_command - lower_command)
|
||||
@@ -847,7 +859,9 @@ def _interpolate_reference_rotation(
|
||||
return lower_rotation * Rotation.from_rotvec(delta * fraction)
|
||||
|
||||
nearest_command = min(rotations, key=lambda command: abs(command - zero))
|
||||
if abs(nearest_command - zero) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8:
|
||||
if maximum_distance_u8 is None or abs(
|
||||
nearest_command - zero
|
||||
) <= int(maximum_distance_u8):
|
||||
return rotations[nearest_command]
|
||||
return None
|
||||
|
||||
@@ -873,7 +887,9 @@ def _near_zero_records(
|
||||
|
||||
|
||||
def _baseline_reference(
|
||||
records: Sequence[Mapping[str, Any]], zero_command_u8: int
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
zero_command_u8: int,
|
||||
maximum_distance_u8: int | None = ZERO_REFERENCE_MAXIMUM_DISTANCE_U8,
|
||||
) -> tuple[float, float, float, float]:
|
||||
groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {}
|
||||
for record in records:
|
||||
@@ -883,17 +899,20 @@ def _baseline_reference(
|
||||
for group in groups.values()
|
||||
if (
|
||||
rotation := _interpolate_reference_rotation(
|
||||
group, zero_command_u8
|
||||
group,
|
||||
zero_command_u8,
|
||||
maximum_distance_u8,
|
||||
)
|
||||
)
|
||||
is not None
|
||||
]
|
||||
if not values:
|
||||
raise ValueError(
|
||||
"joint records have no samples within "
|
||||
f"{ZERO_REFERENCE_MAXIMUM_DISTANCE_U8} commands of zero "
|
||||
f"{zero_command_u8}"
|
||||
distance = (
|
||||
"the observed physical stroke"
|
||||
if maximum_distance_u8 is None
|
||||
else f"{int(maximum_distance_u8)} commands of zero {zero_command_u8}"
|
||||
)
|
||||
raise ValueError(f"joint records have no samples within {distance}")
|
||||
return robust_rotation_summary(values)[0]
|
||||
|
||||
|
||||
@@ -957,12 +976,23 @@ def fit_rotation_joint_curve(
|
||||
*,
|
||||
zero_command_u8: int,
|
||||
canonical_zero_direction: str | None = None,
|
||||
require_observed_domain_endpoints: bool = True,
|
||||
zero_reference_maximum_distance_u8: int | None = (
|
||||
ZERO_REFERENCE_MAXIMUM_DISTANCE_U8
|
||||
),
|
||||
) -> JointCurveFit:
|
||||
"""Fit a direction-aware curve from parent-to-child Tag orientations.
|
||||
|
||||
With ``canonical_zero_direction`` both branches share one physical
|
||||
reference. Only the canonical branch is zero at ``zero_command_u8``;
|
||||
the other branch retains its measured backlash/compliance offset.
|
||||
|
||||
Byte-feedback products keep the default requirement that both exact
|
||||
0/255 endpoints were observed. Physical-angle products may set
|
||||
``require_observed_domain_endpoints=False`` after their acquisition policy
|
||||
has independently proved feedback travel and coverage; the dense internal
|
||||
curve then uses bounded edge extrapolation instead of inventing endpoint
|
||||
feedback samples.
|
||||
"""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
@@ -981,6 +1011,7 @@ def fit_rotation_joint_curve(
|
||||
cycle_records, canonical_zero_direction
|
||||
),
|
||||
zero_command_u8,
|
||||
zero_reference_maximum_distance_u8,
|
||||
)
|
||||
)
|
||||
for record in samples:
|
||||
@@ -996,6 +1027,7 @@ def fit_rotation_joint_curve(
|
||||
samples,
|
||||
values_by_record,
|
||||
preserve_direction_offset=canonical_zero_direction is not None,
|
||||
require_observed_domain_endpoints=require_observed_domain_endpoints,
|
||||
)
|
||||
if canonical_zero_direction is None:
|
||||
for key in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
@@ -1031,6 +1063,7 @@ def fit_rotation_joint_curve(
|
||||
samples, canonical_zero_direction
|
||||
),
|
||||
zero_command_u8,
|
||||
zero_reference_maximum_distance_u8,
|
||||
)
|
||||
],
|
||||
"canonical_zero_direction": canonical_zero_direction,
|
||||
@@ -1105,6 +1138,9 @@ def rotation_curve_holdout_errors(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
zero_command_u8: int,
|
||||
zero_reference_maximum_distance_u8: int | None = (
|
||||
ZERO_REFERENCE_MAXIMUM_DISTANCE_U8
|
||||
),
|
||||
) -> tuple[float, ...]:
|
||||
"""Validate a fitted curve on an untouched scan cycle."""
|
||||
samples = [dict(record) for record in records]
|
||||
@@ -1115,6 +1151,7 @@ def rotation_curve_holdout_errors(
|
||||
_baseline_reference(
|
||||
_canonical_reference_records(samples, canonical_zero_direction),
|
||||
zero_command_u8,
|
||||
zero_reference_maximum_distance_u8,
|
||||
)
|
||||
)
|
||||
axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis")
|
||||
@@ -1190,6 +1227,12 @@ class JointAxisMeasurement:
|
||||
circle_axis_observability: float = 0.0
|
||||
axis_point_source: str = "circle_center"
|
||||
pose_axis_line_rms_m: float = 0.0
|
||||
# Unprojected residuals are evidence, not extra phase observations. The
|
||||
# axial component lies in the null space of (I-R) for a revolute axis.
|
||||
pose_axis_line_raw_rms_m: float | None = None
|
||||
pose_axis_line_axial_rms_m: float | None = None
|
||||
pose_axis_line_transverse_rms_m: float | None = None
|
||||
axis_point_axial_component_separated: bool = False
|
||||
# Measurement record(s) that supplied pose_axis_line_rms_m. A combined
|
||||
# cross-view axis may keep the front direction but take its physical line
|
||||
# point and line-quality residual from the side alias. Retry logic must
|
||||
@@ -1929,6 +1972,7 @@ def _fit_axis_point_from_pose_trajectory(
|
||||
view_normal_common_xyz: Sequence[float] | None,
|
||||
canonical_zero_direction: str | None = None,
|
||||
allow_axial_translation: bool = False,
|
||||
residual_diagnostics: dict[str, float] | None = None,
|
||||
) -> tuple[np.ndarray, float, str]:
|
||||
"""Fit the closest point on a revolute axis from full relative poses.
|
||||
|
||||
@@ -2010,6 +2054,8 @@ def _fit_axis_point_from_pose_trajectory(
|
||||
|
||||
matrices: list[np.ndarray] = []
|
||||
translations: list[np.ndarray] = []
|
||||
raw_matrices: list[np.ndarray] = []
|
||||
raw_translations: list[np.ndarray] = []
|
||||
used_image_plane_projection = False
|
||||
raw_angles: list[float] = []
|
||||
translation_angles: list[float] = []
|
||||
@@ -2100,6 +2146,8 @@ def _fit_axis_point_from_pose_trajectory(
|
||||
)
|
||||
projection = axis_projection
|
||||
axis_point_matrix = (np.eye(3) - delta_rotation) @ basis
|
||||
raw_matrices.append(axis_point_matrix)
|
||||
raw_translations.append(delta_translation)
|
||||
if use_image_plane_projection:
|
||||
assert view_normal_common is not None
|
||||
parent_rotation = Rotation.from_matrix(
|
||||
@@ -2151,6 +2199,17 @@ def _fit_axis_point_from_pose_trajectory(
|
||||
)
|
||||
residual = matrix @ solution.x - translation
|
||||
rms = float(np.sqrt(np.mean(np.square(residual))))
|
||||
if residual_diagnostics is not None:
|
||||
raw_residual = (
|
||||
np.asarray(raw_matrices) @ solution.x - np.asarray(raw_translations)
|
||||
)
|
||||
axial = raw_residual @ axis
|
||||
transverse = raw_residual - axial[:, None] * axis
|
||||
residual_diagnostics.update(
|
||||
raw_rms_m=float(np.sqrt(np.mean(raw_residual ** 2))),
|
||||
axial_rms_m=float(np.sqrt(np.mean(axial ** 2))),
|
||||
transverse_rms_m=float(np.sqrt(np.mean(transverse ** 2))),
|
||||
)
|
||||
source = (
|
||||
"pose_trajectory_image_plane"
|
||||
if used_image_plane_projection
|
||||
@@ -2169,6 +2228,7 @@ def fit_joint_axis_measurement(
|
||||
constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS,
|
||||
view_normal_common_xyz: Sequence[float] | None = None,
|
||||
canonical_zero_direction: str | None = None,
|
||||
separate_axial_residual: bool = False,
|
||||
) -> JointAxisMeasurement:
|
||||
"""Fit one physical screw axis from one complete scan cycle."""
|
||||
samples = [
|
||||
@@ -2267,6 +2327,8 @@ def fit_joint_axis_measurement(
|
||||
)
|
||||
axis_direction_source = "rotation_circle_fusion"
|
||||
axis_common = parent_rotation.apply(fitted_axis)
|
||||
residual_diagnostics: dict[str, float] = {}
|
||||
separate_axial = separate_axial_residual or str(joint).endswith("_side")
|
||||
point_parent, pose_axis_line_rms_m, axis_point_source = (
|
||||
_fit_axis_point_from_pose_trajectory(
|
||||
samples,
|
||||
@@ -2276,7 +2338,8 @@ def fit_joint_axis_measurement(
|
||||
phase_reference_point_parent_xyz=circle["center_xyz_m"],
|
||||
view_normal_common_xyz=view_normal_common_xyz,
|
||||
canonical_zero_direction=canonical_zero_direction,
|
||||
allow_axial_translation=str(joint).endswith("_side"),
|
||||
allow_axial_translation=separate_axial,
|
||||
residual_diagnostics=residual_diagnostics,
|
||||
)
|
||||
)
|
||||
point_common = parent_rotation.apply(point_parent) + parent_translation
|
||||
@@ -2307,6 +2370,10 @@ def fit_joint_axis_measurement(
|
||||
circle_axis_observability=float(circle_axis_observability),
|
||||
axis_point_source=axis_point_source,
|
||||
pose_axis_line_rms_m=pose_axis_line_rms_m,
|
||||
pose_axis_line_raw_rms_m=residual_diagnostics["raw_rms_m"],
|
||||
pose_axis_line_axial_rms_m=residual_diagnostics["axial_rms_m"],
|
||||
pose_axis_line_transverse_rms_m=residual_diagnostics["transverse_rms_m"],
|
||||
axis_point_axial_component_separated=separate_axial,
|
||||
pose_axis_line_source_joints=(str(joint),),
|
||||
)
|
||||
|
||||
@@ -2493,6 +2560,9 @@ class ZeroSolveResult:
|
||||
axis_cone_mismatch_by_joint_rad: Mapping[str, float]
|
||||
axis_cone_bias_classification_by_joint: Mapping[str, str]
|
||||
failure_reasons: Mapping[str, str]
|
||||
# Optional diagnostic evidence from an explicitly selected observation
|
||||
# policy; does not change the generic solver's acceptance decision.
|
||||
axis_residual_diagnostics: Mapping[str, Mapping[str, Any]] | None = None
|
||||
|
||||
|
||||
def merge_right_19_thumb_zero_result(
|
||||
@@ -3370,15 +3440,28 @@ def solve_urdf_zero_offsets(
|
||||
if abs(float(parent_axis @ view_normal)) >= math.cos(
|
||||
math.radians(45.0)
|
||||
):
|
||||
# Orthographic image-plane displacement is invariant to
|
||||
# independent optical-depth bias on the two planar Tags.
|
||||
# With an end-on axis, along-axis placement also projects
|
||||
# to zero (or a small component for a mildly oblique view).
|
||||
predicted_radial = predicted_delta - view_normal * float(
|
||||
predicted_delta @ view_normal
|
||||
# Use an image-plane phase for an end-on observation.
|
||||
# Projection alone removes optical depth, but does NOT
|
||||
# remove an arbitrary along-axis point coordinate when
|
||||
# the camera is even slightly oblique to the axis.
|
||||
# A closest point is defined relative to its parent Tag
|
||||
# origin, not the physical bearing centre. Different Tag
|
||||
# mounts therefore choose different along-axis gauges.
|
||||
# Reusing delta here would reintroduce that arbitrary
|
||||
# coordinate and turn it into a phase at oblique views.
|
||||
predicted_image_input = (
|
||||
predicted_radial if profile.project_axis_gauge_before_image
|
||||
else predicted_delta
|
||||
)
|
||||
observed_radial = observed_delta - view_normal * float(
|
||||
observed_delta @ view_normal
|
||||
observed_image_input = (
|
||||
observed_radial if profile.project_axis_gauge_before_image
|
||||
else observed_delta
|
||||
)
|
||||
predicted_radial = predicted_image_input - view_normal * float(
|
||||
predicted_image_input @ view_normal
|
||||
)
|
||||
observed_radial = observed_image_input - view_normal * float(
|
||||
observed_image_input @ view_normal
|
||||
)
|
||||
angle_axis = view_normal
|
||||
if float(angle_axis @ parent_axis) < 0.0:
|
||||
@@ -4008,7 +4091,13 @@ def solve_urdf_zero_offsets(
|
||||
<= max(significance_sigma * uncertainty, confidence_half_width)
|
||||
):
|
||||
applied_training[name] = 0.0
|
||||
if abs(value) >= minimum_applied_offset_rad:
|
||||
validated_zero_candidate = bool(
|
||||
profile.accept_validated_zero_in_confidence_interval
|
||||
and maximum_confidence_half_width_rad is not None
|
||||
and abs(value) <= confidence_half_width
|
||||
and confidence_half_width <= maximum_confidence_half_width_rad
|
||||
)
|
||||
if abs(value) >= minimum_applied_offset_rad and not validated_zero_candidate:
|
||||
insignificant_large.append(name)
|
||||
else:
|
||||
applied_training[name] = value
|
||||
|
||||
@@ -393,8 +393,43 @@ def fit_coupling_model(
|
||||
maximum_residual_p95_rad: float = math.radians(2.0),
|
||||
maximum_residual_rad: float = math.radians(3.0),
|
||||
) -> MimicFit:
|
||||
if model not in {"linear_mimic", "quadratic_runtime"}:
|
||||
if model not in {
|
||||
"linear_mimic", "quadratic_runtime", "direction_aware_knots"
|
||||
}:
|
||||
raise ValueError(f"unsupported L6 coupling model: {model}")
|
||||
if model == "direction_aware_knots":
|
||||
# The passive joint already has independently fitted decreasing and
|
||||
# increasing curves over the same motor-feedback domain. Those knots
|
||||
# are the exact runtime coupling representation and preserve real
|
||||
# tendon backlash/nonlinearity that a single polynomial cannot model.
|
||||
# Standard URDF has no directional lookup, so its mimic element keeps
|
||||
# the endpoint-equivalent linear fallback only.
|
||||
source_travel = curve_travel_rad(active_fit)
|
||||
target_travel = curve_travel_rad(passive_fit)
|
||||
if abs(source_travel) <= 1.0e-9:
|
||||
raise ValueError("active mimic source has insufficient travel")
|
||||
multiplier = float(target_travel / source_travel)
|
||||
if not minimum_multiplier <= multiplier <= maximum_multiplier:
|
||||
raise ValueError(
|
||||
f"{target_joint} coupling endpoint ratio is outside "
|
||||
f"[{minimum_multiplier}, {maximum_multiplier}]"
|
||||
)
|
||||
return MimicFit(
|
||||
source_joint=source_joint,
|
||||
target_joint=target_joint,
|
||||
model=model,
|
||||
coefficients=(multiplier,),
|
||||
urdf_mimic_multiplier=multiplier,
|
||||
urdf_mimic_policy="endpoint_linear_fallback",
|
||||
cycle_coefficients=(),
|
||||
maximum_cycle_range=0.0,
|
||||
maximum_cycle_prediction_range_rad=0.0,
|
||||
# Runtime residual is assessed by the passive curve's isolated
|
||||
# holdout samples, not by an intentionally lossy URDF fallback.
|
||||
residual_rms_rad=0.0,
|
||||
residual_p95_rad=0.0,
|
||||
residual_max_rad=0.0,
|
||||
)
|
||||
degree = 1 if model == "linear_mimic" else 2
|
||||
active = np.concatenate(
|
||||
(
|
||||
@@ -556,8 +591,6 @@ def fit_l6_session(
|
||||
)
|
||||
if fit.maximum_monotonic_correction_rad > correction_limit:
|
||||
raise ValueError(f"{name} monotonic correction exceeds limit")
|
||||
if fit.maximum_hysteresis_rad > math.radians(2.0):
|
||||
raise ValueError(f"{name} hysteresis exceeds 2 degrees")
|
||||
curves[name] = fit
|
||||
holdout[name] = errors
|
||||
cycle_curves[name] = {
|
||||
|
||||
@@ -7,6 +7,7 @@ from dataclasses import dataclass
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
import threading
|
||||
import time
|
||||
import traceback
|
||||
from typing import Any
|
||||
@@ -14,6 +15,7 @@ from typing import Any
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from apriltag_msgs.msg import AprilTagDetectionArray
|
||||
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from scipy.spatial.transform import Rotation
|
||||
@@ -29,6 +31,8 @@ from ...extrinsics import (
|
||||
)
|
||||
from ...pnp import SquareTagPose, SquareTagPoseTracker
|
||||
from ...storage import append_jsonl, atomic_write_json
|
||||
from ...runtime import ACQUISITION_POLICY_VERSION, CalibrationEngine
|
||||
from ...runtime.adapters import ProfileSdkAdapter
|
||||
from .motion import cosine_position_trajectory_u8
|
||||
from .pipeline import finalize_l6_session
|
||||
from .profile import build_typed_profile
|
||||
@@ -58,6 +62,11 @@ def _stamp_ns(stamp: Any) -> int:
|
||||
class L6ThreeCameraCalibrationNode(Node):
|
||||
"""Own one reviewed six-channel partial-calibration session."""
|
||||
|
||||
@staticmethod
|
||||
def _uses_isolated_motion_callbacks() -> bool:
|
||||
"""Legacy byte profiles retain their original callback behavior."""
|
||||
return False
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
@@ -73,10 +82,47 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
self.baseline_command = tuple(self.profile.command.baseline_values)
|
||||
self.command_lower = tuple(self.profile.command.minimum_values)
|
||||
self.command_upper = tuple(self.profile.command.maximum_values)
|
||||
self.feedback_lower = tuple(
|
||||
self.profile.command.minimum_feedback_values
|
||||
)
|
||||
self.feedback_upper = tuple(
|
||||
self.profile.command.maximum_feedback_values
|
||||
)
|
||||
self.sample_kind = str(sample_kind)
|
||||
self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality"
|
||||
self.finalize_session = finalizer or finalize_l6_session
|
||||
self.calibration_engine = CalibrationEngine(self.profile)
|
||||
self.sdk_adapter = ProfileSdkAdapter(self.profile.command)
|
||||
super().__init__(f"{self.model_name.lower()}_calibration")
|
||||
isolated_motion = self._uses_isolated_motion_callbacks()
|
||||
self.motion_callback_group = (
|
||||
MutuallyExclusiveCallbackGroup() if isolated_motion else None
|
||||
)
|
||||
# O12 receives several independent AprilTag streams while a motion
|
||||
# timer keeps publishing the trajectory. Putting every camera in one
|
||||
# mutually-exclusive group lets a high-rate view monopolize the group:
|
||||
# the primary view can then retain hundreds of frames while the
|
||||
# required cross view sees only a few dozen. Keep callbacks serialized
|
||||
# *within* one camera (CameraInfo and detections share a group), but let
|
||||
# independent camera streams run concurrently. Legacy L6/O6 profiles
|
||||
# do not opt into isolated callbacks and therefore retain their exact
|
||||
# executor behaviour.
|
||||
self.vision_callback_groups = (
|
||||
{
|
||||
view: MutuallyExclusiveCallbackGroup()
|
||||
for view in self.profile.vision.view_names
|
||||
}
|
||||
if isolated_motion else {}
|
||||
)
|
||||
# Retain the singular attribute for diagnostics and downstream code;
|
||||
# subscriptions use the per-view mapping below.
|
||||
self.vision_callback_group = next(
|
||||
iter(self.vision_callback_groups.values()), None
|
||||
)
|
||||
# Only O12 opts into parallel motion/vision callbacks. L6/O6 keep
|
||||
# their established executor behavior and never use this barrier.
|
||||
self.step_data_lock = threading.RLock() if isolated_motion else None
|
||||
self.vision_callbacks_inflight = 0
|
||||
self._declare_parameters()
|
||||
self._load_parameters()
|
||||
self.session_dir.mkdir(parents=True, exist_ok=True)
|
||||
@@ -89,6 +135,11 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
"profile_id": self.profile.key.profile_id,
|
||||
"serial_number": self.serial_number,
|
||||
"curve_input_domain": f"feedback_{self.command_unit}",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
**self.protected_inputs,
|
||||
"resume_checkpoint_requested": (
|
||||
self.resume_raw_samples_path is not None
|
||||
),
|
||||
},
|
||||
)
|
||||
|
||||
@@ -102,7 +153,8 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
String, f"{self.profile.namespace}/status", 10
|
||||
)
|
||||
self.create_subscription(
|
||||
JointState, self.state_topic, self._state_callback, 30
|
||||
JointState, self.state_topic, self._state_callback, 30,
|
||||
callback_group=self.motion_callback_group,
|
||||
)
|
||||
self.camera_matrices: dict[str, np.ndarray] = {}
|
||||
self.image_sizes: dict[str, tuple[int, int]] = {}
|
||||
@@ -132,21 +184,34 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
selected, message
|
||||
),
|
||||
qos_profile_sensor_data,
|
||||
callback_group=self.vision_callback_groups.get(view),
|
||||
)
|
||||
self.create_subscription(
|
||||
AprilTagDetectionArray,
|
||||
self.detection_topics[view],
|
||||
lambda message, selected=view: self._detections_callback(
|
||||
selected, message
|
||||
(
|
||||
lambda message, selected=view:
|
||||
self._guarded_detections_callback(selected, message)
|
||||
) if isolated_motion else (
|
||||
lambda message, selected=view:
|
||||
self._detections_callback(selected, message)
|
||||
),
|
||||
qos_profile_sensor_data,
|
||||
callback_group=self.vision_callback_groups.get(view),
|
||||
)
|
||||
self.create_service(Trigger, f"{self.profile.namespace}/start", self._start)
|
||||
self.create_service(Trigger, f"{self.profile.namespace}/abort", self._abort)
|
||||
self.create_service(
|
||||
Trigger, f"{self.profile.namespace}/start", self._start,
|
||||
callback_group=self.motion_callback_group,
|
||||
)
|
||||
self.create_service(
|
||||
Trigger, f"{self.profile.namespace}/abort", self._abort,
|
||||
callback_group=self.motion_callback_group,
|
||||
)
|
||||
|
||||
self.latest_state_u8: tuple[float, ...] = ()
|
||||
self.state_history: deque[StateSample] = deque(maxlen=2000)
|
||||
self.state_receive_times: deque[float] = deque(maxlen=300)
|
||||
self.command_publish_times: deque[float] = deque(maxlen=500)
|
||||
self.raw_records: list[dict[str, Any]] = []
|
||||
self.steps: list[MotionStep] = []
|
||||
self.step_index = -1
|
||||
@@ -161,8 +226,11 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
self.step_speed_ready_at = 0.0
|
||||
self.step_requested_u8 = float("nan")
|
||||
self.step_start_state_u8: tuple[float, ...] = ()
|
||||
self.step_last_command_u8: tuple[int, ...] | None = None
|
||||
self.step_start_feedback_u8: tuple[float, ...] = ()
|
||||
self.last_published_command_u8: tuple[float, ...] | None = None
|
||||
self.step_last_command_u8: tuple[float, ...] | None = None
|
||||
self.step_trajectory_phase = 0.0
|
||||
self.step_trajectory_blend = 0.0
|
||||
self.step_trajectory_duration_seconds = 0.0
|
||||
self.step_moving_indices: frozenset[int] = frozenset()
|
||||
self.step_valid_frames = 0
|
||||
@@ -185,6 +253,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
1.0 / float(self.command_rate_hz)
|
||||
if self.command_unit == "rad" else 0.01,
|
||||
self._tick,
|
||||
callback_group=self.motion_callback_group,
|
||||
)
|
||||
self.create_timer(0.5, self._publish_status)
|
||||
|
||||
@@ -202,6 +271,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
"calibration_config_expected_sha256": "",
|
||||
"tag_config_expected_sha256": "",
|
||||
"sdk_config_expected_sha256": "",
|
||||
"resume_raw_samples_path": "",
|
||||
"command_topic": f"/{model}/cb_right_hand_control_cmd",
|
||||
"state_topic": f"/{model}/cb_right_hand_state",
|
||||
"setting_topic": f"/{model}/cb_hand_setting_cmd",
|
||||
@@ -231,7 +301,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
"motor_stall_timeout_seconds": 2.0,
|
||||
"position_timeout_seconds": 30.0,
|
||||
"sweep_timeout_seconds": 90.0,
|
||||
"automatic_sweep_retry_limit": 2,
|
||||
"automatic_sweep_retry_limit": 1,
|
||||
"non_target_motion_tolerance_u8": 3.0,
|
||||
"minimum_sweep_frames": 40,
|
||||
"minimum_state_span_u8": 240.0,
|
||||
@@ -247,8 +317,8 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
"maximum_hamming": 0,
|
||||
"minimum_decision_margin": 30.0,
|
||||
"minimum_edge_pixels": 30.0,
|
||||
"fixed_base_maximum_corner_drift_px": 2.0,
|
||||
"fixed_base_movement_confirmation_frames": 5,
|
||||
"fixed_base_maximum_corner_drift_px": 5.0,
|
||||
"fixed_base_movement_confirmation_frames": 10,
|
||||
"tag_size_m": 0.016,
|
||||
"pnp_maximum_reprojection_error_px": 1.5,
|
||||
"pnp_reprojection_tie_px": 1.5,
|
||||
@@ -299,6 +369,12 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
self.protected_inputs["sdk_config_sha256"] = str(
|
||||
value("sdk_config_expected_sha256")
|
||||
)
|
||||
resume_value = str(value("resume_raw_samples_path")).strip()
|
||||
self.resume_raw_samples_path = (
|
||||
None
|
||||
if not resume_value
|
||||
else Path(resume_value).expanduser().resolve()
|
||||
)
|
||||
if (
|
||||
set(self.protected_inputs)
|
||||
!= self.profile.artifacts.protected_input_fields
|
||||
@@ -371,31 +447,17 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
raise ValueError("minimum_joint_frame_rate must be in (0, 1]")
|
||||
|
||||
def _build_steps(self) -> list[MotionStep]:
|
||||
command_unit = getattr(self, "command_unit", "u8")
|
||||
steps = [
|
||||
MotionStep(
|
||||
"baseline", None, None, 255, int(self.baseline_speed_u8)
|
||||
)
|
||||
]
|
||||
for task in self.profile.motion.tasks:
|
||||
midpoint = 0.5 * (task.start_value + task.end_value)
|
||||
preflight_speed = (
|
||||
task.preflight_speed
|
||||
if command_unit == "rad"
|
||||
else task.preflight_speed_u8 or self.preflight_speed_u8
|
||||
)
|
||||
formal_speed = (
|
||||
task.formal_speed
|
||||
if command_unit == "rad"
|
||||
if getattr(self, "command_unit", self.profile.command.unit) == "rad"
|
||||
else self.formal_speed_u8
|
||||
)
|
||||
for target in (task.start_value, midpoint, task.end_value, task.start_value):
|
||||
steps.append(
|
||||
MotionStep(
|
||||
"preflight", task.key, task.command_index, target,
|
||||
float(preflight_speed),
|
||||
)
|
||||
)
|
||||
for cycle in (0, 1, 2, 3):
|
||||
steps.extend(
|
||||
[
|
||||
@@ -428,8 +490,9 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
|
||||
def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
|
||||
hold = (
|
||||
list(self.latest_state_u8)
|
||||
if self.command_unit == "rad" and len(self.latest_state_u8) == self.command_count
|
||||
list(self.last_published_command_u8)
|
||||
if self.command_unit == "rad"
|
||||
and self.last_published_command_u8 is not None
|
||||
else list(self.baseline_command)
|
||||
)
|
||||
self._publish_command(hold)
|
||||
@@ -444,32 +507,60 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
return response
|
||||
|
||||
def _camera_info_callback(self, view: str, message: CameraInfo) -> None:
|
||||
matrix = np.asarray(message.k, dtype=float).reshape(3, 3)
|
||||
if np.all(np.isfinite(matrix)) and matrix[0, 0] > 0 and matrix[1, 1] > 0:
|
||||
self.camera_matrices[view] = matrix
|
||||
self.image_sizes[view] = (int(message.width), int(message.height))
|
||||
from ...core.geometry.camera import rectified_camera_matrix
|
||||
try:
|
||||
matrix = rectified_camera_matrix(message.p)
|
||||
if message.width <= 0 or message.height <= 0:
|
||||
raise ValueError('invalid rectified image dimensions')
|
||||
except ValueError:
|
||||
self.camera_matrices.pop(view, None)
|
||||
self.image_sizes.pop(view, None)
|
||||
return
|
||||
previous = self.camera_matrices.get(view)
|
||||
self.camera_matrices[view] = matrix
|
||||
self.image_sizes[view] = (int(message.width), int(message.height))
|
||||
model = {
|
||||
'kind': 'rectified_camera_model', 'view': view,
|
||||
'matrix_source': 'CameraInfo.P[:3,:3]',
|
||||
'width': int(message.width), 'height': int(message.height),
|
||||
'raw_k': list(message.k), 'raw_d': list(message.d),
|
||||
'rectification_r': list(message.r), 'projection_p': list(message.p),
|
||||
'camera_matrix': matrix.tolist(), 'input_is_rectified': True,
|
||||
}
|
||||
if not hasattr(self, 'camera_models'):
|
||||
self.camera_models = {}
|
||||
if self.camera_models.get(view) != model:
|
||||
self.camera_models[view] = model
|
||||
if hasattr(self, 'raw_path'):
|
||||
append_jsonl(self.raw_path, model)
|
||||
if previous is not None and not np.allclose(previous, matrix):
|
||||
self.trackers[view].reset()
|
||||
group = getattr(self, '_thumb_pose_group', None)
|
||||
if view == 'front' and group is not None:
|
||||
group.reset()
|
||||
|
||||
def _state_callback(self, message: JointState) -> None:
|
||||
if len(message.position) != self.command_count:
|
||||
adapter = getattr(self, "sdk_adapter", ProfileSdkAdapter(self.profile.command))
|
||||
state = adapter.parse_feedback(message.name, message.position)
|
||||
if state is None:
|
||||
return
|
||||
if message.name and not self.profile.command.feedback_by_index:
|
||||
by_name = dict(zip((str(name) for name in message.name), message.position))
|
||||
for alias, canonical in self.profile.command.feedback_name_aliases.items():
|
||||
if alias in by_name and canonical not in by_name:
|
||||
by_name[canonical] = by_name[alias]
|
||||
if any(name not in by_name for name in self.command_names):
|
||||
return
|
||||
state = tuple(float(by_name[name]) for name in self.command_names)
|
||||
else:
|
||||
state = tuple(float(value) for value in message.position)
|
||||
if not all(math.isfinite(value) for value in state):
|
||||
return
|
||||
if any(
|
||||
value < self.command_lower[index] - 1.0e-6
|
||||
or value > self.command_upper[index] + 1.0e-6
|
||||
violation = next((
|
||||
(index, value)
|
||||
for index, value in enumerate(state)
|
||||
):
|
||||
self._pause("feedback_outside_registered_command_domain")
|
||||
if value < self.feedback_lower[index]
|
||||
or value > self.feedback_upper[index]
|
||||
), None)
|
||||
if violation is not None:
|
||||
index, value = violation
|
||||
# Preserve the offending observation for the operator diagnostic,
|
||||
# while the pause path continues to hold the last safe command.
|
||||
self.latest_state_u8 = state
|
||||
self._pause(
|
||||
"feedback_outside_registered_feedback_domain:"
|
||||
f"channel={self.command_names[index]}:value={value:.9f}:"
|
||||
f"lower={self.feedback_lower[index]:.9f}:"
|
||||
f"upper={self.feedback_upper[index]:.9f}"
|
||||
)
|
||||
return
|
||||
stamp = _stamp_ns(message.header.stamp)
|
||||
if stamp <= 0:
|
||||
@@ -478,22 +569,9 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
self.state_history.append(StateSample(stamp, state))
|
||||
self.latest_state_u8 = state
|
||||
self.state_receive_times.append(time.monotonic())
|
||||
step = self._current_step()
|
||||
if step is not None and step.task_key is not None:
|
||||
for index, actual in enumerate(state):
|
||||
if index == step.command_index or index in self.step_moving_indices:
|
||||
continue
|
||||
expected = (
|
||||
self.step_last_command_u8[index]
|
||||
if self.step_last_command_u8 is not None
|
||||
else self.baseline_command[index]
|
||||
)
|
||||
tolerance = float(self.non_target_motion_tolerance_u8)
|
||||
if abs(actual - expected) > tolerance:
|
||||
self._pause(
|
||||
f"non_target_motor_moved:channel={index}:feedback={actual:.2f}"
|
||||
)
|
||||
return
|
||||
# Non-target motion is retained in every sample for diagnostics and
|
||||
# coupling fitting. It is not a runtime stop condition: tendon hands
|
||||
# legitimately back-drive neighbouring SDK coordinates.
|
||||
|
||||
def _task(self, key: str):
|
||||
return next(task for task in self.profile.motion.tasks if task.key == key)
|
||||
@@ -620,6 +698,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
step = self._current_step()
|
||||
recording_this_view = bool(
|
||||
step is not None
|
||||
and self.step_command_sent
|
||||
and step.recording
|
||||
and self._task(str(step.task_key)).view == view
|
||||
)
|
||||
@@ -715,11 +794,48 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
f"fixed_base_tag_moved:{view}:drift_px={drift:.3f}"
|
||||
)
|
||||
return
|
||||
if not all(role in corners and good.get(role, False) for role in roles):
|
||||
# A model may freeze a fixed palm reference before a collision-
|
||||
# clearance pose intentionally occludes that Tag. The default is an
|
||||
# empty mapping, so legacy L6/O6 behaviour is byte-for-byte unchanged;
|
||||
# O12 supplies only its front palm pose for the affected finger tasks.
|
||||
locked_reference_hook = getattr(
|
||||
self, "_locked_reference_poses_for_capture", None
|
||||
)
|
||||
locked_reference_poses = (
|
||||
dict(locked_reference_hook(view, step) or {})
|
||||
if locked_reference_hook is not None
|
||||
else {}
|
||||
)
|
||||
locked_reference_poses = {
|
||||
str(role): pose
|
||||
for role, pose in locked_reference_poses.items()
|
||||
if str(role) in roles
|
||||
}
|
||||
live_roles = tuple(
|
||||
role for role in roles if role not in locked_reference_poses
|
||||
)
|
||||
if not all(
|
||||
role in corners and good.get(role, False)
|
||||
for role in live_roles
|
||||
):
|
||||
return
|
||||
stamp = _stamp_ns(message.header.stamp)
|
||||
selected: dict[str, SquareTagPose] = {}
|
||||
for role in roles:
|
||||
selected: dict[str, SquareTagPose] = dict(locked_reference_poses)
|
||||
# Optional model-specific articulated branch selector. Legacy models
|
||||
# keep the exact independent selection below when the hook is absent.
|
||||
pose_hook = getattr(self, "_select_articulated_capture_poses", None)
|
||||
joint_selection = (
|
||||
pose_hook(view, step, live_roles, corners, stamp)
|
||||
if pose_hook is not None else None
|
||||
)
|
||||
if joint_selection is not None:
|
||||
poses, reason = joint_selection
|
||||
if poses is None:
|
||||
if recording_this_view:
|
||||
self._count_step_rejection(f"pnp:group:{reason}")
|
||||
return
|
||||
selected.update(poses)
|
||||
for role in (() if joint_selection is not None else live_roles):
|
||||
pose, pnp_reason = self.trackers[view].estimate(
|
||||
role,
|
||||
corners[role],
|
||||
@@ -734,6 +850,10 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
return
|
||||
selected[role] = pose
|
||||
evidence_hook = getattr(self, "_record_capture_pose_evidence", None)
|
||||
if evidence_hook is not None:
|
||||
evidence_hook(view, step, roles, corners, selected, stamp,
|
||||
locked_roles=tuple(locked_reference_poses))
|
||||
if recording_this_view:
|
||||
self.step_pnp_valid_frames += 1
|
||||
self.last_view_valid_at[view] = time.monotonic()
|
||||
@@ -754,12 +874,16 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
self.step_state_sync_frames += 1
|
||||
task = self._task(step.task_key)
|
||||
feedback = float(state_u8[task.command_index])
|
||||
lower = self.command_lower[task.command_index]
|
||||
upper = self.command_upper[task.command_index]
|
||||
lower = self.feedback_lower[task.command_index]
|
||||
upper = self.feedback_upper[task.command_index]
|
||||
if not lower <= feedback <= upper:
|
||||
self._count_step_rejection("feedback:outside_registered_domain")
|
||||
self._count_step_rejection("feedback:outside_registered_feedback_domain")
|
||||
return
|
||||
progress = self.profile.command.normalize(task.command_index, feedback)
|
||||
progress = float(np.clip(
|
||||
self.profile.command.normalize(task.command_index, feedback),
|
||||
0.0,
|
||||
1.0,
|
||||
))
|
||||
for joint in task.joints:
|
||||
measurement = self.profile.measurement.measurements[joint]
|
||||
parent = selected[str(measurement.parent_role)]
|
||||
@@ -840,14 +964,34 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
})
|
||||
self.raw_records.append(record)
|
||||
append_jsonl(self.raw_path, record)
|
||||
# This is a frame counter, not a joint-record counter. A frame can
|
||||
# emit active and passive joint records from the same synchronized
|
||||
# observation and must still contribute exactly once to the rate.
|
||||
self.step_valid_frames += 1
|
||||
|
||||
def _guarded_detections_callback(
|
||||
self, view: str, message: AprilTagDetectionArray
|
||||
) -> None:
|
||||
with self.step_data_lock:
|
||||
self.vision_callbacks_inflight += 1
|
||||
try:
|
||||
self._detections_callback(view, message)
|
||||
finally:
|
||||
with self.step_data_lock:
|
||||
self.vision_callbacks_inflight -= 1
|
||||
|
||||
def _feedback_hz(self) -> float:
|
||||
if len(self.state_receive_times) < 2:
|
||||
return 0.0
|
||||
elapsed = self.state_receive_times[-1] - self.state_receive_times[0]
|
||||
return 0.0 if elapsed <= 0 else (len(self.state_receive_times) - 1) / elapsed
|
||||
|
||||
def _command_hz(self) -> float:
|
||||
if len(self.command_publish_times) < 2:
|
||||
return 0.0
|
||||
elapsed = self.command_publish_times[-1] - self.command_publish_times[0]
|
||||
return 0.0 if elapsed <= 0 else (len(self.command_publish_times) - 1) / elapsed
|
||||
|
||||
def _publish_torque(self) -> None:
|
||||
if not self.commands_enabled or self.command_unit != "u8":
|
||||
return
|
||||
@@ -892,6 +1036,8 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
message.position = bounded
|
||||
self.command_publisher.publish(message)
|
||||
self.last_published_command_u8 = tuple(bounded)
|
||||
self.command_publish_times.append(time.monotonic())
|
||||
|
||||
def _target_command(self, step: MotionStep) -> tuple[float, ...]:
|
||||
if step.target_command is not None:
|
||||
@@ -907,6 +1053,15 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
return tuple(target)
|
||||
|
||||
def _begin_step(self, step: MotionStep) -> None:
|
||||
if not self._uses_isolated_motion_callbacks():
|
||||
self._begin_step_without_vision_callback(step)
|
||||
return
|
||||
assert self.step_data_lock is not None
|
||||
with self.step_data_lock:
|
||||
if not self.vision_callbacks_inflight:
|
||||
self._begin_step_without_vision_callback(step)
|
||||
|
||||
def _begin_step_without_vision_callback(self, step: MotionStep) -> None:
|
||||
now = time.monotonic()
|
||||
if self.commanded_speed != step.speed_u8:
|
||||
self._publish_speed(step.speed_u8)
|
||||
@@ -917,7 +1072,18 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
return
|
||||
if len(self.latest_state_u8) != self.command_count:
|
||||
return
|
||||
self.step_start_state_u8 = tuple(float(value) for value in self.latest_state_u8)
|
||||
self.step_start_feedback_u8 = tuple(
|
||||
float(value) for value in self.latest_state_u8
|
||||
)
|
||||
# A radian feedback value is an observation to calibrate, not the last
|
||||
# command that was sent. O12 can report a stable non-zero feedback at
|
||||
# command zero, so trajectories must remain entirely in command space.
|
||||
self.step_start_state_u8 = (
|
||||
tuple(self.last_published_command_u8)
|
||||
if self.command_unit == "rad"
|
||||
and self.last_published_command_u8 is not None
|
||||
else self.step_start_feedback_u8
|
||||
)
|
||||
self.step_started_at = now
|
||||
self.step_last_progress_at = now
|
||||
self.step_last_feedback = (
|
||||
@@ -940,17 +1106,21 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
index for index, error in enumerate(errors)
|
||||
if error > float(self.non_target_motion_tolerance_u8)
|
||||
)
|
||||
self.step_last_distance_u8 = (
|
||||
command_distance = (
|
||||
float(sum(errors))
|
||||
if step.command_index is None
|
||||
else float(errors[step.command_index])
|
||||
)
|
||||
self.step_initial_distance_u8 = self.step_last_distance_u8
|
||||
self.step_last_distance_u8 = (
|
||||
0.0 if self.command_unit == "rad" else command_distance
|
||||
)
|
||||
self.step_initial_distance_u8 = command_distance
|
||||
maximum_distance = max(errors)
|
||||
if self.command_unit == "rad":
|
||||
self.step_trajectory_duration_seconds = (
|
||||
0.0 if maximum_distance <= 0.0 else
|
||||
math.pi * maximum_distance / (2.0 * float(step.speed_u8))
|
||||
_, _, self.step_trajectory_duration_seconds = (
|
||||
self._radian_trajectory_fraction(
|
||||
maximum_distance, 0.0, float(step.speed_u8)
|
||||
)
|
||||
)
|
||||
else:
|
||||
_, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8(
|
||||
@@ -958,6 +1128,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
float(self.command_trajectory_full_range_seconds),
|
||||
)
|
||||
self.step_trajectory_phase = 0.0
|
||||
self.step_trajectory_blend = 0.0
|
||||
self.step_last_command_u8 = None
|
||||
self.step_hold_since = None
|
||||
roles: tuple[str, ...] = ()
|
||||
@@ -971,6 +1142,16 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
f"direction={step.direction}:target={step.target_u8}:attempt={step.attempt}"
|
||||
)
|
||||
|
||||
def _radian_trajectory_fraction(
|
||||
self, distance: float, elapsed: float, maximum_speed: float
|
||||
) -> tuple[float, float, float]:
|
||||
"""Return blend, phase and duration for the default cosine profile."""
|
||||
if distance <= 0.0:
|
||||
return 1.0, 1.0, 0.0
|
||||
duration = math.pi * float(distance) / (2.0 * float(maximum_speed))
|
||||
phase = min(1.0, max(0.0, float(elapsed) / duration))
|
||||
return 0.5 - 0.5 * math.cos(math.pi * phase), phase, duration
|
||||
|
||||
def _advance_step_trajectory(self, step: MotionStep, now: float) -> None:
|
||||
if hasattr(self, "_target_command"):
|
||||
target_values = self._target_command(step)
|
||||
@@ -981,15 +1162,24 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
target[step.command_index] = float(step.target_u8)
|
||||
target_values = tuple(target)
|
||||
elapsed = max(0.0, float(now) - self.step_started_at)
|
||||
duration = float(
|
||||
getattr(
|
||||
self,
|
||||
"step_trajectory_duration_seconds",
|
||||
self.command_trajectory_full_range_seconds,
|
||||
if getattr(self, "command_unit", "u8") == "rad":
|
||||
maximum_distance = max(
|
||||
abs(target - start)
|
||||
for start, target in zip(self.step_start_state_u8, target_values)
|
||||
)
|
||||
)
|
||||
phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration)
|
||||
blend = 0.5 - 0.5 * math.cos(math.pi * phase)
|
||||
blend, phase, duration = self._radian_trajectory_fraction(
|
||||
maximum_distance, elapsed, float(step.speed_u8)
|
||||
)
|
||||
else:
|
||||
duration = float(
|
||||
getattr(
|
||||
self,
|
||||
"step_trajectory_duration_seconds",
|
||||
self.command_trajectory_full_range_seconds,
|
||||
)
|
||||
)
|
||||
phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration)
|
||||
blend = 0.5 - 0.5 * math.cos(math.pi * phase)
|
||||
values = [
|
||||
start + (target - start) * blend
|
||||
for start, target in zip(self.step_start_state_u8, target_values)
|
||||
@@ -1001,12 +1191,21 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
for index, value in enumerate(values)
|
||||
)
|
||||
self.step_trajectory_phase = phase
|
||||
self.step_trajectory_blend = blend
|
||||
self.step_requested_u8 = (
|
||||
float(np.mean(values))
|
||||
if step.command_index is None
|
||||
else float(values[step.command_index])
|
||||
)
|
||||
if command != self.step_last_command_u8:
|
||||
# O12 joint feedback is request-driven: every radian JointState command
|
||||
# triggers both the control write and a fresh readback. Keep streaming
|
||||
# the unchanged endpoint during holds so the next step never starts
|
||||
# from a stale feedback sample. Legacy byte profiles retain their
|
||||
# existing duplicate suppression.
|
||||
if (
|
||||
getattr(self, "command_unit", "u8") == "rad"
|
||||
or command != self.step_last_command_u8
|
||||
):
|
||||
self._publish_command(list(command))
|
||||
self.step_last_command_u8 = command
|
||||
|
||||
@@ -1030,9 +1229,24 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
gap = max((right - left for left, right in zip(bins, bins[1:])), default=256)
|
||||
maximum_gap = float(self.maximum_bin_gap)
|
||||
else:
|
||||
normalized_bin_count = int(
|
||||
getattr(self, "normalized_sweep_bin_count", 32)
|
||||
)
|
||||
if normalized_bin_count < 32:
|
||||
raise ValueError("normalized sweep bin count must be at least 32")
|
||||
normalized = sorted(
|
||||
set(
|
||||
min(31, max(0, int(self.profile.command.normalize(task.command_index, value) * 32.0)))
|
||||
min(
|
||||
normalized_bin_count - 1,
|
||||
max(
|
||||
0,
|
||||
int(
|
||||
self.profile.command.normalize(
|
||||
task.command_index, value
|
||||
) * normalized_bin_count
|
||||
),
|
||||
),
|
||||
)
|
||||
for value in feedback
|
||||
)
|
||||
)
|
||||
@@ -1041,40 +1255,41 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
float(np.ptp([self.profile.command.normalize(task.command_index, value) for value in feedback]))
|
||||
if feedback.size else 0.0
|
||||
)
|
||||
required_span = float(self.minimum_state_span_u8)
|
||||
gap = max((right - left for left, right in zip(bins, bins[1:])), default=32)
|
||||
span_policy = getattr(
|
||||
self, "_required_radian_feedback_span_fraction", None
|
||||
)
|
||||
required_span = (
|
||||
float(self.minimum_state_span_u8)
|
||||
if span_policy is None
|
||||
else float(span_policy(task, step, feedback))
|
||||
)
|
||||
gap = max(
|
||||
(right - left for left, right in zip(bins, bins[1:])),
|
||||
default=normalized_bin_count,
|
||||
)
|
||||
maximum_gap = float(self.maximum_bin_gap)
|
||||
observation = self._step_observation_metrics()
|
||||
detection_rate = float(observation["tag_detection_rate"])
|
||||
joint_frame_rate = float(observation["joint_frame_rate"])
|
||||
failures = []
|
||||
if len(rows) < int(self.minimum_sweep_frames):
|
||||
failures.append(f"frames={len(rows)}")
|
||||
if feedback.size == 0 or span < required_span:
|
||||
failures.append("feedback_span")
|
||||
if len(bins) < int(self.minimum_sweep_bins):
|
||||
failures.append(f"bins={len(bins)}")
|
||||
if gap > maximum_gap:
|
||||
failures.append(f"maximum_gap={gap}")
|
||||
if detection_rate < float(self.minimum_detection_rate):
|
||||
role_rates = observation["tag_detection_rate_by_role"]
|
||||
worst_role = min(role_rates, key=role_rates.get, default="unknown")
|
||||
tag_id = next(
|
||||
(
|
||||
tag.tag_id
|
||||
for view in self.profile.vision.views
|
||||
for tag in view.tags
|
||||
if tag.role == worst_role
|
||||
),
|
||||
"?",
|
||||
)
|
||||
failures.append(
|
||||
f"tag_rate[ID{tag_id}/{worst_role}]={detection_rate:.3f}"
|
||||
)
|
||||
if joint_frame_rate < float(self.minimum_joint_frame_rate):
|
||||
failures.append(f"joint_frame_rate={joint_frame_rate:.3f}")
|
||||
if self._feedback_hz() < float(self.minimum_feedback_hz):
|
||||
failures.append(f"feedback_hz={self._feedback_hz():.2f}")
|
||||
progress = (
|
||||
[self.profile.command.normalize(task.command_index, value) for value in feedback]
|
||||
if command_unit == "rad"
|
||||
else [float(value) / 255.0 for value in feedback]
|
||||
)
|
||||
engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile))
|
||||
decision = engine.evaluate_sweep(
|
||||
progress,
|
||||
minimum_span=(required_span if command_unit == "rad" else required_span / 255.0),
|
||||
total_frames=self.step_total_frames,
|
||||
joint_frame_rate=joint_frame_rate,
|
||||
feedback_hz=self._feedback_hz(),
|
||||
detection_rate=detection_rate,
|
||||
bin_count=(
|
||||
int(getattr(self, "normalized_sweep_bin_count", 256))
|
||||
if command_unit == "rad" else 256
|
||||
),
|
||||
)
|
||||
failures = list(decision.failures)
|
||||
append_jsonl(
|
||||
self.raw_path,
|
||||
{
|
||||
@@ -1088,8 +1303,12 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
"valid_frames": len(rows),
|
||||
"total_frames": self.step_total_frames,
|
||||
"feedback_bins": len(bins),
|
||||
"feedback_span": round(span, 9),
|
||||
"required_feedback_span": round(required_span, 9),
|
||||
"maximum_bin_gap": gap,
|
||||
**observation,
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"warnings": list(decision.warnings),
|
||||
"failures": failures,
|
||||
},
|
||||
)
|
||||
@@ -1099,16 +1318,22 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
def _retry_step(self, step: MotionStep, reason: str) -> bool:
|
||||
key = (str(step.task_key), int(step.cycle), str(step.direction))
|
||||
retries = self.retry_counts.get(key, 0)
|
||||
if retries >= int(self.automatic_sweep_retry_limit):
|
||||
if not CalibrationEngine.permits_retry("sweep_acquisition", retries):
|
||||
return False
|
||||
attempt = retries + 2
|
||||
self.retry_counts[key] = retries + 1
|
||||
task = self._task(str(step.task_key))
|
||||
start = task.start_value if step.direction == "decreasing" else task.end_value
|
||||
retry_speed = float(step.speed_u8) * (0.75 if attempt == 2 else 0.5 / 0.75)
|
||||
engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile))
|
||||
retry_speed = engine.retry_speed(step.speed_u8, attempt)
|
||||
# Preserve model-specific motion metadata on engine-generated retry
|
||||
# steps. O12 extends MotionStep with clearance/probe fields; replacing
|
||||
# it with the legacy L6 class makes the next common timer tick lose
|
||||
# that contract and can terminate the node at the retry boundary.
|
||||
step_type = type(step)
|
||||
replacement = [
|
||||
MotionStep("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt),
|
||||
MotionStep("sweep", step.task_key, step.command_index, step.target_u8, retry_speed, step.cycle, step.direction, attempt),
|
||||
step_type("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt),
|
||||
step_type("sweep", step.task_key, step.command_index, step.target_u8, retry_speed, step.cycle, step.direction, attempt),
|
||||
]
|
||||
self.steps[self.step_index + 1:self.step_index + 1] = replacement
|
||||
append_jsonl(
|
||||
@@ -1126,6 +1351,15 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
return True
|
||||
|
||||
def _finish_step(self, step: MotionStep) -> None:
|
||||
if not self._uses_isolated_motion_callbacks():
|
||||
self._finish_step_without_vision_callback(step)
|
||||
return
|
||||
assert self.step_data_lock is not None
|
||||
with self.step_data_lock:
|
||||
if not self.vision_callbacks_inflight:
|
||||
self._finish_step_without_vision_callback(step)
|
||||
|
||||
def _finish_step_without_vision_callback(self, step: MotionStep) -> None:
|
||||
if step.recording:
|
||||
try:
|
||||
self._qualify_recording_step(step)
|
||||
@@ -1138,6 +1372,77 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
if self.step_index >= len(self.steps):
|
||||
self._finalize()
|
||||
|
||||
def _radian_feedback_travel(self, step: MotionStep) -> float:
|
||||
"""Measure motion in feedback space without comparing it to commands."""
|
||||
if (
|
||||
len(self.latest_state_u8) != self.command_count
|
||||
or len(self.step_start_feedback_u8) != self.command_count
|
||||
):
|
||||
return 0.0
|
||||
if step.command_index is not None:
|
||||
index = int(step.command_index)
|
||||
return abs(
|
||||
float(self.latest_state_u8[index])
|
||||
- float(self.step_start_feedback_u8[index])
|
||||
)
|
||||
return float(sum(
|
||||
abs(
|
||||
float(self.latest_state_u8[index])
|
||||
- float(self.step_start_feedback_u8[index])
|
||||
)
|
||||
for index in self.step_moving_indices
|
||||
))
|
||||
|
||||
def _minimum_radian_feedback_travel(self, step: MotionStep) -> float:
|
||||
"""Return the safety evidence required before a non-recording move ends."""
|
||||
command_distance = float(self.step_initial_distance_u8)
|
||||
if command_distance <= 0.005 or step.recording:
|
||||
return 0.0
|
||||
if step.phase == "preflight":
|
||||
# The model-specific visual hook performs the stronger axis and
|
||||
# direction check after this electrical movement evidence.
|
||||
return min(0.004, 0.25 * command_distance)
|
||||
# Baseline/prepare/clearance/return must make most of their requested
|
||||
# move, but no absolute command-vs-feedback equality is assumed.
|
||||
return 0.60 * command_distance
|
||||
|
||||
def _tick_radian_motion(self, step: MotionStep, now: float) -> None:
|
||||
"""Advance a radian step with command/feedback domains kept separate."""
|
||||
travel = self._radian_feedback_travel(step)
|
||||
if travel >= float(self.step_last_distance_u8) + 0.001:
|
||||
self.step_last_distance_u8 = travel
|
||||
self.step_last_progress_at = now
|
||||
self.step_hold_since = None
|
||||
|
||||
command_distance = float(self.step_initial_distance_u8)
|
||||
commanded_travel = command_distance * self.step_trajectory_blend
|
||||
if self.step_trajectory_phase < 1.0:
|
||||
# Only call it a stall while the command trajectory is demanding
|
||||
# meaningful motion and feedback has provided almost none.
|
||||
if (
|
||||
commanded_travel > 0.02
|
||||
and travel < 0.10 * commanded_travel
|
||||
and now - self.step_last_progress_at
|
||||
> float(self.motor_stall_timeout_seconds)
|
||||
):
|
||||
self._pause(f"mechanical_stall:{self.reason}")
|
||||
return
|
||||
|
||||
minimum_travel = self._minimum_radian_feedback_travel(step)
|
||||
if travel + 0.001 < minimum_travel:
|
||||
if (
|
||||
now - self.step_last_progress_at
|
||||
> float(self.motor_stall_timeout_seconds)
|
||||
):
|
||||
self._pause(f"mechanical_stall:{self.reason}")
|
||||
return
|
||||
|
||||
if self.step_hold_since is None:
|
||||
self.step_hold_since = now
|
||||
return
|
||||
if now - self.step_hold_since >= float(self.endpoint_hold_seconds):
|
||||
self._finish_step(step)
|
||||
|
||||
def _tick(self) -> None:
|
||||
if self.state in {"PASSED", "PAUSED", "ABORTED"}:
|
||||
return
|
||||
@@ -1168,12 +1473,14 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
return
|
||||
now = time.monotonic()
|
||||
self._advance_step_trajectory(step, now)
|
||||
timeout = self.sweep_timeout_seconds if step.recording else self.position_timeout_seconds
|
||||
if now - self.step_started_at > float(timeout):
|
||||
self._pause(f"motion_timeout:{self.reason}")
|
||||
return
|
||||
# Absolute duration is diagnostic only. A slow but continuously
|
||||
# progressing joint is valid; the two-second no-progress watchdog is
|
||||
# the motion safety gate.
|
||||
if len(self.latest_state_u8) != self.command_count:
|
||||
return
|
||||
if self.command_unit == "rad":
|
||||
self._tick_radian_motion(step, now)
|
||||
return
|
||||
target_state = list(self._target_command(step))
|
||||
endpoint_errors = [
|
||||
abs(value - target_state[index])
|
||||
@@ -1220,10 +1527,6 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
if self.step_trajectory_phase < 1.0:
|
||||
self.step_hold_since = None
|
||||
return
|
||||
task_view = None if step.task_key is None else self._task(step.task_key).view
|
||||
if task_view is not None and now - self.last_view_valid_at.get(task_view, 0.0) > 0.25:
|
||||
self.step_hold_since = None
|
||||
return
|
||||
if self.step_hold_since is None:
|
||||
self.step_hold_since = now
|
||||
return
|
||||
@@ -1273,9 +1576,14 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
return
|
||||
self.state = "PAUSED"
|
||||
self.reason = str(reason)
|
||||
# Holding the latest position is safer than issuing an automatic move
|
||||
# after a stall, unexpected motor motion, or moved palm reference.
|
||||
if len(self.latest_state_u8) == self.command_count:
|
||||
# In radian mode, hold the last command rather than feeding an
|
||||
# uncalibrated feedback value back into the command domain.
|
||||
if (
|
||||
self.command_unit == "rad"
|
||||
and self.last_published_command_u8 is not None
|
||||
):
|
||||
self._publish_command(list(self.last_published_command_u8))
|
||||
elif len(self.latest_state_u8) == self.command_count:
|
||||
self._publish_command(
|
||||
[
|
||||
int(np.clip(round(value), 0, 255))
|
||||
@@ -1283,7 +1591,32 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
for value in self.latest_state_u8
|
||||
]
|
||||
)
|
||||
append_jsonl(self.raw_path, {"kind": "paused", "reason": self.reason})
|
||||
step = self._current_step()
|
||||
target = [] if step is None else list(self._target_command(step))
|
||||
errors = (
|
||||
[]
|
||||
if len(target) != len(self.latest_state_u8)
|
||||
else [
|
||||
abs(float(actual) - float(expected))
|
||||
for actual, expected in zip(self.latest_state_u8, target)
|
||||
]
|
||||
)
|
||||
append_jsonl(self.raw_path, {
|
||||
"kind": "paused",
|
||||
"reason": self.reason,
|
||||
"command_unit": self.command_unit,
|
||||
f"latest_state_{self.command_unit}": list(self.latest_state_u8),
|
||||
f"target_state_{self.command_unit}": target,
|
||||
f"channel_errors_{self.command_unit}": errors,
|
||||
"maximum_error_channel": (
|
||||
None
|
||||
if not errors
|
||||
else self.command_names[int(np.argmax(errors))]
|
||||
),
|
||||
f"maximum_error_{self.command_unit}": (
|
||||
None if not errors else max(errors)
|
||||
),
|
||||
})
|
||||
|
||||
def _status(self) -> dict[str, Any]:
|
||||
step = self._current_step()
|
||||
@@ -1305,16 +1638,19 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
]
|
||||
step_fraction = 0.0
|
||||
if step is not None and math.isfinite(feedback):
|
||||
current_distance = (
|
||||
float(sum(channel_errors))
|
||||
if step.command_index is None
|
||||
else channel_errors[step.command_index]
|
||||
)
|
||||
initial_distance = self.step_initial_distance_u8
|
||||
if math.isfinite(initial_distance) and initial_distance > 0.0:
|
||||
step_fraction = 1.0 - current_distance / initial_distance
|
||||
elif current_distance <= float(self.endpoint_tolerance_u8):
|
||||
step_fraction = 1.0
|
||||
if self.command_unit == "rad":
|
||||
step_fraction = self.step_trajectory_blend
|
||||
else:
|
||||
current_distance = (
|
||||
float(sum(channel_errors))
|
||||
if step.command_index is None
|
||||
else channel_errors[step.command_index]
|
||||
)
|
||||
initial_distance = self.step_initial_distance_u8
|
||||
if math.isfinite(initial_distance) and initial_distance > 0.0:
|
||||
step_fraction = 1.0 - current_distance / initial_distance
|
||||
elif current_distance <= float(self.endpoint_tolerance_u8):
|
||||
step_fraction = 1.0
|
||||
step_fraction = float(np.clip(step_fraction, 0.0, 1.0))
|
||||
elif self.state == "PASSED":
|
||||
step_fraction = 1.0
|
||||
@@ -1393,6 +1729,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
||||
self.fixed_base_maximum_corner_drift_px
|
||||
),
|
||||
"feedback_hz": round(self._feedback_hz(), 3),
|
||||
"command_publish_hz": round(self._command_hz(), 3),
|
||||
"camera_info_views": sorted(self.camera_matrices),
|
||||
"state_publisher_count": self.count_publishers(self.state_topic),
|
||||
"command_publisher_count": self.count_publishers(self.command_topic),
|
||||
|
||||
@@ -8,6 +8,8 @@ import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
from ...runtime.engine import CalibrationEngine
|
||||
|
||||
from .artifacts import (
|
||||
artifact_hashes,
|
||||
atomic_write_json,
|
||||
@@ -23,6 +25,7 @@ from .profile import (
|
||||
MEASURED_PASSIVE_JOINTS,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||
build_typed_profile,
|
||||
)
|
||||
from .urdf import L6UrdfCorrection, write_l6_corrected_urdf
|
||||
|
||||
@@ -148,6 +151,13 @@ def finalize_l6_session(
|
||||
accepted_records_by_joint(records),
|
||||
require_thumb_axis_zero=True,
|
||||
)
|
||||
CalibrationEngine(build_typed_profile()).result_from_fit(
|
||||
result,
|
||||
transfers={
|
||||
**TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
**TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||
},
|
||||
)
|
||||
# Validate the complete runtime schema before materializing any corrected
|
||||
# URDF. A fit/schema rejection therefore leaves only the node's failure
|
||||
# diagnostic and the immutable raw samples.
|
||||
|
||||
@@ -272,8 +272,8 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
),
|
||||
motion=MotionPolicy(
|
||||
tasks=tasks,
|
||||
precheck_sweeps=True,
|
||||
steady_command_checkpoints=True,
|
||||
precheck_sweeps=False,
|
||||
steady_command_checkpoints=False,
|
||||
speed_parameters={
|
||||
"preflight_u8": 1,
|
||||
"formal_u8": 1,
|
||||
@@ -313,7 +313,6 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
{
|
||||
"minimum_detection_rate",
|
||||
"maximum_state_image_skew_ms",
|
||||
"maximum_hysteresis_rad",
|
||||
"maximum_validation_error_rad",
|
||||
"maximum_mimic_residual_rad",
|
||||
}
|
||||
|
||||
@@ -33,6 +33,7 @@ _STATE_LABELS = {
|
||||
"WAIT_DEVICES": "等待六通道反馈和三相机内参",
|
||||
"READY": "设备就绪",
|
||||
"RUNNING": "标定中",
|
||||
"FINALIZING": "拟合、验证并生成 URDF",
|
||||
"PASSED": "通过",
|
||||
"PAUSED": "已暂停",
|
||||
"ABORTED": "已中止",
|
||||
@@ -47,6 +48,7 @@ _PHASE_LABELS = {
|
||||
"preflight": "任务运动预检",
|
||||
"prepare": "扫描起点准备",
|
||||
"retry_prepare": "自动重扫起点准备",
|
||||
"resume_prepare": "断点恢复起点准备",
|
||||
"sweep": "正式扫描",
|
||||
"clearance": "手指避让",
|
||||
"clearance_outer": "小指/无名指避让",
|
||||
@@ -111,12 +113,6 @@ def _l6_reason_zh(
|
||||
"查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate；先处理遮挡或"
|
||||
"反馈采样问题，再重新开始。程序已禁止发布本次结果。",
|
||||
)
|
||||
if reason.startswith("non_target_motor_moved:"):
|
||||
return (
|
||||
"MOTION-NONTARGET-304",
|
||||
"扫描期间检测到非目标电机离开保持位置。",
|
||||
"停止其他控制节点并检查机械耦合或反馈通道顺序，确认后重新开始。",
|
||||
)
|
||||
if reason.startswith("multiple_state_publishers:"):
|
||||
count = status.get("state_publisher_count", "?")
|
||||
return (
|
||||
@@ -275,28 +271,38 @@ def render_six_channel_progress_zh(
|
||||
)
|
||||
lines.append(
|
||||
f"运动：峰值 {float(speed_rad_s):.3f} rad/s；"
|
||||
f"本段 {trajectory_seconds:.1f} 秒余弦轨迹"
|
||||
f"本段 {trajectory_seconds:.1f} 秒平滑限速轨迹"
|
||||
)
|
||||
latest_state = status.get("latest_state_u8", [])
|
||||
latest_state = status.get(f"latest_state_{command_unit}", [])
|
||||
command_names = status.get("command_names", [])
|
||||
if (
|
||||
isinstance(latest_state, (list, tuple))
|
||||
and isinstance(command_names, (list, tuple))
|
||||
and len(latest_state) == len(command_names) == 6
|
||||
and len(latest_state) == len(command_names)
|
||||
and len(command_names) in {6, 12}
|
||||
and (phase == "baseline" or state in {"PAUSED", "ABORTED"})
|
||||
):
|
||||
digits = 3 if command_unit == "rad" else 1
|
||||
suffix = " rad" if command_unit == "rad" else ""
|
||||
feedback_text = ", ".join(
|
||||
f"{name}={float(value):.1f}"
|
||||
f"{name}={float(value):.{digits}f}{suffix}"
|
||||
for name, value in zip(command_names, latest_state)
|
||||
)
|
||||
maximum_error_channel = status.get("maximum_error_channel")
|
||||
maximum_error = status.get("maximum_error_u8")
|
||||
maximum_error = status.get(f"maximum_error_{command_unit}")
|
||||
error_text = (
|
||||
"未知"
|
||||
if maximum_error_channel is None or maximum_error is None
|
||||
else f"{maximum_error_channel}={float(maximum_error):.1f}"
|
||||
else (
|
||||
f"{maximum_error_channel}="
|
||||
f"{float(maximum_error):.{digits}f}{suffix}"
|
||||
)
|
||||
)
|
||||
channel_label = "六路" if len(command_names) == 6 else "十二路"
|
||||
error_label = "最大命令/反馈差" if command_unit == "rad" else "最大偏差"
|
||||
lines.append(
|
||||
f"{channel_label}反馈：{feedback_text} {error_label}：{error_text}"
|
||||
)
|
||||
lines.append(f"六路反馈：{feedback_text} 最大偏差：{error_text}")
|
||||
if state in {"PAUSED", "ABORTED"}:
|
||||
_code, problem, suggestion = reason_renderer(status)
|
||||
lines.extend((f"原因：{problem}", f"建议：{suggestion}"))
|
||||
@@ -367,6 +373,7 @@ def _launch_command(
|
||||
record_bag: bool,
|
||||
commands_enabled: bool,
|
||||
sdk_startup_speed_u8: int = 1,
|
||||
resume_from: Path | None = None,
|
||||
) -> list[str]:
|
||||
arguments = {
|
||||
"model": config.model,
|
||||
@@ -393,6 +400,10 @@ def _launch_command(
|
||||
"commands_enabled": str(commands_enabled).lower(),
|
||||
"record_bag": str(record_bag).lower(),
|
||||
}
|
||||
if resume_from is not None:
|
||||
arguments["resume_raw_samples_path"] = str(
|
||||
resume_from / "raw_samples.jsonl"
|
||||
)
|
||||
# HCAN/ZLG vendor products do not have a Linux SocketCAN interface.
|
||||
# Omitting the launch override also avoids the invalid token
|
||||
# ``can_interface:=`` when the reviewed product value is intentionally empty.
|
||||
|
||||
@@ -2,14 +2,21 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .fitting import O12FitResult, curve_input_knots_rad, curve_values_at_rad
|
||||
from .kinematics import (
|
||||
PASSIVE_POLYNOMIAL_BY_JOINT,
|
||||
PASSIVE_SDK_SOURCE_BY_JOINT,
|
||||
vendor_passive_curve,
|
||||
)
|
||||
from .profile import (
|
||||
ACTIVE_JOINTS,
|
||||
COMMAND_INDEX_BY_JOINT,
|
||||
@@ -22,10 +29,14 @@ from .profile import (
|
||||
SAFE_UPPER_RAD,
|
||||
SDK_LOWER_RAD,
|
||||
SDK_TO_URDF_JOINT,
|
||||
SDK_TO_URDF_SIGN,
|
||||
SDK_UPPER_RAD,
|
||||
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
build_typed_profile,
|
||||
)
|
||||
from .urdf import o12_endpoint_mimic_contract
|
||||
from .zero import SPATIAL_ZERO_POLICY
|
||||
|
||||
|
||||
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
|
||||
@@ -102,48 +113,107 @@ def build_o12_runtime_payload(
|
||||
passed: bool,
|
||||
) -> dict[str, Any]:
|
||||
profile = build_typed_profile()
|
||||
if result.full_hand_zero_result is not None and not result.full_hand_zero_result.passed:
|
||||
raise ValueError("O12 full-hand spatial zero validation has not passed")
|
||||
for name in STATIC_ZERO_EXCLUDED_JOINTS:
|
||||
if (
|
||||
not math.isclose(
|
||||
float(result.zero_offsets_rad.get(name, math.nan)),
|
||||
0.0,
|
||||
rel_tol=0.0,
|
||||
abs_tol=1.0e-12,
|
||||
)
|
||||
or result.zero_method_by_joint.get(name)
|
||||
!= "source_cad_zero_profile_excluded"
|
||||
):
|
||||
raise ValueError(
|
||||
f"O12 {name} must retain immutable source-CAD static zero"
|
||||
)
|
||||
source = _source_joint_metadata(source_urdf)
|
||||
endpoint_mimics = o12_endpoint_mimic_contract(source_urdf, result)
|
||||
curves: dict[str, tuple[np.ndarray, np.ndarray, np.ndarray, list[float]]] = {}
|
||||
joints: dict[str, dict[str, Any]] = {}
|
||||
|
||||
for name in ACTIVE_JOINTS:
|
||||
donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
fit = result.curves[donor]
|
||||
inputs = list(curve_input_knots_rad(donor))
|
||||
feedback_domain = result.feedback_domains_rad[donor]
|
||||
inputs = list(curve_input_knots_rad(
|
||||
donor, feedback_domain_rad=feedback_domain
|
||||
))
|
||||
motor = COMMAND_INDEX_BY_JOINT[name]
|
||||
sign = float(SDK_TO_URDF_SIGN[motor])
|
||||
decreasing = _zeroed(
|
||||
curve_values_at_rad(donor, fit, inputs, "decreasing_rad"), inputs
|
||||
curve_values_at_rad(
|
||||
donor, fit, inputs, "decreasing_rad", feedback_domain
|
||||
), inputs
|
||||
)
|
||||
increasing = _zeroed(
|
||||
curve_values_at_rad(donor, fit, inputs, "increasing_rad"), inputs
|
||||
curve_values_at_rad(
|
||||
donor, fit, inputs, "increasing_rad", feedback_domain
|
||||
), inputs
|
||||
)
|
||||
angle = 0.5 * (decreasing + increasing)
|
||||
# fit_rotation_joint_curve has an arbitrary SVD axis sign. Apply the
|
||||
# reviewed SDK-to-CAD direction contract after fitting so all curves
|
||||
# use the target URDF convention deterministically.
|
||||
observed_direction = float(np.sign(angle[-1] - angle[0]))
|
||||
if observed_direction and observed_direction != sign:
|
||||
angle = -angle
|
||||
decreasing = -decreasing
|
||||
increasing = -increasing
|
||||
if donor != name:
|
||||
cad_lower = float(source[name]["lower"])
|
||||
cad_upper = float(source[name]["upper"])
|
||||
# Same feedback means the same transferred correction until the
|
||||
# ring's own CAD/mimic range saturates. Rescaling all donor values
|
||||
# would invent a different gain for this unobserved finger.
|
||||
angle = np.clip(angle, cad_lower, cad_upper)
|
||||
decreasing = np.clip(decreasing, cad_lower, cad_upper)
|
||||
increasing = np.clip(increasing, cad_lower, cad_upper)
|
||||
curves[name] = (angle, decreasing, increasing, inputs)
|
||||
motor = COMMAND_INDEX_BY_JOINT[name]
|
||||
joint: dict[str, Any] = {
|
||||
"urdf_joint": name,
|
||||
"sdk_channel": COMMAND_NAMES[motor],
|
||||
"motor_index": motor,
|
||||
"passive": False,
|
||||
"calibration_status": profile.joint_coverage[name],
|
||||
"calibration_status": (
|
||||
"transferred_static_dynamic" if donor != name
|
||||
else "measured_dynamic_cad_static"
|
||||
if str(result.zero_method_by_joint[name]).startswith("source_cad_zero")
|
||||
else "measured_static_dynamic"
|
||||
),
|
||||
"curve_input_knots_rad": _round(inputs),
|
||||
"angle_rad": _round(angle),
|
||||
"decreasing_rad": _round(decreasing),
|
||||
"increasing_rad": _round(increasing),
|
||||
"zero_feedback_rad": 0.0,
|
||||
"urdf_sign_adapter": (
|
||||
"sdk_negative_to_urdf_positive"
|
||||
if SDK_UPPER_RAD[motor] <= 0.0 and SDK_LOWER_RAD[motor] < 0.0
|
||||
else "identity"
|
||||
"measured_feedback_domain_rad": [
|
||||
round(float(value), 10) for value in feedback_domain
|
||||
],
|
||||
"static_urdf_origin_offset_rad": round(
|
||||
float(result.zero_offsets_rad[name]), 10
|
||||
),
|
||||
"static_zero_method": str(result.zero_method_by_joint[name]),
|
||||
"sdk_to_urdf_direction": sign,
|
||||
"urdf_sign_adapter": "negate" if sign < 0.0 else "identity",
|
||||
"runtime_mapping_source": "tag_rotation_over_sdk_feedback_rad",
|
||||
"visual_arc_diagnostic_rad": round(
|
||||
float(result.visual_arc_diagnostics_rad[donor]), 10
|
||||
),
|
||||
"raw_increasing_curve_branch": (
|
||||
"decreasing"
|
||||
if _task_for_joint(donor).end_value > _task_for_joint(donor).start_value
|
||||
if _task_for_joint(donor).end_value
|
||||
> _task_for_joint(donor).start_value
|
||||
else "increasing"
|
||||
),
|
||||
}
|
||||
if donor != name:
|
||||
joint["transferred_from_joint"] = donor
|
||||
joint["transfer_policy"] = "pinky_feedback_correction_on_ring_cad"
|
||||
joint["transfer_policy"] = (
|
||||
"pinky_curve_clamped_to_ring_cad_range"
|
||||
)
|
||||
joint["static_zero_transfer_policy"] = "donor_offset_on_own_cad_frame"
|
||||
joints[name] = joint
|
||||
|
||||
for name in PASSIVE_JOINTS:
|
||||
@@ -152,20 +222,33 @@ def build_o12_runtime_payload(
|
||||
motor = _motor_index(source_name)
|
||||
if name in MEASURED_PASSIVE_JOINTS:
|
||||
fit = result.curves[name]
|
||||
inputs = list(curve_input_knots_rad(name))
|
||||
decreasing = _zeroed(
|
||||
curve_values_at_rad(name, fit, inputs, "decreasing_rad"), inputs
|
||||
) + float(metadata["offset"])
|
||||
increasing = _zeroed(
|
||||
curve_values_at_rad(name, fit, inputs, "increasing_rad"), inputs
|
||||
) + float(metadata["offset"])
|
||||
angle = 0.5 * (decreasing + increasing)
|
||||
coupling = result.mimic_fits[name]
|
||||
coefficients = [float(metadata["offset"]), *coupling.coefficients]
|
||||
feedback_domain = result.feedback_domains_rad[name]
|
||||
inputs = list(curve_input_knots_rad(
|
||||
name, feedback_domain_rad=feedback_domain
|
||||
))
|
||||
sdk_source = PASSIVE_SDK_SOURCE_BY_JOINT[name]
|
||||
sdk_motor = _motor_index(sdk_source)
|
||||
expected_direction = float(SDK_TO_URDF_SIGN[sdk_motor])
|
||||
vendor_values = np.asarray(
|
||||
vendor_passive_curve(
|
||||
name,
|
||||
inputs,
|
||||
sdk_to_urdf_sign=expected_direction,
|
||||
),
|
||||
dtype=float,
|
||||
)
|
||||
angle = vendor_values
|
||||
decreasing = vendor_values.copy()
|
||||
increasing = vendor_values.copy()
|
||||
observed = result.mimic_fits[name]
|
||||
coefficients = [
|
||||
expected_direction * float(value)
|
||||
for value in PASSIVE_POLYNOMIAL_BY_JOINT[name]
|
||||
]
|
||||
coefficients.extend([0.0] * (6 - len(coefficients)))
|
||||
coupling_model = coupling.model
|
||||
multiplier = coupling.urdf_mimic_multiplier
|
||||
policy = coupling.urdf_mimic_policy
|
||||
coupling_model = "vendor_o12_polynomial"
|
||||
multiplier = endpoint_mimics[name]
|
||||
policy = "source_cad_closed_endpoint"
|
||||
status = profile.joint_coverage[name]
|
||||
else:
|
||||
source_curve, source_dec, source_inc, inputs = curves[source_name]
|
||||
@@ -179,14 +262,14 @@ def build_o12_runtime_payload(
|
||||
policy = "cad_nominal_preserved"
|
||||
status = profile.joint_coverage[name]
|
||||
curves[name] = (angle, decreasing, increasing, list(inputs))
|
||||
joints[name] = {
|
||||
joint = {
|
||||
"urdf_joint": name,
|
||||
"sdk_channel": COMMAND_NAMES[motor],
|
||||
"motor_index": motor,
|
||||
"passive": True,
|
||||
"source_joint": source_name,
|
||||
"calibration_status": status,
|
||||
"static_zero_policy": "cad_preserved_tag_mount_ambiguous",
|
||||
"static_zero_policy": "cad_mechanical_endpoint",
|
||||
"curve_input_knots_rad": _round(inputs),
|
||||
"angle_rad": _round(angle),
|
||||
"decreasing_rad": _round(decreasing),
|
||||
@@ -195,12 +278,37 @@ def build_o12_runtime_payload(
|
||||
"coupling_coefficients": [round(float(v), 10) for v in coefficients],
|
||||
"mimic_multiplier": round(float(multiplier), 10),
|
||||
"urdf_mimic_policy": policy,
|
||||
"runtime_mapping_source": (
|
||||
"o12_vendor_solver_over_sdk_feedback_rad"
|
||||
if name in MEASURED_PASSIVE_JOINTS
|
||||
else "source_urdf_cad_mimic"
|
||||
),
|
||||
"raw_increasing_curve_branch": (
|
||||
"decreasing"
|
||||
if _task_for_joint(name).end_value > _task_for_joint(name).start_value
|
||||
if _task_for_joint(name).end_value
|
||||
> _task_for_joint(name).start_value
|
||||
else "increasing"
|
||||
),
|
||||
}
|
||||
if name in MEASURED_PASSIVE_JOINTS:
|
||||
joint.update({
|
||||
"measured_feedback_domain_rad": [
|
||||
round(float(value), 10) for value in feedback_domain
|
||||
],
|
||||
"visual_observation_role": "independent_dynamic_validation",
|
||||
"vendor_sdk_source_joint": sdk_source,
|
||||
"observed_coupling_model": observed.model,
|
||||
"observed_mimic_multiplier": round(
|
||||
float(observed.urdf_mimic_multiplier), 10
|
||||
),
|
||||
"observed_residual_rms_rad": round(
|
||||
float(observed.residual_rms_rad), 10
|
||||
),
|
||||
"observed_visual_arc_rad": round(
|
||||
float(result.visual_arc_diagnostics_rad[name]), 10
|
||||
),
|
||||
})
|
||||
joints[name] = joint
|
||||
|
||||
errors = np.abs(np.concatenate([
|
||||
np.asarray(values, dtype=float)
|
||||
@@ -213,7 +321,7 @@ def build_o12_runtime_payload(
|
||||
"model": "O12",
|
||||
"side": "right",
|
||||
"serial_number": str(serial_number),
|
||||
"calibration_scope": "full",
|
||||
"calibration_scope": "full_dynamic_except_thumb_mcp_static_zero",
|
||||
"publication_pointer": "latest_passed",
|
||||
"angle_unit": "rad",
|
||||
"curve_input_domain": "feedback_rad",
|
||||
@@ -243,8 +351,61 @@ def build_o12_runtime_payload(
|
||||
"validation_mae_rad": round(float(np.mean(errors)), 10),
|
||||
"validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10),
|
||||
"validation_max_rad": round(float(np.max(errors)), 10),
|
||||
"roll_cross_view": {
|
||||
joint: {
|
||||
key: round(float(value), 10)
|
||||
for key, value in metrics.items()
|
||||
}
|
||||
for joint, metrics in result.cross_view_roll_metrics.items()
|
||||
},
|
||||
"thumb_root_spatial_zero": (
|
||||
None
|
||||
if result.thumb_root_zero_result is None
|
||||
else {
|
||||
"offsets_rad": {
|
||||
name: round(float(value), 10)
|
||||
for name, value in result.thumb_root_zero_result.direct_offsets_rad.items()
|
||||
},
|
||||
"validation_mae_rad": round(float(np.mean(np.abs(
|
||||
result.thumb_root_zero_result.validation_errors_rad
|
||||
))), 10),
|
||||
"validation_p95_rad": round(float(np.percentile(np.abs(
|
||||
result.thumb_root_zero_result.validation_errors_rad
|
||||
), 95.0)), 10),
|
||||
"validation_max_rad": round(float(np.max(np.abs(
|
||||
result.thumb_root_zero_result.validation_errors_rad
|
||||
))), 10),
|
||||
"axis_line_rms_m_diagnostic": round(float(
|
||||
result.thumb_root_zero_result.axis_line_rms_m
|
||||
), 10),
|
||||
"method": "roll_yaw_pitch_axes_with_pinky_palm_orientation",
|
||||
"validation_scope": "root_axis_angles_not_full_chain_position",
|
||||
}
|
||||
),
|
||||
"ring_transfer_source": "pinky_mcp_pitch",
|
||||
"full_hand_spatial_zero": (
|
||||
None if result.full_hand_zero_result is None else {
|
||||
**asdict(result.full_hand_zero_result),
|
||||
"policy": SPATIAL_ZERO_POLICY,
|
||||
"validation_scope": "active_zero_axis_geometry_not_fingertip_contact",
|
||||
}
|
||||
),
|
||||
"cad_static_joints_not_measured": [
|
||||
name for name, method in result.zero_method_by_joint.items()
|
||||
if str(method).startswith("source_cad_zero")
|
||||
],
|
||||
"static_zero_exclusions": {
|
||||
name: {
|
||||
"static_origin_source": "immutable_source_cad",
|
||||
"reason": "downstream_curved_shell_tag_not_absolute_axis_line_datum",
|
||||
"dynamic_curve_and_holdout_required": True,
|
||||
}
|
||||
for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS)
|
||||
},
|
||||
"ring_cad_geometry_and_mimic_preserved": True,
|
||||
"active_angle_source": "tag_rotation_over_sdk_feedback_rad",
|
||||
"passive_runtime_source": "o12_vendor_solver_polynomial",
|
||||
"visual_measurement_role": "active_curve_and_passive_validation",
|
||||
},
|
||||
}
|
||||
validate_o12_runtime_payload(payload)
|
||||
@@ -306,8 +467,149 @@ def validate_o12_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError(f"{name} has invalid O12 motor binding")
|
||||
if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}:
|
||||
raise ValueError(f"{name} has invalid O12 direction binding")
|
||||
for name in ACTIVE_JOINTS:
|
||||
joint = joints[name]
|
||||
motor = COMMAND_INDEX_BY_JOINT[name]
|
||||
sign = float(SDK_TO_URDF_SIGN[motor])
|
||||
offset = float(joint.get("static_urdf_origin_offset_rad", math.nan))
|
||||
if not math.isfinite(offset) or abs(offset) > math.radians(20.0):
|
||||
raise ValueError(f"{name} has invalid O12 static origin offset")
|
||||
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
differences = np.diff(np.asarray(joint[field], dtype=float))
|
||||
if np.any(sign * differences < -1.0e-7):
|
||||
raise ValueError(
|
||||
f"{name}.{field} disagrees with fixed O12 SDK/URDF direction"
|
||||
)
|
||||
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
|
||||
if not math.isclose(
|
||||
float(joints[target]["static_urdf_origin_offset_rad"]),
|
||||
float(joints[donor]["static_urdf_origin_offset_rad"]),
|
||||
rel_tol=0.0, abs_tol=1.0e-9,
|
||||
):
|
||||
raise ValueError(f"{target} static zero differs from transfer donor {donor}")
|
||||
for name in PASSIVE_JOINTS:
|
||||
joint = joints[name]
|
||||
source_joint = str(joint.get("source_joint", ""))
|
||||
if source_joint != MIMIC_SOURCE_BY_JOINT[name]:
|
||||
raise ValueError(f"{name} has invalid O12 mimic source")
|
||||
multiplier = float(joint.get("mimic_multiplier", math.nan))
|
||||
coefficients = np.asarray(
|
||||
joint.get("coupling_coefficients", ()), dtype=float
|
||||
)
|
||||
if coefficients.shape != (6,) or not np.all(np.isfinite(coefficients)):
|
||||
raise ValueError(f"{name} has invalid O12 CAD mimic coefficients")
|
||||
if not math.isfinite(multiplier) or multiplier <= 0.0:
|
||||
raise ValueError(f"{name} has invalid O12 mimic multiplier")
|
||||
if name not in MEASURED_PASSIVE_JOINTS:
|
||||
offset = float(coefficients[0])
|
||||
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
expected_values = offset + multiplier * np.asarray(
|
||||
joints[source_joint][field], dtype=float
|
||||
)
|
||||
if not np.allclose(
|
||||
np.asarray(joint[field], dtype=float),
|
||||
expected_values,
|
||||
rtol=0.0,
|
||||
atol=2.0e-9,
|
||||
):
|
||||
raise ValueError(
|
||||
f"{name}.{field} differs from the source CAD mimic chain"
|
||||
)
|
||||
if not bool(payload["quality"].get("passed")):
|
||||
raise ValueError("failed O12 calibration cannot be published")
|
||||
|
||||
|
||||
__all__ = ["ALL_REVOLUTE_JOINTS", "build_o12_runtime_payload", "validate_o12_runtime_payload"]
|
||||
def validate_o12_runtime_payload_against_urdf(
|
||||
payload: Mapping[str, Any], urdf: str | Path,
|
||||
*, source_urdf: str | Path | None = None,
|
||||
) -> None:
|
||||
"""Check curve bounds/mimics and, with original CAD, actual static frames.
|
||||
|
||||
This verifies the artifact contract, not full-hand physical accuracy.
|
||||
Passive runtime polynomials and linear URDF mimic are distinct models.
|
||||
"""
|
||||
validate_o12_runtime_payload(payload)
|
||||
metadata = _source_joint_metadata(urdf)
|
||||
joints = payload["joints"]
|
||||
for name, joint in joints.items():
|
||||
expected = metadata[name]
|
||||
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
values = np.asarray(joint[field], dtype=float)
|
||||
allowance = 0.0
|
||||
if bool(joint.get("passive")):
|
||||
source_name = str(expected["source_joint"])
|
||||
if source_name != str(joint["source_joint"]):
|
||||
raise ValueError(
|
||||
f"{name} corrected URDF mimic source differs from payload"
|
||||
)
|
||||
if not math.isclose(
|
||||
float(expected["multiplier"]),
|
||||
float(joint["mimic_multiplier"]),
|
||||
rel_tol=0.0,
|
||||
abs_tol=2.0e-9,
|
||||
):
|
||||
raise ValueError(
|
||||
f"{name} corrected URDF mimic differs from payload"
|
||||
)
|
||||
if name not in MEASURED_PASSIVE_JOINTS:
|
||||
source_values = np.asarray(
|
||||
joints[source_name][field], dtype=float
|
||||
)
|
||||
cad_values = (
|
||||
float(expected["offset"])
|
||||
+ float(expected["multiplier"]) * source_values
|
||||
)
|
||||
if not np.allclose(
|
||||
values, cad_values, rtol=0.0, atol=2.0e-9
|
||||
):
|
||||
raise ValueError(
|
||||
f"{name}.{field} differs from preserved CAD mimic"
|
||||
)
|
||||
allowance = max(
|
||||
0.0,
|
||||
float(np.max(cad_values)) - float(expected["upper"]),
|
||||
float(expected["lower"]) - float(np.min(cad_values)),
|
||||
)
|
||||
if (
|
||||
float(np.min(values))
|
||||
< float(expected["lower"]) - allowance - 2.0e-9
|
||||
):
|
||||
raise ValueError(f"{name}.{field} is below the URDF physical limit")
|
||||
if (
|
||||
float(np.max(values))
|
||||
> float(expected["upper"]) + allowance + 2.0e-9
|
||||
):
|
||||
raise ValueError(f"{name}.{field} is above the URDF physical limit")
|
||||
if source_urdf is not None:
|
||||
original = ET.parse(source_urdf).getroot()
|
||||
corrected = ET.parse(urdf).getroot()
|
||||
for name in ACTIVE_JOINTS:
|
||||
before = original.find(f"joint[@name='{name}']")
|
||||
after = corrected.find(f"joint[@name='{name}']")
|
||||
if before is None or after is None:
|
||||
raise ValueError(f"{name} missing from static-frame validation")
|
||||
b_origin, a_origin = before.find("origin"), after.find("origin")
|
||||
if b_origin is None or a_origin is None:
|
||||
raise ValueError(f"{name} missing static origin")
|
||||
if b_origin.get("xyz") != a_origin.get("xyz"):
|
||||
raise ValueError(f"{name} static origin translation changed")
|
||||
axis_node = before.find("axis")
|
||||
if axis_node is None:
|
||||
raise ValueError(f"{name} missing CAD axis")
|
||||
axis = np.asarray([float(v) for v in axis_node.get("xyz").split()])
|
||||
axis /= np.linalg.norm(axis)
|
||||
b_rot = Rotation.from_euler("xyz", [float(v) for v in b_origin.get("rpy", "0 0 0").split()])
|
||||
a_rot = Rotation.from_euler("xyz", [float(v) for v in a_origin.get("rpy", "0 0 0").split()])
|
||||
expected_rot = b_rot * Rotation.from_rotvec(
|
||||
axis * float(joints[name]["static_urdf_origin_offset_rad"])
|
||||
)
|
||||
if (expected_rot.inv() * a_rot).magnitude() > 1.0e-8:
|
||||
raise ValueError(f"{name} static origin disagrees with payload")
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ALL_REVOLUTE_JOINTS",
|
||||
"build_o12_runtime_payload",
|
||||
"validate_o12_runtime_payload",
|
||||
"validate_o12_runtime_payload_against_urdf",
|
||||
]
|
||||
|
||||
@@ -2,26 +2,44 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..g20.profile import JointCurveFit
|
||||
from ..g20.zero_solver import fit_rotation_joint_curve, rotation_curve_holdout_errors
|
||||
from ..g20.profile import (
|
||||
HandCalibrationProfile,
|
||||
JointCurveFit,
|
||||
JointSpec,
|
||||
cross_view_roll_diagnostic_metrics,
|
||||
)
|
||||
from ..g20.zero_solver import (
|
||||
ZeroCalibrationProfile,
|
||||
ZeroSolveResult,
|
||||
fit_joint_axis_measurement,
|
||||
fit_rotation_joint_curve,
|
||||
rotation_curve_holdout_errors,
|
||||
solve_urdf_zero_offsets,
|
||||
with_depth_free_axis_projection,
|
||||
)
|
||||
from ..l6.fitting import MimicFit, fit_coupling_model
|
||||
from .profile import (
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
COMMAND_INDEX_BY_JOINT,
|
||||
COMMAND_NAMES,
|
||||
GEOMETRIC_ZERO_JOINTS,
|
||||
ROOT_GEOMETRIC_ZERO_JOINTS,
|
||||
MEASURED_PASSIVE_JOINTS,
|
||||
MIMIC_SOURCE_BY_JOINT,
|
||||
SDK_TO_URDF_JOINT,
|
||||
SDK_TO_URDF_SIGN,
|
||||
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
build_typed_profile,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class O12FitResult:
|
||||
curves: Mapping[str, JointCurveFit]
|
||||
@@ -29,6 +47,86 @@ class O12FitResult:
|
||||
travels_rad: Mapping[str, float]
|
||||
mimic_fits: Mapping[str, MimicFit]
|
||||
holdout_errors_rad: Mapping[str, tuple[float, ...]]
|
||||
cross_view_roll_metrics: Mapping[str, Mapping[str, float]]
|
||||
visual_arc_diagnostics_rad: Mapping[str, float]
|
||||
feedback_domains_rad: Mapping[str, tuple[float, float]]
|
||||
zero_method_by_joint: Mapping[str, str]
|
||||
thumb_root_zero_result: ZeroSolveResult | None
|
||||
full_hand_zero_result: ZeroSolveResult | None = None
|
||||
|
||||
|
||||
O12_THUMB_ROOT_AXIS_JOINTS: tuple[str, ...] = (
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
)
|
||||
|
||||
|
||||
def _thumb_root_zero_profile() -> ZeroCalibrationProfile:
|
||||
"""Declare the spatial graph that observes O12 thumb roll/yaw zeros.
|
||||
|
||||
A relative Tag rotation observes roll travel but not its constant phase.
|
||||
The downstream yaw screw axis rotates with that phase. The independently
|
||||
observed pinky MCP axis fixes palm orientation, leaving roll as the only
|
||||
fitted roll coordinate. The pitch axis in turn observes yaw phase.
|
||||
Parallel pitch/MCP axes do not identify their own phase from direction;
|
||||
those origins retain CAD values instead of inferring zeros from travel.
|
||||
"""
|
||||
specs = {
|
||||
name: JointSpec(
|
||||
name,
|
||||
COMMAND_INDEX_BY_JOINT[name],
|
||||
True,
|
||||
None,
|
||||
None,
|
||||
None,
|
||||
pose_axis_line_required=False,
|
||||
)
|
||||
for name in O12_THUMB_ROOT_AXIS_JOINTS
|
||||
}
|
||||
hand = HandCalibrationProfile(
|
||||
side="right",
|
||||
reference_finger="pinky",
|
||||
view_tags={},
|
||||
preflight_view_roles={},
|
||||
joint_specs=specs,
|
||||
sweep_specs=(),
|
||||
image_trajectory_joints=frozenset(),
|
||||
roll_clearance_commands={},
|
||||
thumb_pitch_clearance_commands={},
|
||||
layout_id="o12_right_16",
|
||||
model="O12",
|
||||
command_names=COMMAND_NAMES,
|
||||
baseline_command=(255,) * len(COMMAND_NAMES),
|
||||
directional_zero=True,
|
||||
isolated_holdout=True,
|
||||
# As in the G20 multi-view solver, a repeatable planar-PnP cone bias
|
||||
# is diagnostic rather than a zero-phase failure. Acceptance still
|
||||
# requires the per-cycle cone magnitude to be stable, the fitted axis
|
||||
# lines to pass, and the independent holdout to improve.
|
||||
stable_cross_view_cone_bias=True,
|
||||
)
|
||||
return ZeroCalibrationProfile(
|
||||
hand=hand,
|
||||
direct_zero_joints=("thumb_cmc_roll", "thumb_cmc_yaw"),
|
||||
axis_joints=O12_THUMB_ROOT_AXIS_JOINTS,
|
||||
inherited_zero_joints={},
|
||||
inherited_static_zero_joints={},
|
||||
constrained_circle_joints=frozenset(O12_THUMB_ROOT_AXIS_JOINTS),
|
||||
root_anchor_joints=frozenset({"thumb_cmc_roll"}),
|
||||
axis_parent_joint={"thumb_cmc_yaw": "thumb_cmc_roll", "thumb_cmc_pitch": "thumb_cmc_yaw"},
|
||||
phase_parent_joint={},
|
||||
offset_observer_joint={"thumb_cmc_roll": "thumb_cmc_yaw", "thumb_cmc_yaw": "thumb_cmc_pitch"},
|
||||
same_view_axis_pair_by_offset={},
|
||||
fixed_direct_zero_offsets_rad={},
|
||||
static_output_zero_offsets_rad={},
|
||||
base_pose_strategy="thumb_serial",
|
||||
orientation_anchor_joint="pinky_mcp_pitch",
|
||||
directed_base_axis_joints=frozenset({
|
||||
"thumb_cmc_roll", "pinky_mcp_pitch",
|
||||
}),
|
||||
)
|
||||
|
||||
|
||||
def _task_by_joint() -> dict[str, Any]:
|
||||
@@ -39,51 +137,168 @@ def _task_by_joint() -> dict[str, Any]:
|
||||
}
|
||||
|
||||
|
||||
def feedback_rad_to_curve_index(joint: str, feedback_rad: float) -> float:
|
||||
"""Map a physical feedback angle to the fitter's normalized 255..0 axis."""
|
||||
def feedback_rad_to_curve_index(
|
||||
joint: str,
|
||||
feedback_rad: float,
|
||||
feedback_domain_rad: tuple[float, float] | None = None,
|
||||
) -> float:
|
||||
"""Map measured feedback to the fitter's normalized 255..0 axis."""
|
||||
task = _task_by_joint()[str(joint)]
|
||||
denominator = task.end_value - task.start_value
|
||||
if feedback_domain_rad is None:
|
||||
start, end = float(task.start_value), float(task.end_value)
|
||||
else:
|
||||
lower, upper = (float(value) for value in feedback_domain_rad)
|
||||
if task.end_value >= task.start_value:
|
||||
start, end = lower, upper
|
||||
else:
|
||||
start, end = upper, lower
|
||||
denominator = end - start
|
||||
if abs(denominator) <= 1.0e-12:
|
||||
raise ValueError(f"O12 task {task.key} has a degenerate range")
|
||||
phase = (float(feedback_rad) - task.start_value) / denominator
|
||||
phase = (float(feedback_rad) - start) / denominator
|
||||
return 255.0 * (1.0 - float(np.clip(phase, 0.0, 1.0)))
|
||||
|
||||
|
||||
def curve_input_knots_rad(joint: str, count: int = 65) -> tuple[float, ...]:
|
||||
def curve_input_knots_rad(
|
||||
joint: str,
|
||||
count: int = 65,
|
||||
feedback_domain_rad: tuple[float, float] | None = None,
|
||||
) -> tuple[float, ...]:
|
||||
task = _task_by_joint()[str(joint)]
|
||||
if feedback_domain_rad is None:
|
||||
lower, upper = sorted((task.start_value, task.end_value))
|
||||
else:
|
||||
lower, upper = sorted(float(value) for value in feedback_domain_rad)
|
||||
# Zero is the open/centred runtime command. Include it in the public
|
||||
# knot domain while clamping the tiny unobserved offset to the measured
|
||||
# endpoint instead of extrapolating a full SDK command range.
|
||||
lower, upper = min(0.0, lower), max(0.0, upper)
|
||||
return tuple(
|
||||
float(value)
|
||||
for value in np.linspace(
|
||||
min(task.start_value, task.end_value),
|
||||
max(task.start_value, task.end_value),
|
||||
lower,
|
||||
upper,
|
||||
int(count),
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def curve_values_at_rad(
|
||||
joint: str, fit: JointCurveFit, inputs_rad: Sequence[float], branch: str
|
||||
joint: str,
|
||||
fit: JointCurveFit,
|
||||
inputs_rad: Sequence[float],
|
||||
branch: str,
|
||||
feedback_domain_rad: tuple[float, float] | None = None,
|
||||
) -> tuple[float, ...]:
|
||||
values = np.asarray(getattr(fit, branch), dtype=float)
|
||||
indices = np.arange(256, dtype=float)
|
||||
return tuple(
|
||||
float(np.interp(feedback_rad_to_curve_index(joint, value), indices, values))
|
||||
float(np.interp(
|
||||
feedback_rad_to_curve_index(joint, value, feedback_domain_rad),
|
||||
indices,
|
||||
values,
|
||||
))
|
||||
for value in inputs_rad
|
||||
)
|
||||
|
||||
|
||||
def _virtual_records(
|
||||
joint: str, rows: Sequence[Mapping[str, Any]]
|
||||
joint: str,
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
feedback_domain_rad: tuple[float, float] | None = None,
|
||||
feedback_domains_rad: Mapping[str, tuple[float, float]] | None = None,
|
||||
) -> list[dict[str, Any]]:
|
||||
result = []
|
||||
for source in rows:
|
||||
row = dict(source)
|
||||
curve_index = feedback_rad_to_curve_index(joint, float(row["feedback_rad"]))
|
||||
curve_index = feedback_rad_to_curve_index(
|
||||
joint, float(row["feedback_rad"]), feedback_domain_rad
|
||||
)
|
||||
row["command_u8"] = int(np.clip(round(curve_index), 0, 255))
|
||||
if "state_rad" in row:
|
||||
state_rad = tuple(float(value) for value in row["state_rad"])
|
||||
if len(state_rad) != len(SDK_TO_URDF_JOINT):
|
||||
raise ValueError("O12 state_rad has the wrong channel count")
|
||||
state_u8 = []
|
||||
for channel, value in enumerate(state_rad):
|
||||
state_joint = SDK_TO_URDF_JOINT[channel]
|
||||
state_joint = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(
|
||||
state_joint, state_joint
|
||||
)
|
||||
state_u8.append(
|
||||
feedback_rad_to_curve_index(
|
||||
state_joint,
|
||||
value,
|
||||
(feedback_domains_rad or {}).get(state_joint),
|
||||
)
|
||||
)
|
||||
row["state_u8"] = state_u8
|
||||
result.append(row)
|
||||
return result
|
||||
|
||||
|
||||
def _measured_feedback_domain(
|
||||
joint: str, rows: Sequence[Mapping[str, Any]]
|
||||
) -> tuple[float, float]:
|
||||
"""Return the full-stroke feedback interval established by cycle zero."""
|
||||
values = [
|
||||
float(row["feedback_rad"])
|
||||
for row in rows
|
||||
if int(row.get("cycle", -1)) == 0
|
||||
and math.isfinite(float(row.get("feedback_rad", math.nan)))
|
||||
]
|
||||
if not values or max(values) - min(values) <= 1.0e-9:
|
||||
raise ValueError(f"{joint} cycle 0 has no measurable feedback travel")
|
||||
return float(min(values)), float(max(values))
|
||||
|
||||
|
||||
def _validate_cycle_repeatability(
|
||||
joint: str,
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
feedback_domain_rad: tuple[float, float],
|
||||
) -> None:
|
||||
"""Require later cycles to repeat measured travel, not command scale."""
|
||||
reference = feedback_domain_rad[1] - feedback_domain_rad[0]
|
||||
for cycle in (1, 2, 3):
|
||||
values = [
|
||||
float(row["feedback_rad"])
|
||||
for row in rows
|
||||
if int(row.get("cycle", -1)) == cycle
|
||||
and math.isfinite(float(row.get("feedback_rad", math.nan)))
|
||||
]
|
||||
span = max(values) - min(values) if values else 0.0
|
||||
if span < 0.90 * reference:
|
||||
raise ValueError(
|
||||
f"{joint} cycle {cycle} repeats only "
|
||||
f"{span / reference:.3f} of cycle-zero measured travel"
|
||||
)
|
||||
|
||||
|
||||
def _uniform_curve_holdout_errors(
|
||||
rows: Sequence[Mapping[str, Any]], errors: Sequence[float]
|
||||
) -> tuple[float, ...]:
|
||||
"""Give every observed feedback bin equal holdout weight.
|
||||
|
||||
O12 motor feedback can saturate before a passive tendon joint has fully
|
||||
settled. A camera then records many frames at one identical feedback bin.
|
||||
The curve fitter already uses one median per bin; validation must use the
|
||||
same curve-space measure instead of letting endpoint dwell duration
|
||||
multiply the P95 weight of that single coordinate.
|
||||
"""
|
||||
if len(rows) != len(errors):
|
||||
raise ValueError("O12 holdout rows and errors have different lengths")
|
||||
grouped: dict[tuple[str, int], list[float]] = {}
|
||||
for row, error in zip(rows, errors):
|
||||
grouped.setdefault(
|
||||
(str(row["direction"]), int(row["command_u8"])), []
|
||||
).append(float(error))
|
||||
if not grouped:
|
||||
raise ValueError("O12 holdout has no observed feedback bins")
|
||||
return tuple(
|
||||
float(np.median(grouped[key])) for key in sorted(grouped)
|
||||
)
|
||||
|
||||
|
||||
def _travel(fit: JointCurveFit) -> float:
|
||||
return 0.5 * (
|
||||
float(fit.decreasing_rad[0] - fit.decreasing_rad[255])
|
||||
@@ -91,11 +306,134 @@ def _travel(fit: JointCurveFit) -> float:
|
||||
)
|
||||
|
||||
|
||||
def measured_curve_bounds(
|
||||
name: str,
|
||||
fit: JointCurveFit,
|
||||
feedback_domain_rad: tuple[float, float],
|
||||
*,
|
||||
sign: float,
|
||||
) -> tuple[float, float]:
|
||||
"""Return the zero-referenced public URDF range of one O12 curve."""
|
||||
inputs = curve_input_knots_rad(name, feedback_domain_rad=feedback_domain_rad)
|
||||
branches = []
|
||||
for branch in ("decreasing_rad", "increasing_rad"):
|
||||
values = np.asarray(
|
||||
curve_values_at_rad(name, fit, inputs, branch, feedback_domain_rad),
|
||||
dtype=float,
|
||||
)
|
||||
values -= float(np.interp(0.0, np.asarray(inputs, dtype=float), values))
|
||||
branches.append(values)
|
||||
average = 0.5 * (branches[0] + branches[1])
|
||||
direction = float(np.sign(average[-1] - average[0]))
|
||||
if direction not in {0.0, float(sign)}:
|
||||
branches = [-values for values in branches]
|
||||
average = -average
|
||||
values = np.concatenate([average, *branches])
|
||||
return float(np.min(values)), float(np.max(values))
|
||||
|
||||
|
||||
def _has_complete_thumb_root_geometry(
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
) -> bool:
|
||||
required = {
|
||||
"relative_translation_xyz_m",
|
||||
"parent_pose_common",
|
||||
"child_pose_common",
|
||||
"view_normal_common_xyz",
|
||||
"camera_center_common_xyz_m",
|
||||
"state_rad",
|
||||
}
|
||||
return all(
|
||||
rows and all(required.issubset(row) for row in rows)
|
||||
for name in O12_THUMB_ROOT_AXIS_JOINTS
|
||||
for rows in (records_by_joint.get(name, ()),)
|
||||
)
|
||||
|
||||
|
||||
def _fit_thumb_root_zero(
|
||||
source_urdf: str | Path,
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
curves: Mapping[str, JointCurveFit],
|
||||
feedback_domains_rad: Mapping[str, tuple[float, float]],
|
||||
) -> ZeroSolveResult:
|
||||
"""Fit the O12 root thumb phases with the shared G20 geometry kernel."""
|
||||
profile = _thumb_root_zero_profile()
|
||||
measurements = []
|
||||
for cycle in range(4):
|
||||
for name in O12_THUMB_ROOT_AXIS_JOINTS:
|
||||
rows = _virtual_records(
|
||||
name,
|
||||
records_by_joint[name],
|
||||
feedback_domains_rad[name],
|
||||
feedback_domains_rad,
|
||||
)
|
||||
measurement = fit_joint_axis_measurement(
|
||||
name,
|
||||
rows,
|
||||
cycle=cycle,
|
||||
zero_command_u8=255,
|
||||
constrained_circle_joints=profile.constrained_circle_joints,
|
||||
view_normal_common_xyz=rows[0]["view_normal_common_xyz"],
|
||||
canonical_zero_direction="decreasing",
|
||||
)
|
||||
measurements.append(with_depth_free_axis_projection(
|
||||
measurement,
|
||||
rows[0]["camera_center_common_xyz_m"],
|
||||
))
|
||||
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=source_urdf,
|
||||
measurements=measurements,
|
||||
curves=curves,
|
||||
motor_by_joint={
|
||||
name: COMMAND_INDEX_BY_JOINT[name]
|
||||
for name in O12_THUMB_ROOT_AXIS_JOINTS
|
||||
},
|
||||
training_cycles=(0, 1, 2),
|
||||
validation_cycle=3,
|
||||
maximum_offset_rad=math.radians(20.0),
|
||||
finger_maximum_offset_rad=math.radians(20.0),
|
||||
joint_maximum_offset_rad={
|
||||
name: math.radians(20.0) for name in ROOT_GEOMETRIC_ZERO_JOINTS
|
||||
},
|
||||
maximum_cycle_difference_rad=math.radians(0.75),
|
||||
minimum_applied_offset_rad=math.radians(0.1),
|
||||
maximum_validation_mae_rad=math.radians(1.0),
|
||||
maximum_validation_p95_rad=math.radians(2.0),
|
||||
maximum_validation_error_rad=math.radians(3.0),
|
||||
maximum_confidence_half_width_rad=math.radians(1.0),
|
||||
# Axis-line displacement is retained as a diagnostic. Roll phase is
|
||||
# determined by axis directions, so monocular depth/Tag placement is
|
||||
# not allowed to reject an otherwise repeatable angular solution.
|
||||
maximum_pose_axis_line_rms_m=0.0015,
|
||||
maximum_systematic_axis_cone_bias_rad=math.radians(15.0),
|
||||
hand_type="right",
|
||||
tag_layout="o12_right_16",
|
||||
zero_profile=profile,
|
||||
)
|
||||
if not result.passed:
|
||||
details = ",".join(
|
||||
f"{name}={reason}"
|
||||
for name, reason in sorted(result.failure_reasons.items())
|
||||
)
|
||||
raise ValueError("O12 thumb root spatial zero solve failed:" + details)
|
||||
return result
|
||||
|
||||
|
||||
def fit_o12_session(
|
||||
source_urdf: str | Path,
|
||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
*,
|
||||
cross_view_records_by_joint: Mapping[
|
||||
str, Sequence[Mapping[str, Any]]
|
||||
] | None = None,
|
||||
require_cross_view: bool = False,
|
||||
require_thumb_root_spatial_zero: bool = False,
|
||||
require_full_hand_spatial_zero: bool = False,
|
||||
) -> O12FitResult:
|
||||
del source_urdf # topology is validated by the immutable product loader
|
||||
# SDK feedback supplies the continuous radian input domain; Tag relative
|
||||
# rotation supplies the corresponding physical URDF coordinate and the
|
||||
# shared G20 geometry kernel solves observable static thumb phase.
|
||||
expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
|
||||
if set(records_by_joint) != expected:
|
||||
missing = sorted(expected - set(records_by_joint))
|
||||
@@ -104,16 +442,40 @@ def fit_o12_session(
|
||||
curves: dict[str, JointCurveFit] = {}
|
||||
holdout: dict[str, tuple[float, ...]] = {}
|
||||
cycle_curves: dict[str, dict[int, JointCurveFit]] = {}
|
||||
feedback_domains = {
|
||||
joint: _measured_feedback_domain(joint, records_by_joint[joint])
|
||||
for joint in expected
|
||||
}
|
||||
virtual_by_joint = {
|
||||
joint: _virtual_records(
|
||||
joint,
|
||||
records_by_joint[joint],
|
||||
feedback_domains[joint],
|
||||
feedback_domains,
|
||||
)
|
||||
for joint in expected
|
||||
}
|
||||
for joint in sorted(expected):
|
||||
rows = _virtual_records(joint, records_by_joint[joint])
|
||||
rows = virtual_by_joint[joint]
|
||||
_validate_cycle_repeatability(joint, rows, feedback_domains[joint])
|
||||
training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}]
|
||||
validation = [row for row in rows if int(row["cycle"]) == 3]
|
||||
if not training or not validation:
|
||||
raise ValueError(f"{joint} is missing training or holdout records")
|
||||
fit = fit_rotation_joint_curve(
|
||||
training, zero_command_u8=255, canonical_zero_direction="decreasing"
|
||||
training,
|
||||
zero_command_u8=255,
|
||||
canonical_zero_direction="decreasing",
|
||||
require_observed_domain_endpoints=False,
|
||||
zero_reference_maximum_distance_u8=None,
|
||||
)
|
||||
errors = rotation_curve_holdout_errors(fit, validation, zero_command_u8=255)
|
||||
frame_errors = rotation_curve_holdout_errors(
|
||||
fit,
|
||||
validation,
|
||||
zero_command_u8=255,
|
||||
zero_reference_maximum_distance_u8=None,
|
||||
)
|
||||
errors = _uniform_curve_holdout_errors(validation, frame_errors)
|
||||
absolute = np.abs(np.asarray(errors, dtype=float))
|
||||
if (
|
||||
float(np.mean(absolute)) > math.radians(1.0)
|
||||
@@ -121,8 +483,6 @@ def fit_o12_session(
|
||||
or float(np.max(absolute)) > math.radians(3.0)
|
||||
):
|
||||
raise ValueError(f"{joint} isolated holdout failed")
|
||||
if fit.maximum_hysteresis_rad > math.radians(2.0):
|
||||
raise ValueError(f"{joint} hysteresis exceeds 2 degrees")
|
||||
curves[joint] = fit
|
||||
holdout[joint] = tuple(float(value) for value in errors)
|
||||
cycle_curves[joint] = {
|
||||
@@ -130,6 +490,8 @@ def fit_o12_session(
|
||||
[row for row in training if int(row["cycle"]) == cycle],
|
||||
zero_command_u8=255,
|
||||
canonical_zero_direction="decreasing",
|
||||
require_observed_domain_endpoints=False,
|
||||
zero_reference_maximum_distance_u8=None,
|
||||
)
|
||||
for cycle in (0, 1, 2)
|
||||
}
|
||||
@@ -151,31 +513,176 @@ def fit_o12_session(
|
||||
target,
|
||||
curves[source],
|
||||
curves[target],
|
||||
model="quadratic_runtime",
|
||||
# Retain the Tag-derived coupling as an independent diagnostic.
|
||||
# Deployment uses the profile-declared vendor O12 polynomial.
|
||||
model="direction_aware_knots",
|
||||
cycle_curve_pairs=cycle_pairs,
|
||||
minimum_multiplier=0.5,
|
||||
maximum_multiplier=2.2,
|
||||
)
|
||||
|
||||
cross_view_rows = {
|
||||
str(name): tuple(values)
|
||||
for name, values in (cross_view_records_by_joint or {}).items()
|
||||
}
|
||||
required_roll = {"middle_mcp_roll", "index_mcp_roll"}
|
||||
if require_cross_view and set(cross_view_rows) != required_roll:
|
||||
raise ValueError(
|
||||
"O12 roll cross-view records are incomplete: "
|
||||
f"missing={sorted(required_roll - set(cross_view_rows))} "
|
||||
f"extra={sorted(set(cross_view_rows) - required_roll)}"
|
||||
)
|
||||
cross_view_metrics: dict[str, dict[str, float]] = {}
|
||||
for joint, source_rows in sorted(cross_view_rows.items()):
|
||||
if joint not in required_roll:
|
||||
raise ValueError(f"unexpected O12 roll cross-view joint: {joint}")
|
||||
rows = _virtual_records(
|
||||
joint,
|
||||
source_rows,
|
||||
feedback_domains[joint],
|
||||
feedback_domains,
|
||||
)
|
||||
training = [row for row in rows if int(row["cycle"]) in {0, 1, 2}]
|
||||
validation = [row for row in rows if int(row["cycle"]) == 3]
|
||||
if not training or not validation:
|
||||
raise ValueError(f"{joint} side view lacks training or holdout records")
|
||||
secondary = fit_rotation_joint_curve(
|
||||
training,
|
||||
zero_command_u8=255,
|
||||
canonical_zero_direction="decreasing",
|
||||
require_observed_domain_endpoints=False,
|
||||
zero_reference_maximum_distance_u8=None,
|
||||
)
|
||||
frame_errors = rotation_curve_holdout_errors(
|
||||
secondary,
|
||||
validation,
|
||||
zero_command_u8=255,
|
||||
zero_reference_maximum_distance_u8=None,
|
||||
)
|
||||
errors = _uniform_curve_holdout_errors(validation, frame_errors)
|
||||
absolute = np.abs(np.asarray(errors, dtype=float))
|
||||
if (
|
||||
float(np.mean(absolute)) > math.radians(1.0)
|
||||
or float(np.percentile(absolute, 95.0)) > math.radians(2.0)
|
||||
or float(np.max(absolute)) > math.radians(3.0)
|
||||
):
|
||||
raise ValueError(f"{joint} side-view isolated holdout failed")
|
||||
metrics = cross_view_roll_diagnostic_metrics(curves[joint], secondary)
|
||||
if metrics.get("direction_disagrees", 0.0):
|
||||
raise ValueError(f"{joint} cross-view roll direction disagrees")
|
||||
metrics.update({
|
||||
"holdout_mae_rad": float(np.mean(absolute)),
|
||||
"holdout_p95_rad": float(np.percentile(absolute, 95.0)),
|
||||
"holdout_max_rad": float(np.max(absolute)),
|
||||
"maximum_hysteresis_rad": float(
|
||||
secondary.maximum_hysteresis_rad
|
||||
),
|
||||
"sample_count": float(len(rows)),
|
||||
})
|
||||
cross_view_metrics[joint] = metrics
|
||||
|
||||
visual_arcs = {joint: _travel(curves[joint]) for joint in expected}
|
||||
travels = {
|
||||
joint: _travel(curves[joint]) for joint in CALIBRATED_ACTIVE_JOINTS
|
||||
joint: visual_arcs[joint] for joint in CALIBRATED_ACTIVE_JOINTS
|
||||
}
|
||||
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
|
||||
travels[target] = travels[donor]
|
||||
# O12 active feedback is already a physical angle with open/centred zero.
|
||||
# Calibration publishes the visual transfer curves and measured travel;
|
||||
# it must not invent a static offset from an arbitrary Tag mounting angle.
|
||||
offsets = {joint: 0.0 for joint in travels}
|
||||
return O12FitResult(
|
||||
offsets = {name: 0.0 for name in SDK_TO_URDF_JOINT}
|
||||
zero_methods = {name: "source_cad_zero_not_measured" for name in offsets}
|
||||
for name in STATIC_ZERO_EXCLUDED_JOINTS:
|
||||
zero_methods[name] = "source_cad_zero_profile_excluded"
|
||||
if (
|
||||
require_thumb_root_spatial_zero
|
||||
and not _has_complete_thumb_root_geometry(records_by_joint)
|
||||
):
|
||||
raise ValueError(
|
||||
"O12 thumb root absolute zero requires common-frame roll, yaw, pitch "
|
||||
"and palm-orientation axis trajectories"
|
||||
)
|
||||
thumb_root_zero_result = None
|
||||
full_hand_zero_result = None
|
||||
spatial_error = None
|
||||
if require_full_hand_spatial_zero:
|
||||
from .zero import AXIS_JOINTS, O12SpatialZeroError, motor_index, solve_full_hand_zero
|
||||
required = {"relative_translation_xyz_m", "parent_pose_common", "child_pose_common",
|
||||
"view_normal_common_xyz", "camera_center_common_xyz_m", "state_rad"}
|
||||
missing = [name for name in AXIS_JOINTS if not records_by_joint.get(name)
|
||||
or any(not required.issubset(row) for row in records_by_joint[name])]
|
||||
if missing:
|
||||
raise O12SpatialZeroError(
|
||||
"O12 full-hand spatial zero requires pose observations:" + ",".join(missing),
|
||||
{"passed": False, "stage": "missing_geometry", "joints": missing},
|
||||
)
|
||||
# G20's geometry kernel uses a normalized 256-entry coordinate, not
|
||||
# literal u8 motor commands. Rebase that private curve at SDK feedback
|
||||
# zero and apply the same sign contract as the public radian mapper.
|
||||
solver_curves = {}
|
||||
for name in AXIS_JOINTS:
|
||||
fit = curves[name]
|
||||
domain = feedback_domains[name]
|
||||
baseline = feedback_rad_to_curve_index(name, 0., domain)
|
||||
lo = feedback_rad_to_curve_index(name, domain[0], domain)
|
||||
hi = feedback_rad_to_curve_index(name, domain[1], domain)
|
||||
values = np.asarray(fit.angle_rad, dtype=float)
|
||||
direction = np.sign(np.interp(hi, np.arange(256), values)
|
||||
- np.interp(lo, np.arange(256), values))
|
||||
multiplier = 1. if direction == SDK_TO_URDF_SIGN[motor_index(name)] else -1.
|
||||
fields = {}
|
||||
for field in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||
values = np.asarray(getattr(fit, field), dtype=float)
|
||||
fields[field] = tuple(multiplier * (values - np.interp(baseline, np.arange(256), values)))
|
||||
solver_curves[name] = replace(fit, **fields)
|
||||
try:
|
||||
full_hand_zero_result = solve_full_hand_zero(
|
||||
source_urdf,
|
||||
{name: _virtual_records(name, records_by_joint[name], feedback_domains[name], feedback_domains)
|
||||
for name in AXIS_JOINTS},
|
||||
solver_curves,
|
||||
)
|
||||
except O12SpatialZeroError as error:
|
||||
if "result" not in error.diagnostics:
|
||||
raise # Missing/unobservable geometry has no review estimate.
|
||||
full_hand_zero_result = ZeroSolveResult(**error.diagnostics["result"])
|
||||
spatial_error = error
|
||||
for name in GEOMETRIC_ZERO_JOINTS:
|
||||
offsets[name] = float(full_hand_zero_result.direct_offsets_rad[name])
|
||||
zero_methods[name] = "urdf_serial_axis_geometry"
|
||||
elif _has_complete_thumb_root_geometry(records_by_joint):
|
||||
thumb_root_zero_result = _fit_thumb_root_zero(
|
||||
source_urdf,
|
||||
records_by_joint,
|
||||
curves,
|
||||
feedback_domains,
|
||||
)
|
||||
for name in ROOT_GEOMETRIC_ZERO_JOINTS:
|
||||
offsets[name] = float(thumb_root_zero_result.direct_offsets_rad[name])
|
||||
zero_methods[name] = "urdf_serial_axis_geometry"
|
||||
# Transfer the scalar correction, never the donor's origin transform.
|
||||
# The writer composes it with the ring's own CAD joint frame.
|
||||
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
|
||||
offsets[target] = offsets[donor]
|
||||
zero_methods[target] = "transferred_static_zero_on_own_cad"
|
||||
result = O12FitResult(
|
||||
curves=curves,
|
||||
zero_offsets_rad=offsets,
|
||||
travels_rad=travels,
|
||||
mimic_fits=mimic_fits,
|
||||
holdout_errors_rad=holdout,
|
||||
cross_view_roll_metrics=cross_view_metrics,
|
||||
visual_arc_diagnostics_rad=visual_arcs,
|
||||
feedback_domains_rad=feedback_domains,
|
||||
zero_method_by_joint=zero_methods,
|
||||
full_hand_zero_result=full_hand_zero_result,
|
||||
thumb_root_zero_result=thumb_root_zero_result,
|
||||
)
|
||||
if spatial_error is not None:
|
||||
spatial_error.review_fit = result
|
||||
raise spatial_error
|
||||
return result
|
||||
|
||||
|
||||
__all__ = [
|
||||
"O12FitResult", "curve_input_knots_rad", "curve_values_at_rad",
|
||||
"feedback_rad_to_curve_index", "fit_o12_session",
|
||||
"O12FitResult", "O12_THUMB_ROOT_AXIS_JOINTS",
|
||||
"curve_input_knots_rad", "curve_values_at_rad",
|
||||
"feedback_rad_to_curve_index", "fit_o12_session", "measured_curve_bounds",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,133 @@
|
||||
"""O12-specific interpretation of the vendor motor error report.
|
||||
|
||||
The Pro2025 SDK documents ``commu_except`` as a possibly historical flag. It
|
||||
must therefore be interpreted together with fresh command-triggered feedback;
|
||||
the other four flags are active motor faults and are never downgraded.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
ERROR_STALLED = 1 << 0
|
||||
ERROR_OVERHEAT = 1 << 1
|
||||
ERROR_OVER_CURRENT = 1 << 2
|
||||
ERROR_MOTOR_EXCEPTION = 1 << 3
|
||||
ERROR_COMMUNICATION_EXCEPTION = 1 << 4
|
||||
ACTIVE_MOTOR_FAULT_MASK = (
|
||||
ERROR_STALLED
|
||||
| ERROR_OVERHEAT
|
||||
| ERROR_OVER_CURRENT
|
||||
| ERROR_MOTOR_EXCEPTION
|
||||
)
|
||||
DOCUMENTED_ERROR_MASK = ACTIVE_MOTOR_FAULT_MASK | ERROR_COMMUNICATION_EXCEPTION
|
||||
|
||||
ERROR_BIT_NAMES = (
|
||||
"stalled",
|
||||
"overheat",
|
||||
"over_current",
|
||||
"motor_except",
|
||||
"commu_except",
|
||||
)
|
||||
|
||||
HISTORICAL_COMMUNICATION_MINIMUM_REPORTS = 3
|
||||
HISTORICAL_COMMUNICATION_CONFIRM_SECONDS = 1.5
|
||||
HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS = 0.25
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class O12ErrorAssessment:
|
||||
codes: tuple[int, ...]
|
||||
valid: bool
|
||||
active_fault_channels: tuple[int, ...]
|
||||
communication_channels: tuple[int, ...]
|
||||
unknown_bit_channels: tuple[int, ...]
|
||||
|
||||
@property
|
||||
def clear(self) -> bool:
|
||||
return self.valid and not any(self.codes)
|
||||
|
||||
@property
|
||||
def communication_only(self) -> bool:
|
||||
return bool(
|
||||
self.valid
|
||||
and self.communication_channels
|
||||
and not self.active_fault_channels
|
||||
and not self.unknown_bit_channels
|
||||
)
|
||||
|
||||
|
||||
def assess_o12_error_report(values) -> O12ErrorAssessment:
|
||||
"""Decode one complete 12-motor O12 report without hiding any bit."""
|
||||
try:
|
||||
codes = tuple(int(value) for value in values)
|
||||
except (TypeError, ValueError):
|
||||
codes = ()
|
||||
valid = len(codes) == 12 and all(0 <= value <= 0xFFFF for value in codes)
|
||||
if not valid:
|
||||
return O12ErrorAssessment(codes, False, (), (), ())
|
||||
return O12ErrorAssessment(
|
||||
codes=codes,
|
||||
valid=True,
|
||||
active_fault_channels=tuple(
|
||||
index for index, value in enumerate(codes)
|
||||
if value & ACTIVE_MOTOR_FAULT_MASK
|
||||
),
|
||||
communication_channels=tuple(
|
||||
index for index, value in enumerate(codes)
|
||||
if value & ERROR_COMMUNICATION_EXCEPTION
|
||||
),
|
||||
unknown_bit_channels=tuple(
|
||||
index for index, value in enumerate(codes)
|
||||
if value & ~DOCUMENTED_ERROR_MASK
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def decoded_faults(codes: tuple[int, ...]) -> tuple[dict[str, object], ...]:
|
||||
return tuple(
|
||||
{
|
||||
"motor_index": index,
|
||||
"code": int(value),
|
||||
"flags": tuple(
|
||||
name for bit, name in enumerate(ERROR_BIT_NAMES)
|
||||
if int(value) & (1 << bit)
|
||||
),
|
||||
}
|
||||
for index, value in enumerate(codes)
|
||||
if value
|
||||
)
|
||||
|
||||
|
||||
def historical_communication_latch_confirmed(
|
||||
*,
|
||||
assessment: O12ErrorAssessment,
|
||||
matching_report_count: int,
|
||||
observation_seconds: float,
|
||||
feedback_hz: float,
|
||||
feedback_age_seconds: float,
|
||||
minimum_feedback_hz: float,
|
||||
) -> bool:
|
||||
"""Require repeated reports and a fresh full-rate control feedback stream."""
|
||||
return bool(
|
||||
assessment.communication_only
|
||||
and matching_report_count >= HISTORICAL_COMMUNICATION_MINIMUM_REPORTS
|
||||
and observation_seconds >= HISTORICAL_COMMUNICATION_CONFIRM_SECONDS
|
||||
and feedback_hz >= minimum_feedback_hz
|
||||
and 0.0 <= feedback_age_seconds <= HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ACTIVE_MOTOR_FAULT_MASK",
|
||||
"ERROR_BIT_NAMES",
|
||||
"ERROR_COMMUNICATION_EXCEPTION",
|
||||
"HEALTH_FEEDBACK_MAXIMUM_AGE_SECONDS",
|
||||
"HISTORICAL_COMMUNICATION_CONFIRM_SECONDS",
|
||||
"HISTORICAL_COMMUNICATION_MINIMUM_REPORTS",
|
||||
"O12ErrorAssessment",
|
||||
"assess_o12_error_report",
|
||||
"decoded_faults",
|
||||
"historical_communication_latch_confirmed",
|
||||
]
|
||||
@@ -0,0 +1,93 @@
|
||||
"""O12 vendor kinematics used by calibration and URDF publication.
|
||||
|
||||
The O12 SDK exposes twelve active joint coordinates but the hand URDF has
|
||||
nineteen revolute joints. The seven passive coordinates are part of the
|
||||
vendor solver contract; they are not independent quantities that may be
|
||||
replaced by an arbitrary linear fit of a planar AprilTag pose.
|
||||
|
||||
The coefficients below are the public ``OmniHandPro2025Solver`` coefficients
|
||||
from the protected SDK input. Keeping this small, deterministic evaluator in
|
||||
the calibration package makes online and offline publication identical even
|
||||
when the binary Python wheel is only available inside the vendor overlay.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Mapping, Sequence
|
||||
|
||||
|
||||
# Polynomial order is constant, x, x**2, x**3, x**4. The input is the SDK
|
||||
# active-joint feedback in radians. Right-thumb MCP/PIP feedback is negative,
|
||||
# hence its passive DIP result is negative too; ``sdk_to_urdf_sign`` converts
|
||||
# both to the positive target-URDF convention.
|
||||
PASSIVE_POLYNOMIAL_BY_JOINT: Mapping[str, tuple[float, ...]] = {
|
||||
"thumb_dip": (0.0, 0.6359, -0.3539, -0.3066, -0.1240),
|
||||
"index_dip": (0.0, 1.0630, 0.08942, 0.1845, -0.2169),
|
||||
"middle_dip": (0.0, 1.1490, -0.2581, 0.6033, -0.3371),
|
||||
"ring_pip": (0.0, 0.7869, 0.3884, -0.4545, 0.1578),
|
||||
"ring_dip": (0.0, 0.8990, 0.3138, -0.1728, -0.03666),
|
||||
"pinky_pip": (0.0, 0.7869, 0.3884, -0.4545, 0.1578),
|
||||
"pinky_dip": (0.0, 0.8990, 0.3138, -0.1728, -0.03666),
|
||||
}
|
||||
|
||||
# Polynomial inputs are active SDK coordinates, not necessarily the immediate
|
||||
# linear URDF mimic source. In particular ring/pinky DIP is solved directly
|
||||
# from the single active MCP coordinate by the vendor implementation.
|
||||
PASSIVE_SDK_SOURCE_BY_JOINT: Mapping[str, str] = {
|
||||
"thumb_dip": "thumb_mcp",
|
||||
"index_dip": "index_pip",
|
||||
"middle_dip": "middle_pip",
|
||||
"ring_pip": "ring_mcp_pitch",
|
||||
"ring_dip": "ring_mcp_pitch",
|
||||
"pinky_pip": "pinky_mcp_pitch",
|
||||
"pinky_dip": "pinky_mcp_pitch",
|
||||
}
|
||||
|
||||
|
||||
def evaluate_vendor_passive_joint(
|
||||
joint: str,
|
||||
sdk_feedback_rad: float,
|
||||
*,
|
||||
sdk_to_urdf_sign: float,
|
||||
) -> float:
|
||||
"""Evaluate one passive O12 joint in the target URDF convention."""
|
||||
try:
|
||||
coefficients = PASSIVE_POLYNOMIAL_BY_JOINT[str(joint)]
|
||||
except KeyError as error:
|
||||
raise ValueError(f"unknown O12 passive joint: {joint}") from error
|
||||
x = float(sdk_feedback_rad)
|
||||
sign = float(sdk_to_urdf_sign)
|
||||
if not math.isfinite(x) or sign not in {-1.0, 1.0}:
|
||||
raise ValueError("O12 vendor passive input/sign is invalid")
|
||||
value = 0.0
|
||||
power = 1.0
|
||||
for coefficient in coefficients:
|
||||
value += float(coefficient) * power
|
||||
power *= x
|
||||
result = sign * value
|
||||
if not math.isfinite(result):
|
||||
raise ValueError(f"O12 vendor passive result is invalid: {joint}")
|
||||
return result
|
||||
|
||||
|
||||
def vendor_passive_curve(
|
||||
joint: str,
|
||||
sdk_feedback_rad: Sequence[float],
|
||||
*,
|
||||
sdk_to_urdf_sign: float,
|
||||
) -> tuple[float, ...]:
|
||||
return tuple(
|
||||
evaluate_vendor_passive_joint(
|
||||
joint, value, sdk_to_urdf_sign=sdk_to_urdf_sign
|
||||
)
|
||||
for value in sdk_feedback_rad
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"PASSIVE_POLYNOMIAL_BY_JOINT",
|
||||
"PASSIVE_SDK_SOURCE_BY_JOINT",
|
||||
"evaluate_vendor_passive_joint",
|
||||
"vendor_passive_curve",
|
||||
]
|
||||
@@ -31,6 +31,62 @@ def cosine_position_trajectory_rad(
|
||||
)
|
||||
|
||||
|
||||
def cosine_ramp_velocity_trajectory_rad(
|
||||
start_rad: float,
|
||||
target_rad: float,
|
||||
elapsed_seconds: float,
|
||||
maximum_speed_rad_s: float,
|
||||
ramp_seconds: float,
|
||||
) -> tuple[float, float, float]:
|
||||
"""Velocity-limited trajectory with cosine ramps and a constant-speed core."""
|
||||
speed = float(maximum_speed_rad_s)
|
||||
ramp = float(ramp_seconds)
|
||||
if not math.isfinite(speed) or speed <= 0.0:
|
||||
raise ValueError("maximum_speed_rad_s must be positive")
|
||||
if not math.isfinite(ramp) or ramp <= 0.0:
|
||||
raise ValueError("ramp_seconds must be positive")
|
||||
start = float(start_rad)
|
||||
target = float(target_rad)
|
||||
distance = abs(target - start)
|
||||
if distance <= 0.0:
|
||||
return target, 1.0, 0.0
|
||||
|
||||
# For very short moves there is no room for a constant-speed section;
|
||||
# retain the bounded position-cosine trajectory.
|
||||
if distance <= speed * ramp:
|
||||
return cosine_position_trajectory_rad(
|
||||
start, target, elapsed_seconds, speed
|
||||
)
|
||||
|
||||
cruise_seconds = distance / speed - ramp
|
||||
duration = 2.0 * ramp + cruise_seconds
|
||||
elapsed = min(duration, max(0.0, float(elapsed_seconds)))
|
||||
ramp_distance = 0.5 * speed * ramp
|
||||
if elapsed < ramp:
|
||||
travelled = speed * (
|
||||
0.5 * elapsed
|
||||
- ramp * math.sin(math.pi * elapsed / ramp) / (2.0 * math.pi)
|
||||
)
|
||||
elif elapsed < ramp + cruise_seconds:
|
||||
travelled = ramp_distance + speed * (elapsed - ramp)
|
||||
else:
|
||||
down = elapsed - ramp - cruise_seconds
|
||||
travelled = (
|
||||
ramp_distance
|
||||
+ speed * cruise_seconds
|
||||
+ speed * (
|
||||
0.5 * down
|
||||
+ ramp * math.sin(math.pi * down / ramp) / (2.0 * math.pi)
|
||||
)
|
||||
)
|
||||
fraction = min(1.0, max(0.0, travelled / distance))
|
||||
return (
|
||||
start + (target - start) * fraction,
|
||||
min(1.0, max(0.0, elapsed / duration)),
|
||||
duration,
|
||||
)
|
||||
|
||||
|
||||
def build_calibration_motion_command(
|
||||
task: TaskSpec,
|
||||
command_rad: float,
|
||||
@@ -73,4 +129,5 @@ def build_calibration_return_waypoints(
|
||||
__all__ = [
|
||||
"build_calibration_motion_command", "build_calibration_preparation_waypoints",
|
||||
"build_calibration_return_waypoints", "cosine_position_trajectory_rad",
|
||||
"cosine_ramp_velocity_trajectory_rad",
|
||||
]
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,139 @@
|
||||
"""Offline thumb candidate resolution using training data only.
|
||||
|
||||
Re-selects actual corner-derived IPPE solutions; does not manufacture rigid
|
||||
poses, substitute SDK angles, or relax the spatial solver's acceptance gates.
|
||||
"""
|
||||
from itertools import product
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
from ...pnp import solve_square_tag_ippe
|
||||
from .pnp import THUMB_ROLES
|
||||
|
||||
POLICY = 'o12_thumb_training_candidates_v1'
|
||||
MATRIX_SOURCE = 'CameraInfo.P[:3,:3]'
|
||||
|
||||
|
||||
def _rotation(pose):
|
||||
return R.from_quat(pose['quaternion_xyzw'])
|
||||
|
||||
|
||||
def _relative(parent, child):
|
||||
rp = _rotation(parent)
|
||||
return ((rp.inv()*_rotation(child)).as_quat(),
|
||||
rp.inv().apply(np.asarray(child['translation_xyz_m'])-parent['translation_xyz_m']))
|
||||
|
||||
|
||||
def _trajectory_score(parent, child):
|
||||
pairs = [_relative(a, b) for a, b in zip(parent, child)]
|
||||
rotations = R.from_quat([q for q, _ in pairs])
|
||||
points = np.asarray([p for _, p in pairs])
|
||||
vectors = (rotations*rotations[0].inv()).as_rotvec()
|
||||
axis = np.linalg.svd(vectors, full_matrices=False)[2][0]
|
||||
projected = R.from_rotvec((vectors@axis)[:, None]*axis)
|
||||
off_axis = float(np.sqrt(np.mean((projected.inv()*rotations*rotations[0].inv()).magnitude()**2)))
|
||||
# Free six-parameter fixed-pivot fit, no CAD zero or mounting-angle prior.
|
||||
a = np.concatenate((np.tile(np.eye(3), (len(points), 1, 1)), rotations.as_matrix()), axis=2).reshape(-1, 6)
|
||||
predicted = (a@np.linalg.lstsq(a, points.ravel(), rcond=None)[0]).reshape(-1, 3)
|
||||
rms = float(np.sqrt(np.mean(np.sum((points-predicted)**2, axis=1))))
|
||||
return {'pivot_rms_m': rms, 'off_axis_rms_rad': off_axis,
|
||||
'score': rms/.0015 + off_axis/math.radians(1)}
|
||||
|
||||
|
||||
def resolve_thumb_observations(records, *, projection_override=None):
|
||||
"""Return copied observations and auditable candidate-selection evidence.
|
||||
|
||||
Older captures did not save P. They remain on the legacy path; K is never
|
||||
guessed to be P. An explicit override is for isolated offline diagnosis
|
||||
and must be reported as such, not advertised as a verified whole session.
|
||||
"""
|
||||
rows = [dict(r) for r in records]
|
||||
joint_rows = [r for r in rows if r.get('kind') == 'o12_joint_sample'
|
||||
and r.get('task_name') == 'thumb_mcp_dip_front']
|
||||
evidence = {int(r['image_stamp_ns']): r for r in rows
|
||||
if r.get('kind') == 'o12_pnp_candidate_frame'
|
||||
and r.get('task_name') == 'thumb_mcp_dip_front'}
|
||||
report = {'policy': POLICY, 'training_cycles': [0, 1, 2], 'holdout_cycle': 3,
|
||||
'projection_override_used': projection_override is not None,
|
||||
'is_accuracy_certificate': False}
|
||||
if not evidence:
|
||||
return rows, {**report, 'status': 'legacy_no_corners'}
|
||||
if projection_override is None and any(
|
||||
r.get('camera_matrix_source') != MATRIX_SOURCE for r in evidence.values()):
|
||||
return rows, {**report, 'status': 'legacy_projection_unverified',
|
||||
'reason': 'Recorded matrix may be raw K; no silent K-to-P substitution.'}
|
||||
# Use only the same accepted attempt as the downstream fitter.
|
||||
latest = {}
|
||||
for r in joint_rows:
|
||||
key = (int(r['cycle']), r['direction'])
|
||||
latest[key] = max(latest.get(key, 0), int(r.get('attempt', 1)))
|
||||
chosen_rows = [r for r in joint_rows if int(r.get('attempt', 1)) == latest[(int(r['cycle']), r['direction'])]]
|
||||
stamps = sorted({int(r['image_stamp_ns']) for r in chosen_rows})
|
||||
if not stamps or any(s not in evidence for s in stamps):
|
||||
raise ValueError('O12 corner replay lacks evidence for accepted observations')
|
||||
training = np.asarray([int(evidence[s]['cycle']) in (0, 1, 2) for s in stamps])
|
||||
if sum(training) < 40 or not any(int(evidence[s]['cycle']) == 3 for s in stamps):
|
||||
raise ValueError('O12 corner replay requires training and independent holdout')
|
||||
if np.flatnonzero(training)[-1] > np.flatnonzero(~training)[0]:
|
||||
raise ValueError('O12 holdout must follow the complete training trajectory')
|
||||
paths = {role: [[], []] for role in THUMB_ROLES}
|
||||
for stamp in stamps:
|
||||
frame = evidence[stamp]
|
||||
matrix = np.asarray(frame['camera_matrix'] if projection_override is None else projection_override)
|
||||
for role in THUMB_ROLES:
|
||||
data = frame['roles'][role]
|
||||
poses = solve_square_tag_ippe(data['corners_xy'], tag_size_m=frame['tag_size_m'], camera_matrix=matrix)
|
||||
limit = float(data.get('maximum_reprojection_error_px', 1.5))
|
||||
poses = [p for p in poses if p.reprojection_error_px <= limit]
|
||||
if not poses:
|
||||
raise ValueError(f'O12 corner replay has no eligible pose:{stamp}:{role}')
|
||||
cs = [dict(quaternion_xyzw=list(p.quaternion_xyzw), translation_xyz_m=list(p.translation_xyz_m),
|
||||
reprojection_error_px=p.reprojection_error_px) for p in poses[:2]]
|
||||
if len(cs) == 1:
|
||||
cs.append(cs[0])
|
||||
if paths[role][0]:
|
||||
previous = [_rotation(paths[role][i][-1]) for i in range(2)]
|
||||
def cost(order):
|
||||
return sum((previous[i].inv()*_rotation(cs[order[i]])).magnitude() for i in range(2))
|
||||
order = min(((0, 1), (1, 0)), key=cost)
|
||||
cs = [cs[i] for i in order]
|
||||
for i, pose in enumerate(cs):
|
||||
paths[role][i].append(pose)
|
||||
scores = []
|
||||
for branches in product(range(2), repeat=3):
|
||||
train = [[p for p, keep in zip(paths[role][branch], training) if keep]
|
||||
for role, branch in zip(THUMB_ROLES, branches)]
|
||||
metrics = [_trajectory_score(train[i], train[i+1]) for i in (0, 1)]
|
||||
image_penalty = float(np.mean([p['reprojection_error_px'] for ps in train for p in ps]))
|
||||
scores.append({'branches': list(branches), 'score': sum(m['score'] for m in metrics)+.5*image_penalty,
|
||||
'training_pairs': metrics, 'mean_reprojection_px': image_penalty})
|
||||
selected = min(scores, key=lambda s: s['score'])
|
||||
lookup = {stamp: {role: paths[role][branch][i] for role, branch in zip(THUMB_ROLES, selected['branches'])}
|
||||
for i, stamp in enumerate(stamps)}
|
||||
changed = 0
|
||||
for row in chosen_rows:
|
||||
stamp = int(row['image_stamp_ns'])
|
||||
parent_role, child_role = THUMB_ROLES[:2] if row['joint'] == 'thumb_mcp' else THUMB_ROLES[1:]
|
||||
parent, child = lookup[stamp][parent_role], lookup[stamp][child_role]
|
||||
# Preserve the actual camera->common transform, not an assumed identity.
|
||||
old_camera_parent = evidence[stamp]['roles'][parent_role]['selected']
|
||||
if old_camera_parent is None:
|
||||
raise ValueError('Accepted joint row has no original selected parent pose')
|
||||
rc = _rotation(row['parent_pose_common'])*_rotation(old_camera_parent).inv()
|
||||
tc = np.asarray(row['parent_pose_common']['translation_xyz_m'])-rc.apply(old_camera_parent['translation_xyz_m'])
|
||||
quaternion, translation = _relative(parent, child)
|
||||
changed += int((_rotation({'quaternion_xyzw': row['relative_quaternion_xyzw']}).inv()*R.from_quat(quaternion)).magnitude() > 1e-7)
|
||||
row['relative_quaternion_xyzw'] = quaternion.tolist()
|
||||
row['relative_translation_xyz_m'] = translation.tolist()
|
||||
for field, pose in (('parent_pose_common', parent), ('child_pose_common', child)):
|
||||
row[field] = {'quaternion_xyzw': (rc*_rotation(pose)).as_quat().tolist(),
|
||||
'translation_xyz_m': (rc.apply(pose['translation_xyz_m'])+tc).tolist()}
|
||||
row['pnp_reprojection_error_px'] = max(parent['reprojection_error_px'], child['reprojection_error_px'])
|
||||
row['pose_selection_policy'] = POLICY
|
||||
if projection_override is not None:
|
||||
row['projection_reprocessing_scope'] = 'thumb_only_external_projection'
|
||||
return rows, {**report, 'status': 'resolved', 'selected_branches': selected['branches'],
|
||||
'hypotheses': scores, 'training_frames': int(sum(training)),
|
||||
'holdout_frames': int(sum(~training)), 'changed_joint_rows': changed}
|
||||
@@ -3,17 +3,30 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from dataclasses import asdict, replace
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
from ...product import sha256_file
|
||||
from ...runtime.engine import CalibrationEngine
|
||||
from ...storage import atomic_write_json
|
||||
from .artifacts import build_o12_runtime_payload
|
||||
from .artifacts import (
|
||||
build_o12_runtime_payload,
|
||||
validate_o12_runtime_payload_against_urdf,
|
||||
)
|
||||
from .fitting import O12FitResult, fit_o12_session
|
||||
from .profile import CALIBRATED_ACTIVE_JOINTS, MEASURED_PASSIVE_JOINTS
|
||||
from .profile import (
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
MEASURED_PASSIVE_JOINTS,
|
||||
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
build_typed_profile,
|
||||
)
|
||||
from .urdf import O12UrdfCorrection, write_o12_corrected_urdf
|
||||
from .zero import O12SpatialZeroError, SPATIAL_ZERO_POLICY
|
||||
|
||||
|
||||
MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
|
||||
@@ -54,6 +67,36 @@ def accepted_records_by_joint(
|
||||
return result
|
||||
|
||||
|
||||
def accepted_roll_cross_view_records(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
) -> dict[str, list[dict[str, Any]]]:
|
||||
"""Select the latest complete-attempt O12 side observations by roll joint."""
|
||||
samples = [
|
||||
dict(row) for row in records
|
||||
if row.get("kind") == "o12_roll_cross_view_sample"
|
||||
and str(row.get("model_joint", ""))
|
||||
in {"middle_mcp_roll", "index_mcp_roll"}
|
||||
]
|
||||
latest: dict[tuple[str, int, str], int] = {}
|
||||
for row in samples:
|
||||
key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"]))
|
||||
latest[key] = max(latest.get(key, 0), int(row.get("attempt", 1)))
|
||||
result = {"middle_mcp_roll": [], "index_mcp_roll": []}
|
||||
for row in samples:
|
||||
key = (str(row["task_name"]), int(row["cycle"]), str(row["direction"]))
|
||||
if int(row.get("attempt", 1)) != latest[key]:
|
||||
continue
|
||||
result[str(row["model_joint"])].append({
|
||||
"cycle": int(row["cycle"]),
|
||||
"direction": str(row["direction"]),
|
||||
"feedback_rad": float(row["feedback_rad"]),
|
||||
"relative_quaternion_xyzw": list(
|
||||
row["relative_quaternion_xyzw"]
|
||||
),
|
||||
})
|
||||
return {name: rows for name, rows in result.items() if rows}
|
||||
|
||||
|
||||
def load_o12_raw_samples(path: str | Path) -> list[dict[str, Any]]:
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
@@ -97,7 +140,92 @@ def finalize_o12_session(
|
||||
) -> tuple[dict[str, Any], O12FitResult, O12UrdfCorrection]:
|
||||
directory = Path(session_dir).expanduser().resolve()
|
||||
directory.mkdir(parents=True, exist_ok=True)
|
||||
result = fit_o12_session(source_urdf, accepted_records_by_joint(records))
|
||||
if any(row.get("projection_reprocessing_scope") == "thumb_only_external_projection"
|
||||
for row in records):
|
||||
raise ValueError("Partial external camera override is diagnostic-only; "
|
||||
"other task projections are unverified. Cannot finalize a whole-hand artifact.")
|
||||
# Reconsider ambiguous online choices with the complete TRAINING trajectory.
|
||||
# Fourth-cycle candidates are assigned using the frozen training choice;
|
||||
# the original raw file is never rewritten and all quality gates remain.
|
||||
from .observations import resolve_thumb_observations
|
||||
try:
|
||||
records, pose_selection = resolve_thumb_observations(records)
|
||||
except ValueError as error:
|
||||
atomic_write_json(directory / "pose_selection_diagnostics.json", {
|
||||
"status": "failed", "reason": str(error), "publication_allowed": False,
|
||||
})
|
||||
raise ValueError("O12 corner candidate resolution failed:" + str(error)) from error
|
||||
atomic_write_json(directory / "pose_selection_diagnostics.json", pose_selection)
|
||||
try:
|
||||
result = fit_o12_session(
|
||||
source_urdf,
|
||||
accepted_records_by_joint(records),
|
||||
cross_view_records_by_joint=accepted_roll_cross_view_records(records),
|
||||
require_cross_view=True,
|
||||
require_full_hand_spatial_zero=True,
|
||||
)
|
||||
if pose_selection['status'] == 'legacy_projection_unverified':
|
||||
# A successful fit cannot certify observations known to have an
|
||||
# unverified rectified-pixel projection contract. Keep a review
|
||||
# artifact, but never silently publish mixed K/P observations.
|
||||
spatial = replace(result.full_hand_zero_result, passed=False,
|
||||
failure_reasons={**result.full_hand_zero_result.failure_reasons,
|
||||
'camera_projection': 'legacy_projection_unverified'})
|
||||
error = O12SpatialZeroError(
|
||||
'O12 camera projection is unverified for legacy corner records; review only',
|
||||
{'passed': False, 'stage': 'camera_projection',
|
||||
'result': asdict(spatial), 'pose_selection': pose_selection})
|
||||
error.review_fit = replace(result, full_hand_zero_result=spatial)
|
||||
raise error
|
||||
except O12SpatialZeroError as error:
|
||||
# A review model is not a calibration PASS. Keep it out of the
|
||||
# publication directory's artifact set and never produce runtime JSON.
|
||||
if error.review_fit is not None:
|
||||
review = directory / "review_only"
|
||||
try:
|
||||
if any(not math.isfinite(value) or abs(value) > math.radians(20)
|
||||
for value in error.review_fit.zero_offsets_rad.values()):
|
||||
raise ValueError("review zero estimate is outside the physical correction budget")
|
||||
correction = write_o12_corrected_urdf(
|
||||
source_urdf=source_urdf, output_directory=review,
|
||||
serial_number=serial_number + "_REVIEW_ONLY",
|
||||
timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"),
|
||||
result=error.review_fit,
|
||||
)
|
||||
atomic_write_json(review / "review_manifest.json", {
|
||||
"status": "REVIEW_ONLY_NOT_CALIBRATION_PASS",
|
||||
"publication_allowed": False,
|
||||
"policy": SPATIAL_ZERO_POLICY,
|
||||
"urdf": correction.path.name,
|
||||
"source_urdf": str(Path(source_urdf).resolve()),
|
||||
"source_urdf_sha256": sha256_file(source_urdf),
|
||||
"candidate_urdf_sha256": sha256_file(correction.path),
|
||||
"protected_inputs": dict(protected_inputs),
|
||||
"static_origin_offsets_rad": dict(error.review_fit.zero_offsets_rad),
|
||||
"spatial_validation": error.diagnostics,
|
||||
"limitations": ["Not approved for hardware/control",
|
||||
"Passive URDF mimic is a linear approximation",
|
||||
"Fingertip contact accuracy not verified"],
|
||||
})
|
||||
error.diagnostics = {**error.diagnostics, "review_urdf": str(correction.path)}
|
||||
error.args = (str(error) + "; 仅供复核、未发布的 URDF：" + str(correction.path),)
|
||||
except Exception as review_error:
|
||||
# Preserve the original failure even if diagnostic export fails.
|
||||
error.diagnostics = {**error.diagnostics, "review_export_error": str(review_error)}
|
||||
atomic_write_json(directory / "spatial_zero_diagnostics.json", error.diagnostics)
|
||||
raise
|
||||
atomic_write_json(directory / "spatial_zero_diagnostics.json", {
|
||||
"passed": True, "policy": SPATIAL_ZERO_POLICY,
|
||||
"result": asdict(result.full_hand_zero_result),
|
||||
"static_zero_exclusions": {
|
||||
name: "immutable_source_cad"
|
||||
for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS)
|
||||
},
|
||||
})
|
||||
CalibrationEngine(build_typed_profile()).result_from_fit(
|
||||
result,
|
||||
transfers=TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
)
|
||||
payload = build_o12_runtime_payload(
|
||||
serial_number=serial_number,
|
||||
source_urdf=source_urdf,
|
||||
@@ -105,6 +233,26 @@ def finalize_o12_session(
|
||||
protected_inputs=protected_inputs,
|
||||
passed=True,
|
||||
)
|
||||
resume_rows = [
|
||||
row for row in records
|
||||
if row.get("kind") == "o12_resume_checkpoint_import"
|
||||
]
|
||||
resume = (
|
||||
{
|
||||
"used": True,
|
||||
"source_session": str(resume_rows[-1]["source_session"]),
|
||||
"compatibility": str(resume_rows[-1]["compatibility"]),
|
||||
"completed_unit_count": int(
|
||||
resume_rows[-1]["completed_unit_count"]
|
||||
),
|
||||
"completed_task_keys": list(
|
||||
resume_rows[-1].get("completed_task_keys", ())
|
||||
),
|
||||
}
|
||||
if resume_rows
|
||||
else {"used": False}
|
||||
)
|
||||
payload["quality"]["resume"] = resume
|
||||
correction = write_o12_corrected_urdf(
|
||||
source_urdf=source_urdf,
|
||||
output_directory=directory,
|
||||
@@ -112,6 +260,9 @@ def finalize_o12_session(
|
||||
result=result,
|
||||
timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"),
|
||||
)
|
||||
validate_o12_runtime_payload_against_urdf(
|
||||
payload, correction.path, source_urdf=source_urdf
|
||||
)
|
||||
json_path = directory / f"o12_right_{serial_number}_calibration.json"
|
||||
atomic_write_json(json_path, payload)
|
||||
summary = {
|
||||
@@ -119,13 +270,52 @@ def finalize_o12_session(
|
||||
"profile_id": "O12/right/o12_right_16/v1",
|
||||
"serial_number": str(serial_number),
|
||||
"result": "PASS",
|
||||
"calibration_scope": payload["calibration_scope"],
|
||||
"spatial_zero_policy": SPATIAL_ZERO_POLICY,
|
||||
"static_origin_offsets_rad": dict(result.zero_offsets_rad),
|
||||
"static_zero_methods": dict(result.zero_method_by_joint),
|
||||
"static_zero_exclusions": {
|
||||
name: {
|
||||
"static_origin_offset_rad": result.zero_offsets_rad[name],
|
||||
"source": "immutable_source_cad",
|
||||
"dynamic_curve_and_holdout_passed": True,
|
||||
}
|
||||
for name in sorted(STATIC_ZERO_EXCLUDED_JOINTS)
|
||||
},
|
||||
"measured_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS),
|
||||
"measured_passive_joints": sorted(MEASURED_PASSIVE_JOINTS),
|
||||
"thumb_static_origin_offsets_rad": {
|
||||
name: correction.origin_offsets_rad[name]
|
||||
for name in (
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
)
|
||||
},
|
||||
"thumb_static_zero_methods": {
|
||||
name: result.zero_method_by_joint[name]
|
||||
for name in (
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
)
|
||||
},
|
||||
"mechanical_endpoint_origin_offsets_rad": {
|
||||
name: correction.origin_offsets_rad[name]
|
||||
for name in sorted(build_typed_profile().zero.endpoint_anchor_by_joint)
|
||||
},
|
||||
"corrected_limits_rad": {
|
||||
name: list(values)
|
||||
for name, values in sorted(correction.corrected_limits_rad.items())
|
||||
},
|
||||
"ring_transfer": {
|
||||
"source": "pinky_mcp_pitch",
|
||||
"target": "ring_mcp_pitch",
|
||||
"preserved_fields": list(correction.preserved_ring_fields),
|
||||
},
|
||||
"resume": resume,
|
||||
"artifacts": {
|
||||
"json": json_path.name,
|
||||
"urdf": correction.path.name,
|
||||
@@ -140,6 +330,7 @@ def finalize_o12_session(
|
||||
|
||||
|
||||
__all__ = [
|
||||
"MEASURED_JOINTS", "accepted_records_by_joint", "finalize_o12_session",
|
||||
"MEASURED_JOINTS", "accepted_records_by_joint",
|
||||
"accepted_roll_cross_view_records", "finalize_o12_session",
|
||||
"load_o12_raw_samples",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
"""O12 thumb-chain branch selection; never alters a measured pose or angle."""
|
||||
import math
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from ...pnp import SquareTagGroupPoseTracker
|
||||
|
||||
|
||||
THUMB_ROLES = ('thumb_cmc', 'thumb_mcp', 'thumb_dip')
|
||||
|
||||
|
||||
class O12ThumbPoseTracker(SquareTagGroupPoseTracker):
|
||||
"""Use parallel hinge directions, not a CAD zero or passive angle ratio.
|
||||
|
||||
Both relative rotations live in different marker frames. Transport the
|
||||
follower's rotation vector through the *reference* parent rotation before
|
||||
comparing axes. Fixed arbitrary marker mounting rotations cancel out.
|
||||
This is a soft ambiguity discriminator, not a new capture/stop gate.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__(
|
||||
roles=THUMB_ROLES,
|
||||
adjacent_pairs=(THUMB_ROLES[:2], THUMB_ROLES[1:]),
|
||||
maximum_pose_jump_rad=math.pi,
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=math.radians(5),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
initialization_frames=8,
|
||||
)
|
||||
self.reference = None
|
||||
|
||||
def reset(self, *, preserve_task_reference=False):
|
||||
super().reset(preserve_task_reference=preserve_task_reference)
|
||||
self.reference = None
|
||||
|
||||
@staticmethod
|
||||
def relative(poses):
|
||||
rotations = [Rotation.from_quat(poses[r].quaternion_xyzw) for r in THUMB_ROLES]
|
||||
return rotations[0].inv()*rotations[1], rotations[1].inv()*rotations[2]
|
||||
|
||||
def _informative_coupled_rotation_costs(self, combinations):
|
||||
# Reuse the shared group selector's soft-cost extension point without
|
||||
# installing coupled_rotation_pairs (there is NO angle-ratio prior).
|
||||
if self.reference is None:
|
||||
return tuple(0.0 for _ in combinations)
|
||||
driver_ref, follower_ref = self.reference
|
||||
residuals = []
|
||||
for poses in combinations:
|
||||
driver, follower = self.relative(poses)
|
||||
a = (driver*driver_ref.inv()).as_rotvec()
|
||||
b = driver_ref.apply((follower*follower_ref.inv()).as_rotvec())
|
||||
if min(np.linalg.norm(a), np.linalg.norm(b)) < math.radians(5):
|
||||
residuals.append(None)
|
||||
continue
|
||||
axis = a/np.linalg.norm(a)
|
||||
residuals.append(float(np.linalg.norm(b-axis*np.dot(axis,b))))
|
||||
# Insufficient excitation or no geometrically credible candidate:
|
||||
# keep ordinary visual continuity, rather than inventing a pose or
|
||||
# rejecting all observations because a prior did not match.
|
||||
if not any(r is not None and r < math.radians(10) for r in residuals):
|
||||
return tuple(0.0 for _ in combinations)
|
||||
return tuple(0.0 if r is None else r/math.radians(3) for r in residuals)
|
||||
|
||||
def select(self, candidates_by_role, *, stamp_ns, **kwargs):
|
||||
# O12 radians never enter the shared tracker's u8 return-path cache.
|
||||
if (self._previous_stamp_ns is not None and
|
||||
not 0 <= stamp_ns-self._previous_stamp_ns <= self.reset_after_ns):
|
||||
self.reset()
|
||||
selected, reason = super().select(candidates_by_role, stamp_ns=stamp_ns)
|
||||
if selected is not None and self.reference is None:
|
||||
self.reference = self.relative(selected)
|
||||
return selected, reason
|
||||
@@ -2,9 +2,11 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
|
||||
from ...core import (
|
||||
AcquisitionPolicy,
|
||||
ArtifactPolicy,
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
@@ -58,15 +60,24 @@ SDK_UPPER_RAD = (
|
||||
1.53588974175501, 1.53588974175501,
|
||||
)
|
||||
|
||||
# Source-URDF-safe intersections. The SDK exposes a larger range on several
|
||||
# axes, but calibration must never command outside the immutable CAD model.
|
||||
SAFE_LOWER_RAD = (
|
||||
0.0, -0.94, -0.8272860654453121, -1.29,
|
||||
-0.26, 0.0, 0.0, -0.26, 0.0, 0.0, 0.0, 0.0,
|
||||
# The calibration domain is the vendor's physical O12 range. Restricting a
|
||||
# scan to the source URDF would be circular: that URDF is precisely the model
|
||||
# being corrected, and its smaller thumb/outer-finger limits previously hid
|
||||
# the real hardware endpoints. "SAFE" is retained as the public profile name
|
||||
# but now means the reviewed SDK hard range, not a CAD intersection.
|
||||
SAFE_LOWER_RAD = SDK_LOWER_RAD
|
||||
SAFE_UPPER_RAD = SDK_UPPER_RAD
|
||||
|
||||
# Joint-state feedback can cross a commanded endpoint slightly because it is
|
||||
# measured after servo tracking and encoder conversion. The envelope remains
|
||||
# anchored to the reviewed SDK hard range, with an explicit two-degree
|
||||
# observation allowance that is never used to generate a command.
|
||||
FEEDBACK_DOMAIN_MARGIN_RAD = math.radians(2.0)
|
||||
FEEDBACK_LOWER_RAD = tuple(
|
||||
value - FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_LOWER_RAD
|
||||
)
|
||||
SAFE_UPPER_RAD = (
|
||||
0.73, 0.0, 0.0, 0.0,
|
||||
0.26, 1.33, 1.48, 0.26, 1.33, 1.71, 1.38, 1.38,
|
||||
FEEDBACK_UPPER_RAD = tuple(
|
||||
value + FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_UPPER_RAD
|
||||
)
|
||||
|
||||
SDK_TO_URDF_JOINT: tuple[str, ...] = (
|
||||
@@ -84,6 +95,16 @@ SDK_TO_URDF_JOINT: tuple[str, ...] = (
|
||||
"pinky_mcp_pitch",
|
||||
)
|
||||
|
||||
# Fixed direction contract between vendor channels and the target CAD axes.
|
||||
# The magnitude remains visually calibrated because O12's 12 active SDK
|
||||
# coordinates include tendon/solver coordinates that are not one-to-one with
|
||||
# all 19 physical URDF joints.
|
||||
SDK_TO_URDF_SIGN: tuple[float, ...] = (
|
||||
1.0, -1.0, -1.0, -1.0,
|
||||
-1.0, 1.0, 1.0, -1.0,
|
||||
1.0, 1.0, 1.0, 1.0,
|
||||
)
|
||||
|
||||
ACTIVE_JOINTS = SDK_TO_URDF_JOINT
|
||||
PASSIVE_JOINTS: tuple[str, ...] = (
|
||||
"thumb_dip",
|
||||
@@ -110,14 +131,125 @@ MIMIC_SOURCE_BY_JOINT = {
|
||||
"pinky_pip": "pinky_mcp_pitch",
|
||||
"pinky_dip": "pinky_pip",
|
||||
}
|
||||
|
||||
# SDK endpoints measure travel; they have not been surveyed as CAD contact
|
||||
# datums. In particular CAD.upper - travel is NOT an encoder zero. The root
|
||||
# thumb axes observe roll/yaw phase; parallel flexion zeros additionally need
|
||||
# adjacent axis-line observations. The one declared exclusion below remains
|
||||
# CAD-owned; all other observable active zeros are required for publication.
|
||||
ROOT_GEOMETRIC_ZERO_JOINTS = frozenset({"thumb_cmc_roll", "thumb_cmc_yaw"})
|
||||
# ID2/ID3 are still used to fit and independently validate thumb MCP/DIP
|
||||
# motion, but the curved-shell ID3 observation is not a reliable absolute
|
||||
# axis-line datum for the thumb MCP assembly phase. Keep that one static
|
||||
# origin at immutable source CAD until a rigid downstream fiducial is
|
||||
# available. This is an explicit O12 profile contract, not an exception-path
|
||||
# quality bypass; every other observable active zero remains mandatory.
|
||||
STATIC_ZERO_EXCLUDED_JOINTS = frozenset({"thumb_mcp"})
|
||||
GEOMETRIC_ZERO_JOINTS = frozenset(
|
||||
CALIBRATED_ACTIVE_JOINTS - STATIC_ZERO_EXCLUDED_JOINTS
|
||||
)
|
||||
ENDPOINT_ANCHOR_BY_JOINT: dict[str, str] = {}
|
||||
COMMAND_INDEX_BY_JOINT = {
|
||||
joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT)
|
||||
}
|
||||
|
||||
PARK_FINGER_RAD = 0.65 * 1.38
|
||||
PARK_MIDDLE_MCP_RAD = 0.65 * 1.33
|
||||
PARK_MIDDLE_PIP_RAD = 0.65 * 1.71
|
||||
# The outer pair is used only as collision clearance after the pinky's own
|
||||
# full-range scan has already exercised the same safe endpoint. O12 has only
|
||||
# one active coordinate on ring/pinky, so partial MCP flexion leaves both long
|
||||
# passive chains inside the middle/index camera corridor. Park them at the
|
||||
# reviewed CAD/SDK intersection instead of the former 65% pose.
|
||||
OUTER_CLEARANCE_FRACTION = 1.0
|
||||
PARK_RING_MCP_RAD = OUTER_CLEARANCE_FRACTION * SAFE_UPPER_RAD[10]
|
||||
PARK_PINKY_MCP_RAD = OUTER_CLEARANCE_FRACTION * SAFE_UPPER_RAD[11]
|
||||
# Compatibility name for downstream diagnostics; motion code uses the two
|
||||
# channel-specific values above and never takes their minimum.
|
||||
PARK_FINGER_RAD = PARK_PINKY_MCP_RAD
|
||||
PARK_MIDDLE_MCP_RAD = SAFE_UPPER_RAD[8]
|
||||
PARK_MIDDLE_PIP_RAD = SAFE_UPPER_RAD[9]
|
||||
# Feedback radians are the quantity being calibrated and need not numerically
|
||||
# equal the command endpoint. Once a finger has been scanned, clearance uses
|
||||
# that measured feedback travel as its endpoint reference. Simultaneous MCP
|
||||
# and PIP flexion may shorten the vendor solver coordinate slightly, so 90%
|
||||
# of the isolated measured stroke plus a settled hold is the accepted physical
|
||||
# maximum for the coupled clearance pose.
|
||||
CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION = 0.90
|
||||
CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD = 0.06
|
||||
CLEARANCE_SPLAY_ENDPOINT_TOLERANCE_RAD = 0.03
|
||||
INDEX_CLEARANCE_RAD = -math.radians(10.0)
|
||||
# O12's index ABAD/MCP and middle ABAD/MCP are coupled tendon coordinates.
|
||||
# The vendor solver clamps MCP upward when a large ABAD command is paired
|
||||
# with MCP=0. These reviewed values keep the requested pose inside the
|
||||
# solver's feasible set instead of relying on an invisible SDK correction:
|
||||
# - index ABAD -10 deg requires MCP >= 0.109846 rad;
|
||||
# - either ABAD roll endpoint (+/-0.26 rad) requires MCP >= 0.163636 rad.
|
||||
INDEX_CLEARANCE_MCP_RAD = 0.11
|
||||
ROLL_CLEARANCE_MCP_RAD = 0.17
|
||||
# O12 feedback is continuous radians. Use 64 possible normalized cells and
|
||||
# require at least 32 of them; using exactly 32 possible cells accidentally
|
||||
# required both mechanical endpoints despite the separate 90% span gate.
|
||||
NORMALIZED_SWEEP_BIN_COUNT = 64
|
||||
# A feedback curve is the quantity being identified, so its unknown endpoint
|
||||
# scale must never be judged against the command endpoint as if they were the
|
||||
# same coordinate. Cycle 0 is normalized over its own measured endpoints and
|
||||
# therefore establishes a unit-span physical reference. Later cycles must
|
||||
# reproduce at least 90% of that measured full-stroke reference.
|
||||
INITIAL_FEEDBACK_SPAN_FRACTION = 0.99
|
||||
EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION = 0.90
|
||||
# Both FRONT/TOP and FRONT/SIDE are close to orthogonal on the reviewed O12
|
||||
# rig. Their common planar checkerboard must be oblique in at least one view,
|
||||
# so the G20 1.2 px (and O6 1.5 px) batch gates reject highly repeatable O12
|
||||
# solutions for image-geometry reasons. At the measured 3700--3800 px focal
|
||||
# length, 2.0 px is about 0.030 degrees. Treat RMS as a gross-error gate and
|
||||
# keep the independent 0.3 degree and 1.5 mm pose-repeatability gates as the
|
||||
# final geometric consistency evidence.
|
||||
MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX = 2.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RollCrossViewObserver:
|
||||
"""O12-only side-camera contract for one front-camera roll sweep."""
|
||||
|
||||
task_name: str
|
||||
view: str
|
||||
parent_role: str
|
||||
child_role: str
|
||||
source_joint: str
|
||||
model_joint: str
|
||||
coupled_channel_index: int
|
||||
maximum_coupled_motion_rad: float
|
||||
|
||||
|
||||
# The downstream PIP Tag is rigidly carried by MCP roll, so it supplies an
|
||||
# independent side-view roll curve while the front Tag supplies the published
|
||||
# curve. O12's tendon solver also has a small, deterministic PIP back-drive
|
||||
# around centred ABAD: joint->actuator->joint reaches about 0.0124 rad even
|
||||
# when PIP is commanded to zero. The reviewed 0.020 rad envelope includes
|
||||
# encoder quantisation and applies only to roll-task cross-view integrity.
|
||||
ROLL_CROSS_VIEW_OBSERVERS: tuple[RollCrossViewObserver, ...] = (
|
||||
RollCrossViewObserver(
|
||||
"middle_roll_front", "side", "side_base", "middle_pip",
|
||||
"middle_mcp_roll_side", "middle_mcp_roll", 9, 0.020,
|
||||
),
|
||||
RollCrossViewObserver(
|
||||
"index_roll_front", "side", "side_base", "index_pip",
|
||||
"index_mcp_roll_side", "index_mcp_roll", 6, 0.020,
|
||||
),
|
||||
)
|
||||
ROLL_CROSS_VIEW_BY_TASK = {
|
||||
observer.task_name: observer for observer in ROLL_CROSS_VIEW_OBSERVERS
|
||||
}
|
||||
|
||||
# Fast-calibration caps by fixed SDK channel. The thumb CMC pitch cap keeps
|
||||
# explicit margin below its unusually low 0.11 rad/s source-URDF limit; the
|
||||
# remaining caps are deliberately far below their corresponding CAD limits
|
||||
# while keeping one visual sweep long enough for dense 30 Hz observations.
|
||||
FORMAL_SPEED_CAP_RAD_S: tuple[float, ...] = (
|
||||
0.16, 0.16, 0.10, 0.32,
|
||||
# Index/middle ABAD at 0.16 rad/s repeatedly returned only 89.2--89.5%
|
||||
# feedback coverage; 0.12 rad/s measured 91.2% on the physical hand.
|
||||
0.12, 0.32, 0.32, 0.12,
|
||||
0.32, 0.32, 0.32, 0.32,
|
||||
)
|
||||
|
||||
|
||||
def _task(
|
||||
@@ -147,15 +279,24 @@ def _task(
|
||||
def build_typed_profile() -> CalibrationProfile:
|
||||
middle_clearance = (
|
||||
(4, INDEX_CLEARANCE_RAD),
|
||||
(10, PARK_FINGER_RAD),
|
||||
(11, PARK_FINGER_RAD),
|
||||
(5, INDEX_CLEARANCE_MCP_RAD),
|
||||
(10, PARK_RING_MCP_RAD),
|
||||
(11, PARK_PINKY_MCP_RAD),
|
||||
)
|
||||
middle_roll_clearance = (
|
||||
*middle_clearance,
|
||||
(8, ROLL_CLEARANCE_MCP_RAD),
|
||||
)
|
||||
index_clearance = (
|
||||
(7, 0.0),
|
||||
(8, PARK_MIDDLE_MCP_RAD),
|
||||
(9, PARK_MIDDLE_PIP_RAD),
|
||||
(10, PARK_FINGER_RAD),
|
||||
(11, PARK_FINGER_RAD),
|
||||
(10, PARK_RING_MCP_RAD),
|
||||
(11, PARK_PINKY_MCP_RAD),
|
||||
)
|
||||
index_roll_clearance = (
|
||||
*index_clearance,
|
||||
(5, ROLL_CLEARANCE_MCP_RAD),
|
||||
)
|
||||
tasks = (
|
||||
_task("thumb_pitch_front", "front", 2, ("thumb_cmc_pitch",), start=0.0, end=SAFE_LOWER_RAD[2], speed=0.03),
|
||||
@@ -163,10 +304,10 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
_task("thumb_mcp_dip_front", "front", 3, ("thumb_mcp", "thumb_dip"), start=0.0, end=SAFE_LOWER_RAD[3], speed=0.08),
|
||||
_task("thumb_yaw_top", "top", 1, ("thumb_cmc_yaw",), start=0.0, end=SAFE_LOWER_RAD[1], speed=0.04),
|
||||
_task("pinky_chain_side", "side", 11, ("pinky_mcp_pitch", "pinky_pip", "pinky_dip"), start=0.0, end=SAFE_UPPER_RAD[11], speed=0.08),
|
||||
_task("middle_roll_front", "front", 7, ("middle_mcp_roll",), start=SAFE_UPPER_RAD[7], end=SAFE_LOWER_RAD[7], speed=0.04, auxiliary=middle_clearance),
|
||||
_task("middle_roll_front", "front", 7, ("middle_mcp_roll",), start=SAFE_UPPER_RAD[7], end=SAFE_LOWER_RAD[7], speed=0.04, auxiliary=middle_roll_clearance),
|
||||
_task("middle_mcp_side", "side", 8, ("middle_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[8], speed=0.08, auxiliary=middle_clearance),
|
||||
_task("middle_pip_dip_side", "side", 9, ("middle_pip", "middle_dip"), start=0.0, end=SAFE_UPPER_RAD[9], speed=0.08, auxiliary=middle_clearance),
|
||||
_task("index_roll_front", "front", 4, ("index_mcp_roll",), start=SAFE_UPPER_RAD[4], end=SAFE_LOWER_RAD[4], speed=0.04, auxiliary=index_clearance),
|
||||
_task("index_roll_front", "front", 4, ("index_mcp_roll",), start=SAFE_UPPER_RAD[4], end=SAFE_LOWER_RAD[4], speed=0.04, auxiliary=index_roll_clearance),
|
||||
_task("index_mcp_side", "side", 5, ("index_mcp_pitch",), start=0.0, end=SAFE_UPPER_RAD[5], speed=0.08, auxiliary=index_clearance),
|
||||
_task("index_pip_dip_side", "side", 6, ("index_pip", "index_dip"), start=0.0, end=SAFE_UPPER_RAD[6], speed=0.08, auxiliary=index_clearance),
|
||||
)
|
||||
@@ -184,7 +325,7 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
"middle_pip": MeasurementSpec("middle_pip", "relative_rotation", "side", "side_base", "middle_pip"),
|
||||
"middle_dip": MeasurementSpec("middle_dip", "relative_rotation", "side", "middle_pip", "middle_dip", pose_axis_line_required=False),
|
||||
"index_mcp_roll": MeasurementSpec("index_mcp_roll", "relative_rotation", "front", "front_base", "index_roll"),
|
||||
"index_mcp_pitch": MeasurementSpec("index_mcp_pitch", "relative_rotation", "side", "side_base", "index_pip"),
|
||||
"index_mcp_pitch": MeasurementSpec("index_mcp_pitch", "relative_rotation", "side", "side_base", "index_dip"),
|
||||
"index_pip": MeasurementSpec("index_pip", "relative_rotation", "side", "side_base", "index_pip"),
|
||||
"index_dip": MeasurementSpec("index_dip", "relative_rotation", "side", "index_pip", "index_dip", pose_axis_line_required=False),
|
||||
}
|
||||
@@ -196,6 +337,8 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
"transferred_static_dynamic"
|
||||
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
|
||||
else "measured_static_dynamic"
|
||||
if name in GEOMETRIC_ZERO_JOINTS
|
||||
else "measured_dynamic_cad_static"
|
||||
)
|
||||
for name in active
|
||||
},
|
||||
@@ -218,6 +361,8 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
# The public command domain is the reviewed SDK/CAD intersection.
|
||||
lower_bounds=SAFE_LOWER_RAD,
|
||||
upper_bounds=SAFE_UPPER_RAD,
|
||||
feedback_lower_bounds=FEEDBACK_LOWER_RAD,
|
||||
feedback_upper_bounds=FEEDBACK_UPPER_RAD,
|
||||
unit="rad",
|
||||
feedback_by_index=True,
|
||||
command_index_by_joint=COMMAND_INDEX_BY_JOINT,
|
||||
@@ -243,7 +388,7 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
common_frame="calibration_common",
|
||||
extrinsic_reference_view="front",
|
||||
extrinsics_quality_limits={
|
||||
"reprojection_rms_px": 1.2,
|
||||
"reprojection_rms_px": MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX,
|
||||
"maximum_rotation_repeatability_deg": 0.3,
|
||||
"maximum_translation_repeatability_m": 0.0015,
|
||||
},
|
||||
@@ -251,10 +396,12 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
),
|
||||
motion=MotionPolicy(
|
||||
tasks=tasks,
|
||||
precheck_sweeps=True,
|
||||
steady_command_checkpoints=True,
|
||||
# O12 uses AcquisitionPolicy.mapping_probe_maximum_rad for one
|
||||
# channel-mapping jog; it does not inherit legacy sweep prechecks.
|
||||
precheck_sweeps=False,
|
||||
steady_command_checkpoints=False,
|
||||
speed_parameters={
|
||||
"command_rate_hz": 20.0,
|
||||
"command_rate_hz": 50.0,
|
||||
"clearance_flex_rad_s": 0.10,
|
||||
"clearance_splay_rad_s": 0.04,
|
||||
"probe_travel_rad": math.radians(3.0),
|
||||
@@ -267,22 +414,31 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
zero=ZeroSolvePolicy(
|
||||
active_joints=active,
|
||||
passive_joints=passive,
|
||||
direct_zero_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS)),
|
||||
axis_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS)),
|
||||
mechanical_endpoint_joints=frozenset(CALIBRATED_ACTIVE_JOINTS),
|
||||
direct_zero_joints=tuple(sorted(GEOMETRIC_ZERO_JOINTS)),
|
||||
axis_joints=tuple(sorted(
|
||||
CALIBRATED_ACTIVE_JOINTS
|
||||
| (MEASURED_PASSIVE_JOINTS - {"thumb_dip"})
|
||||
)),
|
||||
mechanical_endpoint_joints=frozenset(ENDPOINT_ANCHOR_BY_JOINT),
|
||||
post_solve_endpoint_joints=frozenset(),
|
||||
mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT,
|
||||
cad_frozen_joints=passive,
|
||||
endpoint_anchor_by_joint={name: "lower_at_start" for name in CALIBRATED_ACTIVE_JOINTS},
|
||||
fitted_mimic_joints=MEASURED_PASSIVE_JOINTS,
|
||||
coupling_model_by_joint={name: "quadratic_runtime" for name in MEASURED_PASSIVE_JOINTS},
|
||||
cad_frozen_joints=passive | (active - GEOMETRIC_ZERO_JOINTS - set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT)),
|
||||
endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT,
|
||||
# AprilTags validate passive motion, but the deployed passive
|
||||
# mapping is owned by the O12 SDK solver. A planar-PnP branch must
|
||||
# never replace that product kinematic contract in the URDF.
|
||||
fitted_mimic_joints=frozenset(),
|
||||
coupling_model_by_joint={
|
||||
name: "vendor_o12_polynomial"
|
||||
for name in MEASURED_PASSIVE_JOINTS
|
||||
},
|
||||
),
|
||||
quality=QualityPolicy(
|
||||
training_cycles=(0, 1, 2),
|
||||
holdout_cycle=3,
|
||||
hard_threshold_keys=frozenset({
|
||||
"minimum_detection_rate", "maximum_state_image_skew_ms",
|
||||
"maximum_hysteresis_rad", "maximum_validation_error_rad",
|
||||
"maximum_validation_error_rad",
|
||||
"maximum_mimic_residual_rad",
|
||||
}),
|
||||
isolated_holdout=True,
|
||||
@@ -301,9 +457,16 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
"calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256",
|
||||
}),
|
||||
publication_pointer="latest_passed",
|
||||
session_compatibility_tokens=frozenset({"o12_right_16_v1", "feedback_rad_v1"}),
|
||||
session_compatibility_tokens=frozenset({
|
||||
"o12_right_16_v1", "feedback_rad_v1", "full_sdk_range_v2"
|
||||
}),
|
||||
publish_corrected_urdf=True,
|
||||
),
|
||||
acquisition=AcquisitionPolicy(
|
||||
mapping_probe_maximum_rad=math.radians(3.0),
|
||||
physical_first_cycle_minimum_span_01=INITIAL_FEEDBACK_SPAN_FRACTION,
|
||||
physical_repeat_minimum_fraction=EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION,
|
||||
),
|
||||
joint_coverage=coverage,
|
||||
)
|
||||
|
||||
@@ -336,9 +499,19 @@ def build_profile() -> RegisteredProfile:
|
||||
|
||||
__all__ = [
|
||||
"ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT",
|
||||
"COMMAND_NAMES", "INDEX_CLEARANCE_RAD", "KEY", "MEASURED_PASSIVE_JOINTS",
|
||||
"MIMIC_SOURCE_BY_JOINT", "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD",
|
||||
"CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD",
|
||||
"CLEARANCE_SPLAY_ENDPOINT_TOLERANCE_RAD", "COMMAND_NAMES",
|
||||
"GEOMETRIC_ZERO_JOINTS", "INDEX_CLEARANCE_RAD", "KEY",
|
||||
"MEASURED_PASSIVE_JOINTS",
|
||||
"EFFECTIVE_TRAVEL_REPEATABILITY_FRACTION",
|
||||
"INITIAL_FEEDBACK_SPAN_FRACTION", "MIMIC_SOURCE_BY_JOINT",
|
||||
"NORMALIZED_SWEEP_BIN_COUNT", "OUTER_CLEARANCE_FRACTION",
|
||||
"PARK_FINGER_RAD", "PARK_MIDDLE_MCP_RAD", "PARK_PINKY_MCP_RAD",
|
||||
"PARK_RING_MCP_RAD", "CLEARANCE_MINIMUM_FEEDBACK_TRAVEL_FRACTION",
|
||||
"PARK_MIDDLE_PIP_RAD", "PASSIVE_JOINTS", "SAFE_LOWER_RAD", "SAFE_UPPER_RAD",
|
||||
"ROLL_CROSS_VIEW_BY_TASK", "ROLL_CROSS_VIEW_OBSERVERS",
|
||||
"RollCrossViewObserver",
|
||||
"SDK_LOWER_RAD", "SDK_TO_URDF_JOINT", "SDK_UPPER_RAD",
|
||||
"STATIC_ZERO_EXCLUDED_JOINTS",
|
||||
"TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
"""O12-only sweep observability policy.
|
||||
|
||||
The O12 records continuous radian feedback into a fixed normalized grid. A
|
||||
raw per-frame Tag rate is useful for camera diagnostics, but it is not by
|
||||
itself evidence that a fitted curve is unobservable: a long sweep may retain
|
||||
hundreds of synchronized samples and dense travel coverage after a short Tag
|
||||
occlusion. This module makes the actual fitting information the acceptance
|
||||
contract and keeps the stricter camera targets as warnings.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
QUALITY_POLICY_VERSION = 5
|
||||
# G20 permits roughly one sixteenth of its 256-bin domain to be unobserved
|
||||
# contiguously. Express that invariant as a fraction so O12's 64-bin grid is
|
||||
# judged at the same physical scale instead of copying a raw bin count.
|
||||
MAXIMUM_UNOBSERVED_BIN_FRACTION = 1.0 / 16.0
|
||||
|
||||
|
||||
def evaluate_o12_observation_quality(
|
||||
normalized_feedback: Sequence[float],
|
||||
observation: Mapping[str, Any],
|
||||
*,
|
||||
normalized_bin_count: int,
|
||||
required_feedback_span: float,
|
||||
minimum_sweep_frames: int,
|
||||
minimum_sweep_bins: int,
|
||||
minimum_joint_frame_rate: float,
|
||||
minimum_feedback_hz: float,
|
||||
feedback_hz: float,
|
||||
target_detection_rate: float,
|
||||
target_maximum_bin_gap: int,
|
||||
) -> dict[str, Any]:
|
||||
"""Return one authoritative O12 acceptance decision and diagnostics."""
|
||||
count = int(normalized_bin_count)
|
||||
if count < 32:
|
||||
raise ValueError("normalized sweep bin count must be at least 32")
|
||||
values = np.asarray(normalized_feedback, dtype=float)
|
||||
values = values[np.isfinite(values)]
|
||||
clipped = np.clip(values, 0.0, 1.0)
|
||||
bins = sorted(set(
|
||||
min(count - 1, max(0, int(value * count)))
|
||||
for value in clipped
|
||||
))
|
||||
span = float(np.ptp(clipped)) if clipped.size else 0.0
|
||||
maximum_gap = max(
|
||||
(right - left - 1 for left, right in zip(bins, bins[1:])),
|
||||
default=0 if bins else count,
|
||||
)
|
||||
maximum_unobserved_bins = max(
|
||||
1, int(math.floor(count * MAXIMUM_UNOBSERVED_BIN_FRACTION))
|
||||
)
|
||||
allowed_maximum_gap = maximum_unobserved_bins
|
||||
detection_rate = float(observation.get("tag_detection_rate", 0.0))
|
||||
joint_frame_rate = float(observation.get("joint_frame_rate", 0.0))
|
||||
|
||||
failures: list[str] = []
|
||||
warnings: list[str] = []
|
||||
if clipped.size < int(minimum_sweep_frames):
|
||||
failures.append(f"frames={clipped.size}")
|
||||
if clipped.size == 0 or span < float(required_feedback_span):
|
||||
failures.append("feedback_span")
|
||||
if len(bins) < int(minimum_sweep_bins):
|
||||
failures.append(f"bins={len(bins)}")
|
||||
if maximum_gap > allowed_maximum_gap:
|
||||
failures.append(f"maximum_gap={maximum_gap}")
|
||||
if joint_frame_rate < float(minimum_joint_frame_rate):
|
||||
warnings.append(f"joint_frame_rate={joint_frame_rate:.3f}")
|
||||
if float(feedback_hz) < float(minimum_feedback_hz):
|
||||
warnings.append(f"feedback_hz={float(feedback_hz):.2f}")
|
||||
|
||||
# These remain explicit operator diagnostics. They do not duplicate the
|
||||
# observability gates above or force a complete rescan of otherwise dense
|
||||
# data after a short, localized occlusion.
|
||||
if detection_rate < float(target_detection_rate):
|
||||
role_rates = observation.get("tag_detection_rate_by_role", {})
|
||||
worst_role = min(role_rates, key=role_rates.get, default="unknown")
|
||||
warnings.append(
|
||||
f"tag_rate_target[{worst_role}]={detection_rate:.3f}"
|
||||
)
|
||||
if maximum_gap > int(target_maximum_bin_gap):
|
||||
warnings.append(f"maximum_gap_target={maximum_gap}")
|
||||
|
||||
return {
|
||||
"quality_policy_version": QUALITY_POLICY_VERSION,
|
||||
"decision_basis": "normalized_fit_observability",
|
||||
"valid_frames": int(clipped.size),
|
||||
"feedback_bins": len(bins),
|
||||
"feedback_span": round(span, 9),
|
||||
"required_feedback_span": round(float(required_feedback_span), 9),
|
||||
"maximum_bin_gap": int(maximum_gap),
|
||||
"allowed_maximum_bin_gap": int(allowed_maximum_gap),
|
||||
"gap_scope": "observed_feedback_span",
|
||||
"maximum_unobserved_bin_fraction": (
|
||||
MAXIMUM_UNOBSERVED_BIN_FRACTION
|
||||
),
|
||||
"target_detection_rate": float(target_detection_rate),
|
||||
"target_maximum_bin_gap": int(target_maximum_bin_gap),
|
||||
"warnings": warnings,
|
||||
"failures": failures,
|
||||
"passed": not failures,
|
||||
}
|
||||
|
||||
|
||||
__all__ = [
|
||||
"MAXIMUM_UNOBSERVED_BIN_FRACTION",
|
||||
"QUALITY_POLICY_VERSION",
|
||||
"evaluate_o12_observation_quality",
|
||||
]
|
||||
@@ -0,0 +1,384 @@
|
||||
"""Durable, safety-checked O12 scan checkpoint recovery."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
from ...product import ProductConfig
|
||||
from ...runtime import ACQUISITION_POLICY_VERSION
|
||||
from .pipeline import load_o12_raw_samples
|
||||
from .profile import ROLL_CROSS_VIEW_BY_TASK
|
||||
|
||||
|
||||
PROTECTED_INPUT_KEYS = (
|
||||
"source_urdf_sha256",
|
||||
"camera_extrinsics_sha256",
|
||||
"calibration_config_sha256",
|
||||
"tag_config_sha256",
|
||||
"sdk_config_sha256",
|
||||
)
|
||||
|
||||
ResumeUnit = tuple[str, int, str]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class O12ResumeCheckpoint:
|
||||
source_session: Path
|
||||
compatibility: str
|
||||
completed_units: tuple[ResumeUnit, ...]
|
||||
completed_tasks: tuple[str, ...]
|
||||
imported_records: tuple[dict[str, Any], ...]
|
||||
|
||||
|
||||
def ordered_resume_units(profile) -> tuple[ResumeUnit, ...]:
|
||||
return tuple(
|
||||
(task.key, cycle, direction)
|
||||
for task in profile.motion.tasks
|
||||
for cycle in (0, 1, 2, 3)
|
||||
for direction in ("decreasing", "increasing")
|
||||
)
|
||||
|
||||
|
||||
def _quality_evidence_accepted(
|
||||
row: Mapping[str, Any], valid: int, total: int
|
||||
) -> bool:
|
||||
"""Use the decision recorded by the matching O12 quality-policy version."""
|
||||
if int(row.get("quality_policy_version", 0)) >= 2:
|
||||
return bool(
|
||||
row.get("passed")
|
||||
and not list(row.get("failures", ()))
|
||||
and valid >= 40
|
||||
and total > 0
|
||||
)
|
||||
# Preserve the exact contract used by sessions written before the O12
|
||||
# observability policy existed. This branch is compatibility only; it
|
||||
# does not reinterpret a previously rejected sweep as passing.
|
||||
return bool(
|
||||
not list(row.get("failures", ()))
|
||||
and valid >= 40
|
||||
and total > 0
|
||||
# Older sessions can contain one final synchronized callback after
|
||||
# the total counter reset (for example 259/258); cap the ratio at one.
|
||||
and valid / max(total, valid) >= 0.85
|
||||
and float(row.get("tag_detection_rate", 0.0)) >= 0.95
|
||||
and int(row.get("feedback_bins", 0)) >= 32
|
||||
and int(row.get("maximum_bin_gap", 999)) <= 2
|
||||
)
|
||||
|
||||
|
||||
def _session_start(rows: Sequence[Mapping[str, Any]]) -> Mapping[str, Any]:
|
||||
starts = [row for row in rows if row.get("kind") == "session_start"]
|
||||
if len(starts) != 1:
|
||||
raise ValueError("resume raw must contain exactly one session_start")
|
||||
return starts[0]
|
||||
|
||||
|
||||
def _protected_values(config: ProductConfig) -> dict[str, str]:
|
||||
return {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"tag_config_sha256": config.tag_config_sha256,
|
||||
"sdk_config_sha256": config.sdk_config_sha256,
|
||||
}
|
||||
|
||||
|
||||
def _legacy_checkpoint_is_attested(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
) -> bool:
|
||||
"""Accept pre-checkpoint O12 data only when its local inputs are unchanged."""
|
||||
raw_path = session / "raw_samples.jsonl"
|
||||
try:
|
||||
raw_time = raw_path.stat().st_mtime
|
||||
protected_paths = (
|
||||
config.source_urdf,
|
||||
config.camera_extrinsics,
|
||||
config.calibration_config,
|
||||
config.tag_config,
|
||||
config.sdk_config,
|
||||
)
|
||||
if any(
|
||||
path is None or path.stat().st_mtime > raw_time
|
||||
for path in protected_paths
|
||||
):
|
||||
return False
|
||||
log = (session / "calibration.log").read_text(
|
||||
encoding="utf-8", errors="replace"
|
||||
)
|
||||
except OSError:
|
||||
return False
|
||||
if str(config.source_urdf) not in log:
|
||||
return False
|
||||
return any(
|
||||
row.get("kind") == "o12_temperature_capability_fallback"
|
||||
and row.get("sdk_config_sha256") == config.sdk_config_sha256
|
||||
for row in rows
|
||||
)
|
||||
|
||||
|
||||
def validate_resume_source(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
) -> tuple[list[dict[str, Any]], str]:
|
||||
candidate = Path(session).expanduser().resolve(strict=True)
|
||||
root = config.session_root.expanduser().resolve(strict=True)
|
||||
if candidate.parent != root or not candidate.is_dir():
|
||||
raise ValueError("resume session must be a direct child of the serial root")
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
if summary_path.is_file():
|
||||
try:
|
||||
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as error:
|
||||
raise ValueError("resume session has an invalid summary") from error
|
||||
if isinstance(summary, Mapping) and summary.get("result") == "PASS":
|
||||
raise ValueError("a passed O12 session cannot be used as a checkpoint")
|
||||
rows = load_o12_raw_samples(candidate / "raw_samples.jsonl")
|
||||
start = _session_start(rows)
|
||||
if (
|
||||
start.get("profile_id") != config.profile_key.profile_id
|
||||
or start.get("serial_number") != config.serial_number
|
||||
or int(start.get("sample_schema_version", -1))
|
||||
!= config.calibration_contract.typed_profile.artifacts.output_schema_version
|
||||
):
|
||||
raise ValueError("resume profile, serial number, or schema differs")
|
||||
if start.get("acquisition_policy_version") != ACQUISITION_POLICY_VERSION:
|
||||
raise ValueError(
|
||||
"resume acquisition policy differs; unified_engine_v1 requires "
|
||||
"a new full capture"
|
||||
)
|
||||
expected = _protected_values(config)
|
||||
recorded = {key: str(start.get(key, "")) for key in PROTECTED_INPUT_KEYS}
|
||||
if all(recorded.values()):
|
||||
if recorded != expected:
|
||||
raise ValueError("resume protected input hashes differ")
|
||||
compatibility = "protected_hashes_v1"
|
||||
else:
|
||||
raise ValueError("resume checkpoint has no protected input hashes")
|
||||
return rows, compatibility
|
||||
|
||||
|
||||
def build_resume_checkpoint(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
) -> O12ResumeCheckpoint:
|
||||
rows, compatibility = validate_resume_source(config, session)
|
||||
return build_resume_checkpoint_from_rows(
|
||||
config.calibration_contract.typed_profile,
|
||||
session,
|
||||
rows,
|
||||
compatibility=compatibility,
|
||||
)
|
||||
|
||||
|
||||
def build_resume_checkpoint_from_rows(
|
||||
profile,
|
||||
session: str | Path,
|
||||
rows: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
compatibility: str,
|
||||
) -> O12ResumeCheckpoint:
|
||||
task_by_key = {task.key: task for task in profile.motion.tasks}
|
||||
cross_view_passing: set[tuple[ResumeUnit, int]] = set()
|
||||
for row in rows:
|
||||
if row.get("kind") != "o12_roll_cross_view_quality":
|
||||
continue
|
||||
try:
|
||||
unit = (
|
||||
str(row["task_name"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
attempt = int(row.get("attempt", 1))
|
||||
valid = int(row.get("valid_frames", 0))
|
||||
total = int(row.get("total_frames", 0))
|
||||
accepted = (
|
||||
unit[0] in ROLL_CROSS_VIEW_BY_TASK
|
||||
and unit[1] in (0, 1, 2, 3)
|
||||
and unit[2] in {"decreasing", "increasing"}
|
||||
and _quality_evidence_accepted(row, valid, total)
|
||||
)
|
||||
except (KeyError, TypeError, ValueError, ZeroDivisionError):
|
||||
continue
|
||||
if accepted:
|
||||
cross_view_passing.add((unit, attempt))
|
||||
|
||||
passing: dict[ResumeUnit, tuple[int, Mapping[str, Any]]] = {}
|
||||
for row in rows:
|
||||
if row.get("kind") != "o12_sweep_observation_quality":
|
||||
continue
|
||||
try:
|
||||
unit = (
|
||||
str(row["task_name"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
attempt = int(row.get("attempt", 1))
|
||||
valid = int(row.get("valid_frames", 0))
|
||||
total = int(row.get("total_frames", 0))
|
||||
accepted = (
|
||||
unit[0] in task_by_key
|
||||
and unit[1] in (0, 1, 2, 3)
|
||||
and unit[2] in {"decreasing", "increasing"}
|
||||
and _quality_evidence_accepted(row, valid, total)
|
||||
and (
|
||||
unit[0] not in ROLL_CROSS_VIEW_BY_TASK
|
||||
or (unit, attempt) in cross_view_passing
|
||||
)
|
||||
)
|
||||
except (KeyError, TypeError, ValueError, ZeroDivisionError):
|
||||
continue
|
||||
if accepted and attempt >= passing.get(unit, (0, {}))[0]:
|
||||
passing[unit] = (attempt, row)
|
||||
|
||||
ordered = ordered_resume_units(profile)
|
||||
completed: list[ResumeUnit] = []
|
||||
for unit in ordered:
|
||||
if unit not in passing:
|
||||
break
|
||||
completed.append(unit)
|
||||
if not completed:
|
||||
raise ValueError("resume session has no contiguous passed scan unit")
|
||||
# A session created while resuming declares how many source units it
|
||||
# intended to import. If startup was interrupted during persistence, its
|
||||
# JSONL can contain that audit header but only a prefix of the associated
|
||||
# samples. Do not let such a newer, truncated session shadow the older
|
||||
# complete checkpoint during automatic selection.
|
||||
declared_import_counts = [
|
||||
int(row.get("completed_unit_count", -1))
|
||||
for row in rows
|
||||
if row.get("kind") == "o12_resume_checkpoint_import"
|
||||
]
|
||||
if any(count < 0 or count > len(completed) for count in declared_import_counts):
|
||||
raise ValueError("resume checkpoint import is incomplete or truncated")
|
||||
|
||||
selected_attempt = {
|
||||
unit: passing[unit][0] for unit in completed
|
||||
}
|
||||
imported: list[dict[str, Any]] = []
|
||||
for row in rows:
|
||||
kind = row.get("kind")
|
||||
if kind not in {
|
||||
"o12_joint_sample",
|
||||
"o12_pnp_candidate_frame",
|
||||
"o12_sweep_observation_quality",
|
||||
"o12_roll_cross_view_sample",
|
||||
"o12_roll_cross_view_quality",
|
||||
}:
|
||||
continue
|
||||
try:
|
||||
unit = (
|
||||
str(row["task_name"]),
|
||||
int(row["cycle"]),
|
||||
str(row["direction"]),
|
||||
)
|
||||
attempt = int(row.get("attempt", 1))
|
||||
except (KeyError, TypeError, ValueError):
|
||||
continue
|
||||
if unit not in selected_attempt or attempt != selected_attempt[unit]:
|
||||
continue
|
||||
copied = dict(row)
|
||||
copied["resume_imported"] = True
|
||||
copied["resume_source_session"] = Path(session).resolve().name
|
||||
imported.append(copied)
|
||||
|
||||
completed_set = set(completed)
|
||||
completed_tasks = tuple(
|
||||
task.key
|
||||
for task in profile.motion.tasks
|
||||
if all(
|
||||
(task.key, cycle, direction) in completed_set
|
||||
for cycle in (0, 1, 2, 3)
|
||||
for direction in ("decreasing", "increasing")
|
||||
)
|
||||
)
|
||||
for row in rows:
|
||||
if (
|
||||
row.get("kind") == "o12_fixed_mapping_preflight"
|
||||
and str(row.get("task_name", "")) in completed_tasks
|
||||
):
|
||||
copied = dict(row)
|
||||
copied["resume_imported"] = True
|
||||
copied["resume_source_session"] = Path(session).resolve().name
|
||||
imported.append(copied)
|
||||
|
||||
primary_by_unit = {
|
||||
unit: 0 for unit in completed
|
||||
}
|
||||
for row in imported:
|
||||
if row.get("kind") != "o12_joint_sample":
|
||||
continue
|
||||
unit = (
|
||||
str(row["task_name"]), int(row["cycle"]), str(row["direction"])
|
||||
)
|
||||
task = task_by_key[unit[0]]
|
||||
if row.get("joint") == task.joints[0]:
|
||||
primary_by_unit[unit] += 1
|
||||
if any(count < 40 for count in primary_by_unit.values()):
|
||||
raise ValueError("resume quality record lacks its primary joint samples")
|
||||
|
||||
cross_samples_by_unit = {
|
||||
unit: 0 for unit in completed if unit[0] in ROLL_CROSS_VIEW_BY_TASK
|
||||
}
|
||||
for row in imported:
|
||||
if row.get("kind") != "o12_roll_cross_view_sample":
|
||||
continue
|
||||
unit = (
|
||||
str(row["task_name"]), int(row["cycle"]), str(row["direction"])
|
||||
)
|
||||
if unit in cross_samples_by_unit:
|
||||
cross_samples_by_unit[unit] += 1
|
||||
if any(count < 40 for count in cross_samples_by_unit.values()):
|
||||
raise ValueError("resume roll quality lacks its side-view samples")
|
||||
|
||||
return O12ResumeCheckpoint(
|
||||
source_session=Path(session).expanduser().resolve(),
|
||||
compatibility=compatibility,
|
||||
completed_units=tuple(completed),
|
||||
completed_tasks=completed_tasks,
|
||||
imported_records=tuple(imported),
|
||||
)
|
||||
|
||||
|
||||
def automatic_resume_candidate(config: ProductConfig) -> O12ResumeCheckpoint | None:
|
||||
root = config.session_root
|
||||
try:
|
||||
candidates = sorted(
|
||||
(
|
||||
path for path in root.resolve(strict=True).iterdir()
|
||||
if path.is_dir() and not path.name.startswith("latest_")
|
||||
),
|
||||
key=lambda path: path.name,
|
||||
reverse=True,
|
||||
)
|
||||
except OSError:
|
||||
return None
|
||||
passed: Path | None = None
|
||||
pointer = root / "latest_passed"
|
||||
if pointer.exists():
|
||||
try:
|
||||
passed = pointer.resolve(strict=True)
|
||||
except OSError:
|
||||
pass
|
||||
for candidate in candidates:
|
||||
if passed is not None and candidate.name <= passed.name:
|
||||
continue
|
||||
try:
|
||||
return build_resume_checkpoint(config, candidate)
|
||||
except (OSError, ValueError):
|
||||
continue
|
||||
return None
|
||||
|
||||
|
||||
__all__ = [
|
||||
"O12ResumeCheckpoint",
|
||||
"automatic_resume_candidate",
|
||||
"build_resume_checkpoint",
|
||||
"build_resume_checkpoint_from_rows",
|
||||
"ordered_resume_units",
|
||||
"validate_resume_source",
|
||||
]
|
||||
@@ -7,6 +7,7 @@ from datetime import datetime
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
import subprocess
|
||||
import time
|
||||
from typing import Any
|
||||
@@ -22,10 +23,10 @@ from ..l6.runner import (
|
||||
_l6_reason_zh,
|
||||
_launch_command,
|
||||
_stop_stack,
|
||||
_wait_until,
|
||||
render_six_channel_progress_zh,
|
||||
)
|
||||
from .pipeline import finalize_o12_session, load_o12_raw_samples
|
||||
from .resume import automatic_resume_candidate, ordered_resume_units
|
||||
|
||||
|
||||
_TASK_LABELS = {
|
||||
@@ -34,17 +35,30 @@ _TASK_LABELS = {
|
||||
"thumb_mcp_dip_front": "拇指 MCP / 被动 DIP（正面 ID1→ID2→ID3）",
|
||||
"thumb_yaw_top": "拇指 CMC yaw（顶部 ID14→ID15）",
|
||||
"pinky_chain_side": "小指 MCP / 被动 PIP/DIP（侧面 ID4→ID5→ID6→ID7）",
|
||||
"middle_roll_front": "中指 MCP roll（正面 ID0→ID12）",
|
||||
"middle_roll_front": "中指 MCP roll（正面 ID0→ID12 + 侧面 ID4→ID8）",
|
||||
"middle_mcp_side": "中指 MCP pitch（侧面 ID4→ID8）",
|
||||
"middle_pip_dip_side": "中指 PIP / 被动 DIP（侧面 ID8→ID9）",
|
||||
"index_roll_front": "食指 MCP roll（正面 ID0→ID13）",
|
||||
"index_mcp_side": "食指 MCP pitch（侧面 ID4→ID10）",
|
||||
"index_roll_front": "食指 MCP roll（正面 ID0→ID13 + 侧面 ID4→ID10）",
|
||||
"index_mcp_side": "食指 MCP pitch（侧面 ID4→ID11）",
|
||||
"index_pip_dip_side": "食指 PIP / 被动 DIP（侧面 ID10→ID11）",
|
||||
}
|
||||
|
||||
|
||||
def _o12_reason_zh(status):
|
||||
reason = str(status.get("reason", ""))
|
||||
if reason.startswith("sweep_quality_failed:"):
|
||||
details = reason.split(":", 2)[-1]
|
||||
if any(
|
||||
item in details
|
||||
for item in ("side_frames=", "side_bins=", "side_maximum_gap=")
|
||||
):
|
||||
return (
|
||||
"O12-CROSS-VIEW-SAMPLING-104",
|
||||
"侧摆任务的主视角数据已采到，但辅助侧面视角在本段轨迹内"
|
||||
f"保留的时序覆盖不足：{details}。这不表示 Tag 被遮挡。",
|
||||
"程序会保留已通过断点；重新启动后只重做该扫描单元。"
|
||||
"若 Tag 持续可见，优先检查侧面 AprilTag 节点的发布频率和 CPU 调度。",
|
||||
)
|
||||
if reason.startswith("mapping_preflight_no_target_motion:"):
|
||||
task = reason.split(":", 1)[1]
|
||||
return (
|
||||
@@ -52,11 +66,42 @@ def _o12_reason_zh(status):
|
||||
f"固定 SDK 映射点动时没有观察到目标关节运动：{task}。",
|
||||
"检查对应 Tag、通道映射和机械连接；不要交换通道后强行继续。",
|
||||
)
|
||||
if reason.startswith("o12_error_report_nonzero"):
|
||||
if reason.startswith("mapping_preflight_wrong_feedback_direction:"):
|
||||
task = reason.split(":", 1)[1]
|
||||
return (
|
||||
"O12-MAPPING-302",
|
||||
f"固定 SDK 映射点动的目标反馈方向不正确：{task}。",
|
||||
"检查对应 SDK 通道和机械连接；不要交换通道后强行继续。",
|
||||
)
|
||||
if reason.startswith("o12_active_motor_fault"):
|
||||
return (
|
||||
"O12-HEALTH-201",
|
||||
f"O12 返回非零错误码：{status.get('latest_error_codes', [])}。",
|
||||
"检查堵转、过流、过热或通信异常，排除后重新启动。",
|
||||
f"O12 返回活动电机故障：{status.get('error_faults', [])}。",
|
||||
"检查对应电机的堵转、过流、过热或电机异常；排除后从断点启动。",
|
||||
)
|
||||
if reason.startswith("o12_new_communication_fault"):
|
||||
return (
|
||||
"O12-HEALTH-202",
|
||||
f"运行中出现了新的 O12 通信异常：{status.get('error_faults', [])}。",
|
||||
"检查 HCAN 线缆、供电和对应电机通信；程序已保持当前位置。",
|
||||
)
|
||||
if reason.startswith("o12_feedback_stream_timeout"):
|
||||
return (
|
||||
"O12-HEALTH-203",
|
||||
"O12 命令触发反馈流中断超过 1 秒。",
|
||||
"检查 HCAN 连接、SDK 节点和供电；恢复后从断点启动。",
|
||||
)
|
||||
if reason.startswith("feedback_outside_registered_feedback_domain:"):
|
||||
return (
|
||||
"O12-FEEDBACK-202",
|
||||
"O12 反馈超出了已登记的物理反馈范围。",
|
||||
"保持当前位置，检查诊断中的通道、反馈值和机械端点。",
|
||||
)
|
||||
if reason == "calibration_node_status_timeout":
|
||||
return (
|
||||
"O12-NODE-500",
|
||||
"O12 标定节点已停止发布状态，运行栈将自动退出。",
|
||||
"查看本次 calibration.log 中的 Python traceback，并从断点重启。",
|
||||
)
|
||||
return _l6_reason_zh(status, model_name="O12")
|
||||
|
||||
@@ -76,12 +121,107 @@ def render_o12_progress_zh(status, estimator=None) -> str:
|
||||
if status.get("temperature_fallback_active")
|
||||
else "等待温度能力确认"
|
||||
)
|
||||
return text + (
|
||||
resume = status.get("resume", {})
|
||||
resume_text = ""
|
||||
if isinstance(resume, dict) and resume.get("used"):
|
||||
resume_text = (
|
||||
"\n断点恢复：已复用 "
|
||||
f"{int(resume.get('completed_unit_count', 0))} 个扫描单元；"
|
||||
f"来源 {resume.get('source_session', '-')}"
|
||||
)
|
||||
cross_view = status.get("roll_cross_view", {})
|
||||
cross_view_text = ""
|
||||
if isinstance(cross_view, dict) and cross_view.get("active"):
|
||||
recognized = "/".join(
|
||||
f"ID{int(value)}"
|
||||
for value in cross_view.get("recognized_tag_ids", ())
|
||||
) or "无"
|
||||
missing = "/".join(
|
||||
f"ID{int(value)}"
|
||||
for value in cross_view.get("unrecognized_tag_ids", ())
|
||||
) or "无"
|
||||
cross_view_text = (
|
||||
"\n侧摆双机位：侧面已识别 "
|
||||
f"{recognized}；未识别/不合格 {missing}；联合 "
|
||||
f"{int(cross_view.get('valid_frames', 0))}/"
|
||||
f"{int(cross_view.get('total_frames', 0))} 帧（"
|
||||
f"{float(cross_view.get('joint_frame_rate', 0.0)):.1%}）"
|
||||
)
|
||||
coupling = status.get("feedback_coupling", {})
|
||||
coupling_text = ""
|
||||
if isinstance(coupling, dict) and coupling.get("active"):
|
||||
stage = {
|
||||
"training_observation": "训练采集",
|
||||
"holdout_pending_full_sweep_validation": "第四轮整段验证",
|
||||
"vendor_solver_motion_prior": "运动准备",
|
||||
}.get(str(coupling.get("reference_kind", "")), "耦合观测")
|
||||
coupling_text = (
|
||||
"\nO12耦合："
|
||||
f"{coupling.get('coupled_channel', '-')} 位移 "
|
||||
f"{float(coupling.get('displacement_rad', 0.0)):.3f}/"
|
||||
f"{float(coupling.get('hard_displacement_limit_rad', 0.0)):.3f} rad；"
|
||||
f"{stage}；vendor偏差 "
|
||||
f"{float(coupling.get('residual_rad', 0.0)):.3f} rad（仅诊断）"
|
||||
)
|
||||
auxiliary = status.get("auxiliary_tracking", {})
|
||||
auxiliary_text = ""
|
||||
if isinstance(auxiliary, dict) and auxiliary.get("active"):
|
||||
channels = list(auxiliary.get("channels", ()))
|
||||
worst = max(
|
||||
channels,
|
||||
key=lambda item: float(item.get("error_rad", 0.0)),
|
||||
default={},
|
||||
)
|
||||
stage = (
|
||||
"平滑过渡中"
|
||||
if auxiliary.get("mode") == "transitioning"
|
||||
else "已稳定保持"
|
||||
if auxiliary.get("ready")
|
||||
else "等待稳定"
|
||||
)
|
||||
auxiliary_text = (
|
||||
"\nO12避让轴："
|
||||
f"{stage}；最大偏差 {worst.get('channel', '-')}="
|
||||
f"{float(worst.get('error_rad', 0.0)):.3f} rad"
|
||||
)
|
||||
locked_ids = list(status.get("locked_reference_tag_ids", ()))
|
||||
locked_reference_text = ""
|
||||
if locked_ids:
|
||||
locked_reference_text = (
|
||||
"\n固定基准："
|
||||
+ "/".join(f"ID{int(value)}" for value in locked_ids)
|
||||
+ " 已锁定，避让遮挡期间复用"
|
||||
)
|
||||
error_health = str(
|
||||
status.get("error_health_classification", "awaiting_error_report")
|
||||
)
|
||||
if error_health == "historical_communication_latch":
|
||||
channels = "/".join(
|
||||
str(value) for value in status.get(
|
||||
"confirmed_historical_communication_channels", ()
|
||||
)
|
||||
) or "未知通道"
|
||||
error_health_text = f"历史通信位已核验({channels})"
|
||||
elif error_health == "confirming_historical_communication":
|
||||
error_health_text = (
|
||||
"确认历史通信位 "
|
||||
f"{int(status.get('error_report_matching_count', 0))}/3"
|
||||
)
|
||||
elif error_health == "clear":
|
||||
error_health_text = "正常"
|
||||
else:
|
||||
error_health_text = error_health
|
||||
return (
|
||||
text + cross_view_text + coupling_text + auxiliary_text
|
||||
+ locked_reference_text + resume_text + (
|
||||
"\nO12安全：POSITION="
|
||||
f"{bool(status.get('position_mode_verified'))}；"
|
||||
f"错误码通道={bool(status.get('error_report_verified'))}；"
|
||||
f"错误码通道={bool(status.get('error_report_verified'))}"
|
||||
f"（{error_health_text}）；"
|
||||
f"温度策略={temperature}；速度倍率="
|
||||
f"{float(status.get('motion_speed_scale', 1.0)):.1f}x"
|
||||
f"{float(status.get('motion_speed_scale', 1.0)):.1f}x；"
|
||||
f"控制发布={float(status.get('command_publish_hz', 0.0)):.1f} Hz"
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
@@ -89,6 +229,7 @@ class _Monitor(Node):
|
||||
def __init__(self, progress: _ProgressConsole) -> None:
|
||||
super().__init__("o12_calibration_runner")
|
||||
self.status: dict[str, Any] = {}
|
||||
self.last_status_at = 0.0
|
||||
self.progress = progress
|
||||
self.create_subscription(String, "/o12_calibration/status", self._status, 10)
|
||||
self.start_client = self.create_client(Trigger, "/o12_calibration/start")
|
||||
@@ -101,9 +242,76 @@ class _Monitor(Node):
|
||||
return
|
||||
if isinstance(value, dict):
|
||||
self.status = value
|
||||
self.last_status_at = time.monotonic()
|
||||
self.progress.update(value)
|
||||
|
||||
|
||||
def _wait_until_o12(
|
||||
monitor: _Monitor,
|
||||
process: subprocess.Popen[Any],
|
||||
predicate,
|
||||
*,
|
||||
timeout: float | None,
|
||||
status_stale_after: float = 3.0,
|
||||
) -> bool:
|
||||
"""Wait for O12 while also detecting a dead calibration child node."""
|
||||
started = time.monotonic()
|
||||
while rclpy.ok():
|
||||
if process.poll() is not None:
|
||||
return False
|
||||
rclpy.spin_once(monitor, timeout_sec=0.2)
|
||||
if predicate(monitor.status):
|
||||
return True
|
||||
now = time.monotonic()
|
||||
stale_limit = (
|
||||
180.0
|
||||
if monitor.status.get("state") == "FINALIZING"
|
||||
else float(status_stale_after)
|
||||
)
|
||||
if (
|
||||
monitor.last_status_at > 0.0
|
||||
and now - monitor.last_status_at > stale_limit
|
||||
):
|
||||
status_publishers = monitor.count_publishers(
|
||||
"/o12_calibration/status"
|
||||
)
|
||||
monitor.status = {
|
||||
**monitor.status,
|
||||
"state": "ABORTED",
|
||||
"reason": (
|
||||
"calibration_node_process_exited"
|
||||
if status_publishers == 0
|
||||
else "calibration_node_status_timeout"
|
||||
),
|
||||
}
|
||||
return False
|
||||
if timeout is not None and now - started > timeout:
|
||||
return False
|
||||
return False
|
||||
|
||||
|
||||
def _log_exception_summary(log_path: Path) -> str:
|
||||
"""Return the final child exception instead of hiding it as status loss."""
|
||||
try:
|
||||
lines = log_path.read_text(
|
||||
encoding="utf-8", errors="replace"
|
||||
).splitlines()
|
||||
except OSError:
|
||||
return ""
|
||||
prefixes = (
|
||||
"AttributeError:", "AssertionError:", "ImportError:",
|
||||
"IndexError:", "KeyError:", "ModuleNotFoundError:",
|
||||
"OSError:", "RuntimeError:", "TypeError:", "ValueError:",
|
||||
)
|
||||
ansi = re.compile(r"\x1b\[[0-9;]*m")
|
||||
for raw in reversed(lines[-300:]):
|
||||
line = ansi.sub("", raw).strip()
|
||||
payload = line.rsplit("] ", 1)[-1].strip()
|
||||
if payload.startswith(prefixes):
|
||||
return payload
|
||||
return ""
|
||||
|
||||
|
||||
def _overlay_environment(setup: Path) -> dict[str, str]:
|
||||
completed = subprocess.run(
|
||||
["bash", "-c", 'source "$1" >/dev/null 2>&1; env -0', "bash", str(setup)],
|
||||
@@ -118,39 +326,140 @@ def _overlay_environment(setup: Path) -> dict[str, str]:
|
||||
return environment
|
||||
|
||||
|
||||
def _protected_inputs(config: ProductConfig) -> dict[str, str]:
|
||||
return {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"tag_config_sha256": config.tag_config_sha256,
|
||||
"sdk_config_sha256": config.sdk_config_sha256,
|
||||
}
|
||||
|
||||
|
||||
def _finalize_completed_resume(config, resume, session: Path) -> int:
|
||||
"""Publish a complete checkpoint without touching calibration hardware."""
|
||||
expected = ordered_resume_units(
|
||||
config.calibration_contract.typed_profile
|
||||
)
|
||||
if tuple(resume.completed_units) != tuple(expected):
|
||||
raise ValueError("completed-resume finalization requires every scan unit")
|
||||
print(
|
||||
"O12 完整断点已包含全部 "
|
||||
f"{len(expected)} 个扫描单元；不启动 SDK、相机或避让运动，"
|
||||
"直接拟合、验证并生成 URDF（约需 1 分钟）。",
|
||||
flush=True,
|
||||
)
|
||||
try:
|
||||
payload, _fit, correction = finalize_o12_session(
|
||||
session_dir=session,
|
||||
serial_number=config.serial_number,
|
||||
source_urdf=config.source_urdf,
|
||||
protected_inputs=_protected_inputs(config),
|
||||
# The checkpoint builder imports only matching, passed scan
|
||||
# transactions plus their PnP provenance. This also avoids
|
||||
# recursively replaying prior session/audit wrapper records.
|
||||
records=list(resume.imported_records),
|
||||
publish=True,
|
||||
)
|
||||
except BaseException as error:
|
||||
print(
|
||||
"O12 完整断点的拟合、验证或 URDF 写回失败："
|
||||
f"{error}",
|
||||
flush=True,
|
||||
)
|
||||
return 3
|
||||
print("\n".join((
|
||||
"PASS：O12 完整断点已通过拟合和独立 holdout；"
|
||||
"thumb_mcp 静态零位保留原始 CAD。",
|
||||
f"发布结果：{config.session_root / 'latest_passed'}",
|
||||
f"JSON：{session / config.calibration_contract.typed_profile.artifacts.calibration_filename.format(serial_number=config.serial_number)}",
|
||||
f"URDF：{correction.path}",
|
||||
)), flush=True)
|
||||
return 0
|
||||
|
||||
|
||||
def _run_online(
|
||||
config: ProductConfig, *, record_bag: bool, commands_enabled: bool
|
||||
config: ProductConfig, *, record_bag: bool, commands_enabled: bool,
|
||||
allow_resume: bool,
|
||||
) -> int:
|
||||
if config.sdk_setup is None or config.sdk_config is None:
|
||||
raise ValueError("O12 vendor SDK overlay/config are required")
|
||||
resume = automatic_resume_candidate(config) if allow_resume else None
|
||||
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
while session.exists():
|
||||
time.sleep(1.0)
|
||||
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session.mkdir(parents=True)
|
||||
expected_resume_units = ordered_resume_units(
|
||||
config.calibration_contract.typed_profile
|
||||
)
|
||||
if (
|
||||
resume is not None
|
||||
and tuple(resume.completed_units) == tuple(expected_resume_units)
|
||||
):
|
||||
return _finalize_completed_resume(config, resume, session)
|
||||
log_path = session / "calibration.log"
|
||||
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
|
||||
print(f"O12 标定环境正在启动；日志：{log_path}", flush=True)
|
||||
process = subprocess.Popen(
|
||||
_launch_command(
|
||||
config, session, record_bag=record_bag,
|
||||
commands_enabled=commands_enabled,
|
||||
),
|
||||
cwd=config.workspace,
|
||||
env=_overlay_environment(config.sdk_setup),
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
rclpy.init()
|
||||
monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh))
|
||||
process: subprocess.Popen[str] | None = None
|
||||
try:
|
||||
ready = _wait_until(
|
||||
# Discover already-running SDK/GUI/calibration publishers before this
|
||||
# command starts its own vendor node. Otherwise a vendor open failure
|
||||
# could be masked by feedback from the stale process holding HCAN.
|
||||
discovery_deadline = time.monotonic() + 1.5
|
||||
while rclpy.ok() and time.monotonic() < discovery_deadline:
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
existing_state = monitor.count_publishers("/o12/right/joint_states")
|
||||
existing_command = monitor.count_publishers("/o12/right/joint_cmd")
|
||||
if existing_state or existing_command:
|
||||
print(
|
||||
"O12 启动前独占检查失败：检测到既有 SDK/GUI/标定进程"
|
||||
f"（状态发布者={existing_state}，命令发布者={existing_command}）。"
|
||||
"请先关闭它们，再只运行本标定命令。",
|
||||
flush=True,
|
||||
)
|
||||
return 2
|
||||
print(f"O12 标定环境正在启动；日志：{log_path}", flush=True)
|
||||
if resume is not None:
|
||||
print(
|
||||
"O12 断点恢复：来源 "
|
||||
f"{resume.source_session}，复用 "
|
||||
f"{len(resume.completed_units)} 个已通过扫描单元，"
|
||||
"启动后先恢复安全姿态。",
|
||||
flush=True,
|
||||
)
|
||||
process = subprocess.Popen(
|
||||
_launch_command(
|
||||
config, session, record_bag=record_bag,
|
||||
commands_enabled=commands_enabled,
|
||||
resume_from=(
|
||||
None if resume is None else resume.source_session
|
||||
),
|
||||
),
|
||||
cwd=config.workspace,
|
||||
env=_overlay_environment(config.sdk_setup),
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
ready = _wait_until_o12(
|
||||
monitor, process,
|
||||
lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"},
|
||||
timeout=120.0,
|
||||
)
|
||||
if not ready and not monitor.status:
|
||||
exception = _log_exception_summary(log_path)
|
||||
print(
|
||||
"O12 标定节点初始化超时：设备进程已启动，但标定节点未发布状态；"
|
||||
"请查看日志中的节点构造或断点导入阶段。"
|
||||
f"日志：{log_path}",
|
||||
flush=True,
|
||||
)
|
||||
if exception:
|
||||
print(f"标定节点异常：{exception}", flush=True)
|
||||
return 2
|
||||
if not ready or monitor.status.get("state") != "READY":
|
||||
print(
|
||||
"O12 启动预检失败：请检查 HCAN、POSITION 模式、错误/温度回读、"
|
||||
@@ -167,21 +476,29 @@ def _run_online(
|
||||
if response is None or not response.success:
|
||||
print(f"O12 标定未启动：{getattr(response, 'message', '')}", flush=True)
|
||||
return 2
|
||||
print("O12 标定已自动开始：POSITION，20 Hz，弧度余弦轨迹。", flush=True)
|
||||
finished = _wait_until(
|
||||
print(
|
||||
"O12 标定已自动开始：POSITION，50 Hz，平滑限速轨迹。",
|
||||
flush=True,
|
||||
)
|
||||
finished = _wait_until_o12(
|
||||
monitor, process,
|
||||
lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"},
|
||||
timeout=None,
|
||||
)
|
||||
if not finished or monitor.status.get("state") != "PASSED":
|
||||
exception = _log_exception_summary(log_path)
|
||||
print(
|
||||
"O12 标定已安全停止并保持当前位置："
|
||||
+ str(monitor.status.get("reason", "process_exit")),
|
||||
flush=True,
|
||||
)
|
||||
if exception:
|
||||
print(f"标定节点异常：{exception}", flush=True)
|
||||
print(f"诊断日志：{log_path}", flush=True)
|
||||
return 3
|
||||
print("\n".join((
|
||||
"PASS：O12 右手 11 个实测任务和独立 holdout 已通过。",
|
||||
"PASS：O12 右手 11 个实测任务和独立 holdout 已通过；"
|
||||
"thumb_mcp 静态零位保留原始 CAD。",
|
||||
f"发布结果：{config.session_root / 'latest_passed'}",
|
||||
f"JSON：{monitor.status.get('final_json')}",
|
||||
f"URDF：{monitor.status.get('final_urdf')}",
|
||||
@@ -196,7 +513,8 @@ def _run_online(
|
||||
monitor.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
_stop_stack(process)
|
||||
if process is not None:
|
||||
_stop_stack(process)
|
||||
log_stream.flush()
|
||||
os.fsync(log_stream.fileno())
|
||||
log_stream.close()
|
||||
@@ -212,6 +530,10 @@ def main(args: list[str] | None = None) -> None:
|
||||
parser.add_argument("--offline-raw", default="")
|
||||
parser.add_argument("--offline-output", default="")
|
||||
parser.add_argument("--publish-offline", action="store_true")
|
||||
parser.add_argument(
|
||||
"--no-resume", action="store_true",
|
||||
help="忽略兼容的失败会话，从头开始采集",
|
||||
)
|
||||
selected = parser.parse_args(args)
|
||||
config = load_product_config(
|
||||
selected.config,
|
||||
@@ -234,22 +556,20 @@ def main(args: list[str] | None = None) -> None:
|
||||
session_dir=output,
|
||||
serial_number=config.serial_number,
|
||||
source_urdf=config.source_urdf,
|
||||
protected_inputs={
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"tag_config_sha256": config.tag_config_sha256,
|
||||
"sdk_config_sha256": config.sdk_config_sha256,
|
||||
},
|
||||
protected_inputs=_protected_inputs(config),
|
||||
records=load_o12_raw_samples(selected.offline_raw),
|
||||
publish=selected.publish_offline,
|
||||
)
|
||||
print(f"离线回放PASS：schema {payload['schema_version']}，URDF {correction.path}")
|
||||
print(
|
||||
f"离线回放PASS：schema {payload['schema_version']}，"
|
||||
f"thumb_mcp 静态零位保留原始 CAD，URDF {correction.path}"
|
||||
)
|
||||
return
|
||||
raise SystemExit(_run_online(
|
||||
config,
|
||||
record_bag=selected.record_bag,
|
||||
commands_enabled=not selected.commands_disabled,
|
||||
allow_resume=not selected.no_resume,
|
||||
))
|
||||
|
||||
|
||||
|
||||
@@ -10,23 +10,144 @@ import re
|
||||
from typing import Mapping
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches
|
||||
from .fitting import O12FitResult
|
||||
import numpy as np
|
||||
|
||||
from ...core.urdf import (
|
||||
UrdfJointPatch,
|
||||
UrdfPatchSet,
|
||||
validate_urdf_mimic_ranges,
|
||||
write_urdf_patches,
|
||||
)
|
||||
from .fitting import O12FitResult, measured_curve_bounds
|
||||
from .kinematics import PASSIVE_SDK_SOURCE_BY_JOINT, vendor_passive_curve
|
||||
from ..l6.urdf import _corrected_origin_rpy
|
||||
from .profile import (
|
||||
ACTIVE_JOINTS,
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
COMMAND_INDEX_BY_JOINT,
|
||||
MEASURED_PASSIVE_JOINTS,
|
||||
MIMIC_SOURCE_BY_JOINT,
|
||||
SDK_TO_URDF_SIGN,
|
||||
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
)
|
||||
|
||||
|
||||
SPLAY_JOINTS = frozenset(
|
||||
{"index_mcp_roll", "middle_mcp_roll"}
|
||||
)
|
||||
SPLAY_JOINTS = frozenset({"index_mcp_roll", "middle_mcp_roll"})
|
||||
|
||||
|
||||
def _measured_curve_bounds(
|
||||
name: str, result: O12FitResult, *, sign: float
|
||||
) -> tuple[float, float]:
|
||||
donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
||||
return measured_curve_bounds(
|
||||
donor,
|
||||
result.curves[donor],
|
||||
result.feedback_domains_rad[donor],
|
||||
sign=sign,
|
||||
)
|
||||
|
||||
|
||||
def _source_reachable_upper(
|
||||
name: str,
|
||||
joints: Mapping[str, ET.Element],
|
||||
cache: dict[str, float],
|
||||
) -> float:
|
||||
if name in cache:
|
||||
return cache[name]
|
||||
node = joints[name]
|
||||
limit = node.find("limit")
|
||||
if limit is None or limit.get("upper") is None:
|
||||
raise ValueError(f"source URDF joint has no upper limit: {name}")
|
||||
mimic = node.find("mimic")
|
||||
if mimic is None:
|
||||
value = float(limit.get("upper"))
|
||||
else:
|
||||
source_name = str(mimic.get("joint", ""))
|
||||
if source_name not in joints:
|
||||
raise ValueError(f"source URDF mimic source is missing: {source_name}")
|
||||
value = (
|
||||
float(mimic.get("offset", "0"))
|
||||
+ float(mimic.get("multiplier", "1"))
|
||||
* _source_reachable_upper(source_name, joints, cache)
|
||||
)
|
||||
if not math.isfinite(value):
|
||||
raise ValueError(f"source URDF reachable endpoint is invalid: {name}")
|
||||
cache[name] = value
|
||||
return value
|
||||
|
||||
|
||||
def o12_active_ranges(result: O12FitResult) -> dict[str, tuple[float, float]]:
|
||||
"""Return the exact active ranges used by O12 JSON and URDF publication."""
|
||||
ranges: dict[str, tuple[float, float]] = {}
|
||||
for name in sorted(
|
||||
CALIBRATED_ACTIVE_JOINTS | set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT)
|
||||
):
|
||||
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
|
||||
continue
|
||||
measured_lower, measured_upper = _measured_curve_bounds(
|
||||
name,
|
||||
result,
|
||||
sign=SDK_TO_URDF_SIGN[COMMAND_INDEX_BY_JOINT[name]],
|
||||
)
|
||||
ranges[name] = (
|
||||
(measured_lower, measured_upper)
|
||||
if name in SPLAY_JOINTS
|
||||
else (min(0.0, measured_lower), max(0.0, measured_upper))
|
||||
)
|
||||
return ranges
|
||||
|
||||
|
||||
def o12_endpoint_mimic_contract(
|
||||
source_urdf: str | Path,
|
||||
result: O12FitResult,
|
||||
) -> dict[str, float]:
|
||||
"""Linear URDF mimics that preserve the source-CAD closed chain pose.
|
||||
|
||||
O12 runtime uses the nonlinear vendor solver. URDF ``mimic`` is only a
|
||||
linear visualization fallback, so choose its coefficient to reproduce the
|
||||
reviewed CAD mechanical endpoint rather than fitting an arbitrary planar
|
||||
Tag phase. Chained pinky DIP is resolved after pinky PIP.
|
||||
"""
|
||||
root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot()
|
||||
joints = {
|
||||
str(node.get("name")): node
|
||||
for node in root.findall("joint")
|
||||
if node.get("type") == "revolute"
|
||||
}
|
||||
reachable_source: dict[str, float] = {}
|
||||
desired = {
|
||||
name: _source_reachable_upper(name, joints, reachable_source)
|
||||
for name in MEASURED_PASSIVE_JOINTS
|
||||
}
|
||||
corrected_source_upper = {
|
||||
name: values[1] for name, values in o12_active_ranges(result).items()
|
||||
}
|
||||
multipliers: dict[str, float] = {}
|
||||
for name in (
|
||||
"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"
|
||||
):
|
||||
node = joints[name]
|
||||
mimic = node.find("mimic")
|
||||
if mimic is None:
|
||||
raise ValueError(f"O12 passive joint has no mimic element: {name}")
|
||||
source_name = MIMIC_SOURCE_BY_JOINT[name]
|
||||
source_endpoint = corrected_source_upper.get(source_name, desired.get(source_name))
|
||||
if source_endpoint is None or abs(source_endpoint) <= 1.0e-12:
|
||||
raise ValueError(f"O12 mimic source endpoint is invalid: {source_name}")
|
||||
offset = float(mimic.get("offset", "0"))
|
||||
multiplier = (desired[name] - offset) / source_endpoint
|
||||
if not math.isfinite(multiplier) or multiplier <= 0.0:
|
||||
raise ValueError(f"O12 endpoint mimic is invalid: {name}")
|
||||
multipliers[name] = multiplier
|
||||
corrected_source_upper[name] = desired[name]
|
||||
return multipliers
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class O12UrdfCorrection:
|
||||
path: Path
|
||||
origin_offsets_rad: Mapping[str, float]
|
||||
corrected_limits_rad: Mapping[str, tuple[float, float]]
|
||||
mimic_multipliers: Mapping[str, float]
|
||||
preserved_ring_fields: tuple[str, ...]
|
||||
@@ -40,11 +161,13 @@ def write_o12_corrected_urdf(
|
||||
result: O12FitResult,
|
||||
timestamp: str | None = None,
|
||||
) -> O12UrdfCorrection:
|
||||
"""Patch only measured travel and dynamic mimic terms.
|
||||
"""Write full-range visual joint limits and equivalent endpoint mimics.
|
||||
|
||||
Tag mounting angle cannot be separated from a static passive-joint zero,
|
||||
so origins and passive limits stay byte-for-byte CAD. The unobserved ring
|
||||
keeps its own origin, limits and both CAD mimic ratios.
|
||||
O12 SDK feedback is the continuous input coordinate, while the 16-Tag
|
||||
trajectories identify the corresponding 19-joint URDF motion. The source
|
||||
CAD is never allowed to truncate acquisition. Ring geometry and mimic
|
||||
ratios remain CAD-owned; its active range remains constrained by that
|
||||
preserved chain.
|
||||
"""
|
||||
source = Path(source_urdf).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
@@ -57,6 +180,23 @@ def write_o12_corrected_urdf(
|
||||
raise ValueError("O12 travel result has the wrong active joint set")
|
||||
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
|
||||
raise ValueError("O12 mimic result has the wrong passive joint set")
|
||||
for name in STATIC_ZERO_EXCLUDED_JOINTS:
|
||||
if (
|
||||
not math.isclose(
|
||||
float(result.zero_offsets_rad.get(name, math.nan)),
|
||||
0.0,
|
||||
rel_tol=0.0,
|
||||
abs_tol=1.0e-12,
|
||||
)
|
||||
or result.zero_method_by_joint.get(name)
|
||||
!= "source_cad_zero_profile_excluded"
|
||||
):
|
||||
raise ValueError(
|
||||
f"O12 {name} must retain immutable source-CAD static zero"
|
||||
)
|
||||
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
|
||||
if not math.isclose(result.zero_offsets_rad[target], result.zero_offsets_rad[donor], abs_tol=1.0e-9, rel_tol=0.0):
|
||||
raise ValueError(f"{target} static zero differs from transfer donor {donor}")
|
||||
|
||||
root = ET.parse(source).getroot()
|
||||
joints = {
|
||||
@@ -72,40 +212,95 @@ def write_o12_corrected_urdf(
|
||||
|
||||
corrected_limits: dict[str, tuple[float, float]] = {}
|
||||
patches: dict[str, UrdfJointPatch] = {}
|
||||
for name in sorted(CALIBRATED_ACTIVE_JOINTS):
|
||||
active_targets = CALIBRATED_ACTIVE_JOINTS | set(
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
|
||||
)
|
||||
measured_active_ranges = o12_active_ranges(result)
|
||||
active_ranges: dict[str, tuple[float, float]] = {}
|
||||
for name in sorted(active_targets):
|
||||
limit = joints[name].find("limit")
|
||||
if limit is None:
|
||||
raise ValueError(f"O12 source active joint has no limit: {name}")
|
||||
cad_lower = float(limit.get("lower", "nan"))
|
||||
cad_upper = float(limit.get("upper", "nan"))
|
||||
lower_text = limit.get("lower")
|
||||
upper_text = limit.get("upper")
|
||||
if lower_text is None or upper_text is None:
|
||||
raise ValueError(f"O12 source active joint has incomplete limits: {name}")
|
||||
cad_lower = float(lower_text)
|
||||
cad_upper = float(upper_text)
|
||||
travel = abs(float(result.travels_rad[name]))
|
||||
if not math.isfinite(travel) or travel <= math.radians(2.0):
|
||||
raise ValueError(f"invalid measured O12 travel: {name}")
|
||||
if name in SPLAY_JOINTS:
|
||||
half = min(0.5 * travel, abs(cad_lower), abs(cad_upper))
|
||||
lower, upper = -half, half
|
||||
else:
|
||||
lower, upper = max(0.0, cad_lower), min(cad_upper, travel)
|
||||
if not lower < upper:
|
||||
if not cad_lower < cad_upper:
|
||||
raise ValueError(f"invalid corrected O12 limits: {name}")
|
||||
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT:
|
||||
# The ring has no Tag. Preserve its own reviewed CAD range and
|
||||
# passive-chain contract rather than transplanting pinky geometry.
|
||||
lower, upper = cad_lower, cad_upper
|
||||
else:
|
||||
lower, upper = measured_active_ranges[name]
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper:
|
||||
raise ValueError(f"invalid measured O12 limits: {name}")
|
||||
corrected_limits[name] = (lower, upper)
|
||||
active_ranges[name] = (lower, upper)
|
||||
origin_offset = float(result.zero_offsets_rad.get(name, 0.0))
|
||||
patches[name] = UrdfJointPatch(
|
||||
origin_rpy=(
|
||||
_corrected_origin_rpy(joints[name], origin_offset)
|
||||
if abs(origin_offset) > 1.0e-12 else None
|
||||
),
|
||||
limit_lower=f"{lower:.15g}", limit_upper=f"{upper:.15g}"
|
||||
)
|
||||
|
||||
mimic_multipliers: dict[str, float] = {}
|
||||
mimic_multipliers = o12_endpoint_mimic_contract(source, result)
|
||||
for name in sorted(MEASURED_PASSIVE_JOINTS):
|
||||
mimic = joints[name].find("mimic")
|
||||
if mimic is None:
|
||||
raise ValueError(f"O12 passive joint has no mimic element: {name}")
|
||||
multiplier = float(result.mimic_fits[name].urdf_mimic_multiplier)
|
||||
multiplier = float(mimic_multipliers[name])
|
||||
if not math.isfinite(multiplier) or not 0.5 <= multiplier <= 2.2:
|
||||
raise ValueError(f"invalid O12 mimic multiplier: {name}")
|
||||
mimic_multipliers[name] = multiplier
|
||||
patches[name] = UrdfJointPatch(
|
||||
mimic_multiplier=f"{multiplier:.15g}"
|
||||
)
|
||||
|
||||
# Passive limits remain CAD-owned unless the reviewed vendor polynomial
|
||||
# itself has a small interior extremum outside that range (index DIP does).
|
||||
# Never expand a physical limit from the noisier Tag-derived curve.
|
||||
for name in (
|
||||
"thumb_dip", "index_dip", "middle_dip", "pinky_pip", "pinky_dip"
|
||||
):
|
||||
node = joints[name]
|
||||
limit = node.find("limit")
|
||||
assert limit is not None
|
||||
cad_lower = float(limit.get("lower", "nan"))
|
||||
cad_upper = float(limit.get("upper", "nan"))
|
||||
sdk_source = PASSIVE_SDK_SOURCE_BY_JOINT[name]
|
||||
feedback_lower, feedback_upper = result.feedback_domains_rad[name]
|
||||
inputs = np.linspace(
|
||||
min(0.0, feedback_lower), max(0.0, feedback_upper), 257
|
||||
)
|
||||
motor = COMMAND_INDEX_BY_JOINT[sdk_source]
|
||||
vendor_values = vendor_passive_curve(
|
||||
name,
|
||||
inputs,
|
||||
sdk_to_urdf_sign=SDK_TO_URDF_SIGN[motor],
|
||||
)
|
||||
lower = min(cad_lower, *vendor_values)
|
||||
upper = max(cad_upper, *vendor_values)
|
||||
if not lower < upper:
|
||||
raise ValueError(f"invalid corrected O12 passive limits: {name}")
|
||||
corrected_limits[name] = (lower, upper)
|
||||
existing = patches[name]
|
||||
patches[name] = UrdfJointPatch(
|
||||
limit_lower=(
|
||||
f"{lower:.15g}" if lower < cad_lower - 1.0e-12 else None
|
||||
),
|
||||
limit_upper=(
|
||||
f"{upper:.15g}" if upper > cad_upper + 1.0e-12 else None
|
||||
),
|
||||
mimic_multiplier=existing.mimic_multiplier,
|
||||
)
|
||||
|
||||
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
if re.fullmatch(r"\d{8}_\d{6}", stamp) is None:
|
||||
raise ValueError("O12 URDF timestamp must use YYYYMMDD_HHMMSS")
|
||||
@@ -125,16 +320,32 @@ def write_o12_corrected_urdf(
|
||||
forbidden_source_stem_patterns=(r"calibrated",),
|
||||
copy_complete_mesh_directory=True,
|
||||
)
|
||||
try:
|
||||
validate_urdf_mimic_ranges(destination, reference_urdf=source)
|
||||
except Exception:
|
||||
# Publication is transactional: a generated file that failed the
|
||||
# physical mimic-chain gate must never remain as a plausible result.
|
||||
destination.unlink(missing_ok=True)
|
||||
raise
|
||||
return O12UrdfCorrection(
|
||||
path=destination,
|
||||
origin_offsets_rad={
|
||||
name: float(result.zero_offsets_rad[name])
|
||||
for name in ACTIVE_JOINTS
|
||||
},
|
||||
corrected_limits_rad=corrected_limits,
|
||||
mimic_multipliers=mimic_multipliers,
|
||||
preserved_ring_fields=(
|
||||
"ring_mcp_pitch.origin", "ring_mcp_pitch.limit",
|
||||
"ring_mcp_pitch.origin.xyz", "ring_mcp_pitch.limit",
|
||||
"ring_pip.origin", "ring_pip.limit", "ring_pip.mimic",
|
||||
"ring_dip.origin", "ring_dip.limit", "ring_dip.mimic",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["O12UrdfCorrection", "write_o12_corrected_urdf"]
|
||||
__all__ = [
|
||||
"O12UrdfCorrection",
|
||||
"o12_active_ranges",
|
||||
"o12_endpoint_mimic_contract",
|
||||
"write_o12_corrected_urdf",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,180 @@
|
||||
"""O12 observation graph for the shared G20 spatial-zero solver.
|
||||
|
||||
Parallel flexion axes observe phase through their line positions, not their
|
||||
directions. SDK travel endpoints are deliberately not treated as CAD datums.
|
||||
Passive joints supply observations but never independently fitted static zeros.
|
||||
"""
|
||||
|
||||
from dataclasses import asdict, replace
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
from ..g20.profile import JointCurveFit, JointSpec
|
||||
from ..g20.zero_solver import (
|
||||
ZeroCalibrationProfile, ZeroSolveResult,
|
||||
fit_joint_axis_measurement, solve_urdf_zero_offsets,
|
||||
with_depth_free_axis_projection,
|
||||
)
|
||||
from .profile import (
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
COMMAND_INDEX_BY_JOINT,
|
||||
)
|
||||
from .kinematics import PASSIVE_SDK_SOURCE_BY_JOINT
|
||||
|
||||
SPATIAL_ZERO_POLICY = (
|
||||
"o12_full_hand_spatial_v5_thumb_mcp_cad_static_stable_pnp_bias"
|
||||
)
|
||||
|
||||
AXIS_PARENT = {
|
||||
"thumb_cmc_yaw": "thumb_cmc_roll",
|
||||
"thumb_cmc_pitch": "thumb_cmc_yaw",
|
||||
"index_mcp_pitch": "index_mcp_roll",
|
||||
"middle_mcp_pitch": "middle_mcp_roll",
|
||||
}
|
||||
PHASE_PARENT = {
|
||||
"thumb_mcp": "thumb_cmc_pitch",
|
||||
"index_pip": "index_mcp_pitch", "index_dip": "index_pip",
|
||||
"middle_pip": "middle_mcp_pitch", "middle_dip": "middle_pip",
|
||||
"pinky_pip": "pinky_mcp_pitch",
|
||||
}
|
||||
ZERO_OBSERVER = {
|
||||
"thumb_cmc_roll": "thumb_cmc_yaw",
|
||||
"thumb_cmc_yaw": "thumb_cmc_pitch",
|
||||
"thumb_cmc_pitch": "thumb_mcp",
|
||||
"index_mcp_roll": "index_mcp_pitch",
|
||||
"index_mcp_pitch": "index_pip", "index_pip": "index_dip",
|
||||
"middle_mcp_roll": "middle_mcp_pitch",
|
||||
"middle_mcp_pitch": "middle_pip", "middle_pip": "middle_dip",
|
||||
"pinky_mcp_pitch": "pinky_pip",
|
||||
}
|
||||
# Order also ensures an upstream axis exists before its passive observer.
|
||||
AXIS_JOINTS = (
|
||||
"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp",
|
||||
"pinky_mcp_pitch", "pinky_pip",
|
||||
"middle_mcp_roll", "middle_mcp_pitch", "middle_pip", "middle_dip",
|
||||
"index_mcp_roll", "index_mcp_pitch", "index_pip", "index_dip",
|
||||
)
|
||||
|
||||
|
||||
def motor_index(joint: str) -> int:
|
||||
return COMMAND_INDEX_BY_JOINT[PASSIVE_SDK_SOURCE_BY_JOINT.get(joint, joint)]
|
||||
|
||||
|
||||
def full_hand_zero_profile() -> ZeroCalibrationProfile:
|
||||
# Reuse the existing root orientation datum; adding flexion phases must
|
||||
# not silently change the palm frame or the fixed SDK channel contract.
|
||||
from .fitting import _thumb_root_zero_profile
|
||||
root = _thumb_root_zero_profile()
|
||||
specs = {
|
||||
name: JointSpec(name, motor_index(name), name in CALIBRATED_ACTIVE_JOINTS,
|
||||
None, None, None, pose_axis_line_required=name in PHASE_PARENT)
|
||||
for name in AXIS_JOINTS
|
||||
}
|
||||
return replace(
|
||||
root, hand=replace(root.hand, joint_specs=specs),
|
||||
direct_zero_joints=tuple(ZERO_OBSERVER), axis_joints=AXIS_JOINTS,
|
||||
constrained_circle_joints=frozenset(AXIS_JOINTS),
|
||||
axis_parent_joint=AXIS_PARENT, phase_parent_joint=PHASE_PARENT,
|
||||
offset_observer_joint=ZERO_OBSERVER,
|
||||
accept_validated_zero_in_confidence_interval=True,
|
||||
project_axis_gauge_before_image=True,
|
||||
)
|
||||
|
||||
|
||||
class O12SpatialZeroError(ValueError):
|
||||
"""Unpublishable geometry, with machine-readable per-joint evidence."""
|
||||
|
||||
def __init__(self, message: str, diagnostics: Mapping[str, Any]) -> None:
|
||||
super().__init__(message)
|
||||
self.diagnostics = diagnostics
|
||||
# Populated only after the full dynamic fit and an identifiable
|
||||
# spatial estimate exist. Never a substitute for a passed result.
|
||||
self.review_fit = None
|
||||
|
||||
|
||||
def solve_full_hand_zero(
|
||||
source_urdf: str | Path,
|
||||
virtual_records: Mapping[str, Sequence[Mapping[str, Any]]],
|
||||
solver_curves: Mapping[str, JointCurveFit],
|
||||
) -> ZeroSolveResult:
|
||||
profile = full_hand_zero_profile()
|
||||
measurements = []
|
||||
for cycle in range(4):
|
||||
by_joint = {}
|
||||
for name in AXIS_JOINTS:
|
||||
rows = virtual_records[name]
|
||||
# Relative passive motion is measured in its moving parent Tag
|
||||
# frame. Its common-frame line is evaluated at the same baseline
|
||||
# condition, just as in G20; it is not a circle of the entire chain.
|
||||
parent = PHASE_PARENT.get(name)
|
||||
# PHASE_PARENT declares parallel physical flexion axes, including
|
||||
# ACTIVE PIP/MCP axes. Independent planar-PnP direction errors must
|
||||
# not turn those into skew lines and then appear as encoder zeros.
|
||||
# The task profile holds nonparallel upstream axes at the same
|
||||
# pose for these paired captures. Preserve each child's measured
|
||||
# scalar rotation; constrain only the shared physical direction.
|
||||
constraint = by_joint[parent].axis_common_xyz if parent in by_joint else None
|
||||
try:
|
||||
item = fit_joint_axis_measurement(
|
||||
name, rows, cycle=cycle, zero_command_u8=255,
|
||||
constrained_circle_joints=profile.constrained_circle_joints,
|
||||
view_normal_common_xyz=rows[0]["view_normal_common_xyz"],
|
||||
canonical_zero_direction="decreasing",
|
||||
axis_common_constraint=constraint,
|
||||
separate_axial_residual=True,
|
||||
)
|
||||
except ValueError as error:
|
||||
raise O12SpatialZeroError(
|
||||
f"O12 spatial zero observation failed:{name}:cycle={cycle}:{error}",
|
||||
{"passed": False, "stage": "axis_observation", "joint": name,
|
||||
"cycle": cycle, "reason": str(error)},
|
||||
) from error
|
||||
item = with_depth_free_axis_projection(item, rows[0]["camera_center_common_xyz_m"])
|
||||
measurements.append(item)
|
||||
by_joint[name] = item
|
||||
try:
|
||||
result = solve_urdf_zero_offsets(
|
||||
source_urdf=source_urdf, measurements=measurements, curves=solver_curves,
|
||||
motor_by_joint={name: motor_index(name) for name in AXIS_JOINTS},
|
||||
training_cycles=(0, 1, 2), validation_cycle=3,
|
||||
maximum_offset_rad=math.radians(20), finger_maximum_offset_rad=math.radians(20),
|
||||
maximum_cycle_difference_rad=math.radians(.75),
|
||||
minimum_applied_offset_rad=math.radians(.1),
|
||||
maximum_validation_mae_rad=math.radians(1),
|
||||
maximum_validation_p95_rad=math.radians(2),
|
||||
maximum_validation_error_rad=math.radians(3),
|
||||
maximum_confidence_half_width_rad=math.radians(1),
|
||||
# Match G20's established treatment of a repeatable planar-Tag
|
||||
# direction bias: 5 deg remains the normal cone target, while a
|
||||
# stable (<=0.75 deg cycle spread) bias up to 15 deg is retained
|
||||
# as an explicit diagnostic instead of corrupting/rejecting an
|
||||
# otherwise observable encoder zero.
|
||||
maximum_systematic_axis_cone_bias_rad=math.radians(15),
|
||||
maximum_pose_axis_line_rms_m=.0015,
|
||||
hand_type="right", tag_layout="o12_right_16", zero_profile=profile,
|
||||
)
|
||||
except ValueError as error:
|
||||
raise O12SpatialZeroError(
|
||||
"O12 full-hand spatial zero geometry is invalid:" + str(error),
|
||||
{"passed": False, "stage": "spatial_solve", "reason": str(error),
|
||||
"axis_measurements": [asdict(item) for item in measurements]},
|
||||
) from error
|
||||
result = replace(result, axis_residual_diagnostics={
|
||||
f"{item.joint}:cycle{item.cycle}": {
|
||||
"axial_component_separated": item.axis_point_axial_component_separated,
|
||||
"fit_rms_m": item.pose_axis_line_rms_m,
|
||||
"raw_rms_m": item.pose_axis_line_raw_rms_m,
|
||||
"axial_rms_m": item.pose_axis_line_axial_rms_m,
|
||||
"transverse_rms_m": item.pose_axis_line_transverse_rms_m,
|
||||
} for item in measurements
|
||||
})
|
||||
if not result.passed:
|
||||
details = ",".join(f"{n}={r}" for n, r in sorted(result.failure_reasons.items()))
|
||||
raise O12SpatialZeroError(
|
||||
"O12 full-hand spatial zero solve failed:" + details,
|
||||
{"passed": False, "stage": "spatial_solve", "policy": SPATIAL_ZERO_POLICY,
|
||||
"result": asdict(result),
|
||||
"axis_measurements": [asdict(item) for item in measurements]},
|
||||
)
|
||||
return result
|
||||
@@ -272,11 +272,6 @@ def fit_o6_session(
|
||||
)
|
||||
if fit.maximum_monotonic_correction_rad > correction_limit:
|
||||
raise ValueError(f"{name} monotonic correction exceeds limit")
|
||||
if fit.maximum_hysteresis_rad > math.radians(MAXIMUM_HYSTERESIS_DEG):
|
||||
raise ValueError(
|
||||
f"{name} hysteresis {math.degrees(fit.maximum_hysteresis_rad):.3f} "
|
||||
f"degrees exceeds O6 limit {MAXIMUM_HYSTERESIS_DEG:.3f} degrees"
|
||||
)
|
||||
curves[name] = fit
|
||||
holdout[name] = errors
|
||||
cycle_curves[name] = {
|
||||
|
||||
@@ -8,6 +8,8 @@ import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
from ...runtime.engine import CalibrationEngine
|
||||
|
||||
from .artifacts import (
|
||||
artifact_hashes,
|
||||
atomic_write_json,
|
||||
@@ -24,6 +26,7 @@ from .profile import (
|
||||
MEASURED_PASSIVE_JOINTS,
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||
build_typed_profile,
|
||||
)
|
||||
from .urdf import O6UrdfCorrection, write_o6_corrected_urdf
|
||||
|
||||
@@ -111,6 +114,13 @@ def finalize_o6_session(
|
||||
result = fit_o6_session(
|
||||
source_urdf, accepted_records_by_joint(records), require_thumb_axis_zero=True
|
||||
)
|
||||
CalibrationEngine(build_typed_profile()).result_from_fit(
|
||||
result,
|
||||
transfers={
|
||||
**TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||
**TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||
},
|
||||
)
|
||||
payload = build_o6_runtime_payload(
|
||||
serial_number=serial_number,
|
||||
source_urdf=source_urdf,
|
||||
|
||||
@@ -217,8 +217,8 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
),
|
||||
motion=MotionPolicy(
|
||||
tasks=tasks,
|
||||
precheck_sweeps=True,
|
||||
steady_command_checkpoints=True,
|
||||
precheck_sweeps=False,
|
||||
steady_command_checkpoints=False,
|
||||
speed_parameters={
|
||||
"baseline_u8": BASELINE_SPEED_U8,
|
||||
"preflight_u8": PREFLIGHT_SPEED_U8,
|
||||
@@ -253,7 +253,6 @@ def build_typed_profile() -> CalibrationProfile:
|
||||
hard_threshold_keys=frozenset({
|
||||
"minimum_detection_rate",
|
||||
"maximum_state_image_skew_ms",
|
||||
"maximum_hysteresis_rad",
|
||||
"maximum_validation_error_rad",
|
||||
"maximum_mimic_residual_rad",
|
||||
}),
|
||||
|
||||
@@ -6,11 +6,13 @@ from dataclasses import dataclass
|
||||
from typing import Any, Callable, Iterator
|
||||
|
||||
from ..core import CalibrationProfile, ProfileKey, validate_profile
|
||||
from ..runtime.engine import CalibrationEngine
|
||||
from ..runtime.adapters import ProfileSdkAdapter
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EngineBindings:
|
||||
"""Temporary bridge from typed policies to the proven engine objects."""
|
||||
"""Small model plugin: SDK I/O hooks plus compatible CLI serializers."""
|
||||
|
||||
hand_profile: Any
|
||||
zero_profile: Any
|
||||
@@ -19,6 +21,7 @@ class EngineBindings:
|
||||
return_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
||||
cli_main: Callable[[list[str] | None], None]
|
||||
node_main: Callable[[list[str] | None], None]
|
||||
sdk_adapter_factory: Callable[[Any], ProfileSdkAdapter] = ProfileSdkAdapter
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -26,6 +29,12 @@ class RegisteredProfile:
|
||||
profile: CalibrationProfile
|
||||
engine: EngineBindings
|
||||
|
||||
def build_engine(self) -> CalibrationEngine:
|
||||
return CalibrationEngine(self.profile)
|
||||
|
||||
def build_sdk_adapter(self) -> ProfileSdkAdapter:
|
||||
return self.engine.sdk_adapter_factory(self.profile.command)
|
||||
|
||||
|
||||
class ProfileRegistry:
|
||||
"""An in-package registry; no discovery plugins or string evaluation."""
|
||||
|
||||
@@ -1,11 +1,22 @@
|
||||
"""Common orchestration; ROS integration is isolated below ``nodes``."""
|
||||
"""Common calibration engine; model registry imports remain lazy."""
|
||||
|
||||
from .controller import ControllerSnapshot, SessionController, SessionState
|
||||
from .runner import build_session_controller
|
||||
from .engine import (
|
||||
ACQUISITION_POLICY_VERSION,
|
||||
CalibrationEngine,
|
||||
CalibrationResult,
|
||||
ScanUnit,
|
||||
SweepQuality,
|
||||
)
|
||||
|
||||
|
||||
def build_session_controller(*args, **kwargs):
|
||||
# Avoid a registry/runtime import cycle while preserving the public API.
|
||||
from .runner import build_session_controller as build
|
||||
return build(*args, **kwargs)
|
||||
|
||||
__all__ = [
|
||||
"ControllerSnapshot",
|
||||
"SessionController",
|
||||
"SessionState",
|
||||
"build_session_controller",
|
||||
"ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult",
|
||||
"ControllerSnapshot", "ScanUnit", "SessionController", "SessionState",
|
||||
"SweepQuality", "build_session_controller",
|
||||
]
|
||||
|
||||
@@ -1 +1,5 @@
|
||||
"""Camera, detector, and hand-SDK runtime adapters."""
|
||||
|
||||
from .base import HardwareHealth, ProfileSdkAdapter, SdkAdapter
|
||||
|
||||
__all__ = ["HardwareHealth", "ProfileSdkAdapter", "SdkAdapter"]
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
"""Hardware-facing contract used by the model-independent calibration engine.
|
||||
|
||||
Adapters deliberately stop at the SDK boundary. Tag geometry, task ordering,
|
||||
fitting, and URDF edits belong to :class:`CalibrationProfile`, so adding a hand
|
||||
does not add another acquisition state machine.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
from typing import Protocol, Sequence, runtime_checkable
|
||||
|
||||
from ...core import CommandLayout
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HardwareHealth:
|
||||
connected: bool
|
||||
position_mode: bool
|
||||
active_faults: tuple[str, ...] = ()
|
||||
|
||||
@property
|
||||
def safe(self) -> bool:
|
||||
return self.connected and self.position_mode and not self.active_faults
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class SdkAdapter(Protocol):
|
||||
"""Minimum SDK surface required by ``CalibrationEngine``.
|
||||
|
||||
ROS publishers/subscribers or vendor API objects can implement this
|
||||
protocol. The engine never assumes byte commands, radians, CAN names, or
|
||||
a particular ``JointState.name`` ordering.
|
||||
"""
|
||||
|
||||
@property
|
||||
def command_layout(self) -> CommandLayout: ...
|
||||
|
||||
def parse_feedback(
|
||||
self, names: Sequence[str], values: Sequence[float]
|
||||
) -> tuple[float, ...] | None: ...
|
||||
|
||||
def validate_command(self, values: Sequence[float]) -> tuple[float, ...]: ...
|
||||
|
||||
def health(self) -> HardwareHealth: ...
|
||||
|
||||
def publish_position(self, values: Sequence[float]) -> None: ...
|
||||
|
||||
def set_speed(self, command_index: int, speed: float) -> None: ...
|
||||
|
||||
|
||||
class ProfileSdkAdapter:
|
||||
"""Shared profile-driven parsing and physical-domain validation.
|
||||
|
||||
Live model adapters subclass this object only to implement SDK I/O and
|
||||
health. The parsing rules are shared by legacy-byte and physical-angle
|
||||
model nodes.
|
||||
"""
|
||||
|
||||
def __init__(self, command_layout: CommandLayout) -> None:
|
||||
self.command_layout = command_layout
|
||||
|
||||
def parse_feedback(
|
||||
self, names: Sequence[str], values: Sequence[float]
|
||||
) -> tuple[float, ...] | None:
|
||||
layout = self.command_layout
|
||||
if len(values) != layout.command_count:
|
||||
return None
|
||||
if names and not layout.feedback_by_index:
|
||||
by_name = dict(zip((str(name) for name in names), values))
|
||||
for alias, canonical in layout.feedback_name_aliases.items():
|
||||
if alias in by_name and canonical not in by_name:
|
||||
by_name[canonical] = by_name[alias]
|
||||
if any(name not in by_name for name in layout.names):
|
||||
return None
|
||||
parsed = tuple(float(by_name[name]) for name in layout.names)
|
||||
else:
|
||||
parsed = tuple(float(value) for value in values)
|
||||
return parsed if all(math.isfinite(value) for value in parsed) else None
|
||||
|
||||
def validate_command(self, values: Sequence[float]) -> tuple[float, ...]:
|
||||
layout = self.command_layout
|
||||
command = tuple(float(value) for value in values)
|
||||
if len(command) != layout.command_count:
|
||||
raise ValueError("command channel count differs from profile")
|
||||
for index, value in enumerate(command):
|
||||
if (
|
||||
not math.isfinite(value)
|
||||
or value < layout.minimum_values[index]
|
||||
or value > layout.maximum_values[index]
|
||||
):
|
||||
raise ValueError(
|
||||
f"command outside physical range: channel={index}:value={value}"
|
||||
)
|
||||
return command
|
||||
|
||||
def health(self) -> HardwareHealth: # pragma: no cover - live subclass hook
|
||||
raise NotImplementedError
|
||||
|
||||
def publish_position(self, values: Sequence[float]) -> None: # pragma: no cover
|
||||
raise NotImplementedError
|
||||
|
||||
def set_speed(self, command_index: int, speed: float) -> None: # pragma: no cover
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
__all__ = ["HardwareHealth", "ProfileSdkAdapter", "SdkAdapter"]
|
||||
@@ -0,0 +1,322 @@
|
||||
"""Model-independent calibration schedule and lightweight acceptance policy."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import math
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..core import CalibrationProfile, TaskSpec, validate_profile
|
||||
|
||||
|
||||
ACQUISITION_POLICY_VERSION = "unified_engine_v1"
|
||||
TRAINING_CYCLES = (0, 1, 2)
|
||||
HOLDOUT_CYCLE = 3
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ScanUnit:
|
||||
task_key: str
|
||||
cycle: int
|
||||
direction: str
|
||||
start: float
|
||||
end: float
|
||||
speed: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SweepQuality:
|
||||
passed: bool
|
||||
failures: tuple[str, ...]
|
||||
warnings: tuple[str, ...]
|
||||
metrics: Mapping[str, Any]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CalibrationResult:
|
||||
"""Unified internal result; serializers retain each deployed schema."""
|
||||
|
||||
profile_id: str
|
||||
policy_version: str
|
||||
curves: Mapping[str, Any]
|
||||
zero_offsets_rad: Mapping[str, float]
|
||||
travel: Mapping[str, float]
|
||||
coupling: Mapping[str, Any]
|
||||
transfers: Mapping[str, str]
|
||||
quality: Mapping[str, Any]
|
||||
metadata: Mapping[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
class CalibrationEngine:
|
||||
"""One policy kernel shared by all model-specific ROS/SDK wrappers."""
|
||||
|
||||
def __init__(self, profile: CalibrationProfile) -> None:
|
||||
validate_profile(profile)
|
||||
self.profile = profile
|
||||
|
||||
@property
|
||||
def checkpoint_token(self) -> str:
|
||||
return ACQUISITION_POLICY_VERSION
|
||||
|
||||
def scan_units(self) -> tuple[ScanUnit, ...]:
|
||||
units: list[ScanUnit] = []
|
||||
for task in self.profile.motion.tasks:
|
||||
speed = self._formal_speed(task)
|
||||
for cycle in (*TRAINING_CYCLES, HOLDOUT_CYCLE):
|
||||
units.extend((
|
||||
ScanUnit(
|
||||
task.key, cycle, "decreasing", task.start_value,
|
||||
task.end_value, speed,
|
||||
),
|
||||
ScanUnit(
|
||||
task.key, cycle, "increasing", task.end_value,
|
||||
task.start_value, speed,
|
||||
),
|
||||
))
|
||||
return tuple(units)
|
||||
|
||||
def mapping_probe_delta(self, task: TaskSpec) -> float | None:
|
||||
"""Only physical-angle profiles perform a single <=3 degree jog."""
|
||||
if self.profile.command.unit != "rad":
|
||||
return None
|
||||
distance = task.end_value - task.start_value
|
||||
return math.copysign(
|
||||
min(abs(distance), self.profile.acquisition.mapping_probe_maximum_rad),
|
||||
distance,
|
||||
)
|
||||
|
||||
def retry_speed(self, original_speed: float, attempt: int) -> float:
|
||||
if int(attempt) != 2:
|
||||
raise ValueError("unified engine permits exactly one rescan")
|
||||
return float(original_speed)
|
||||
|
||||
@staticmethod
|
||||
def permits_retry(stage: str, completed_retries: int) -> bool:
|
||||
"""Only an under-sampled scan direction gets one same-speed retry."""
|
||||
return str(stage) == "sweep_acquisition" and int(completed_retries) == 0
|
||||
|
||||
def evaluate_sweep(
|
||||
self,
|
||||
feedback_progress_01: Sequence[float],
|
||||
*,
|
||||
minimum_span: float,
|
||||
total_frames: int,
|
||||
joint_frame_rate: float,
|
||||
feedback_hz: float,
|
||||
detection_rate: float,
|
||||
bin_count: int = 256,
|
||||
) -> SweepQuality:
|
||||
"""Judge fitting observability; ideal rates are diagnostics only."""
|
||||
values = np.asarray(feedback_progress_01, dtype=float)
|
||||
values = np.clip(values[np.isfinite(values)], 0.0, 1.0)
|
||||
bins = sorted(set(
|
||||
min(bin_count - 1, max(0, int(value * bin_count)))
|
||||
for value in values
|
||||
))
|
||||
span = float(np.ptp(values)) if values.size else 0.0
|
||||
internal_missing_runs = [
|
||||
right - left - 1 for left, right in zip(bins, bins[1:])
|
||||
]
|
||||
# Byte-command products have a known 0..255 feedback domain, so an
|
||||
# unobserved endpoint is a real acquisition hole. A radian product's
|
||||
# feedback endpoint scale is itself being calibrated: its separate
|
||||
# span/repeatability gate proves effective travel, while this gate must
|
||||
# only reject holes *inside* that observed physical stroke.
|
||||
gap_scope = (
|
||||
"full_command_domain"
|
||||
if self.profile.command.unit == "u8"
|
||||
else "observed_feedback_span"
|
||||
)
|
||||
missing_runs = (
|
||||
[bins[0], bin_count - 1 - bins[-1], *internal_missing_runs]
|
||||
if bins and gap_scope == "full_command_domain"
|
||||
else internal_missing_runs
|
||||
if bins
|
||||
else [bin_count]
|
||||
)
|
||||
maximum_gap = max(missing_runs, default=0)
|
||||
policy = self.profile.acquisition
|
||||
allowed_gap = max(
|
||||
1, int(math.floor(bin_count * policy.maximum_unobserved_fraction))
|
||||
)
|
||||
failures: list[str] = []
|
||||
if values.size < policy.minimum_valid_samples:
|
||||
failures.append(f"frames={values.size}")
|
||||
if span < float(minimum_span):
|
||||
failures.append("feedback_span")
|
||||
if len(bins) < policy.minimum_bins:
|
||||
failures.append(f"bins={len(bins)}")
|
||||
if maximum_gap > allowed_gap:
|
||||
failures.append(f"maximum_gap={maximum_gap}")
|
||||
warnings: list[str] = []
|
||||
if detection_rate < 0.95:
|
||||
warnings.append(f"tag_rate={detection_rate:.3f}")
|
||||
if joint_frame_rate < 0.85:
|
||||
warnings.append(f"joint_frame_rate={joint_frame_rate:.3f}")
|
||||
# A rate target is useful to diagnose latency but valid synchronized
|
||||
# samples, coverage, and gaps are the actual correctness evidence.
|
||||
if feedback_hz <= 0.0:
|
||||
warnings.append("feedback_rate_unavailable")
|
||||
metrics = {
|
||||
"policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"valid_frames": int(values.size),
|
||||
"total_frames": int(total_frames),
|
||||
"feedback_span": span,
|
||||
"feedback_bins": len(bins),
|
||||
"maximum_bin_gap": maximum_gap,
|
||||
"allowed_maximum_bin_gap": allowed_gap,
|
||||
"gap_scope": gap_scope,
|
||||
"joint_frame_rate": float(joint_frame_rate),
|
||||
"feedback_hz": float(feedback_hz),
|
||||
"tag_detection_rate": float(detection_rate),
|
||||
}
|
||||
return SweepQuality(not failures, tuple(failures), tuple(warnings), metrics)
|
||||
|
||||
def resume_compatible(self, session_start: Mapping[str, Any]) -> bool:
|
||||
return (
|
||||
session_start.get("acquisition_policy_version")
|
||||
== ACQUISITION_POLICY_VERSION
|
||||
and session_start.get("profile_id") == self.profile.key.profile_id
|
||||
)
|
||||
|
||||
def result_from_fit(
|
||||
self,
|
||||
fit_result: Any,
|
||||
*,
|
||||
transfers: Mapping[str, str] | None = None,
|
||||
holdout_errors_rad: Mapping[str, Sequence[float]] | None = None,
|
||||
curves: Mapping[str, Any] | None = None,
|
||||
zero_offsets_rad: Mapping[str, float] | None = None,
|
||||
travel: Mapping[str, float] | None = None,
|
||||
coupling: Mapping[str, Any] | None = None,
|
||||
metadata: Mapping[str, Any] | None = None,
|
||||
) -> CalibrationResult:
|
||||
"""Normalize a model fitter result before its legacy serializer runs.
|
||||
|
||||
Model-specific serializers deliberately remain responsible for the
|
||||
deployed v4/v6/v7 JSON shapes. This object is the common publication
|
||||
gate between fitting and both JSON/URDF writers.
|
||||
"""
|
||||
holdout = (
|
||||
holdout_errors_rad
|
||||
if holdout_errors_rad is not None
|
||||
else getattr(fit_result, "holdout_errors_rad", {})
|
||||
)
|
||||
quality_by_joint: dict[str, Mapping[str, float]] = {}
|
||||
fit_diagnostics_by_joint: dict[str, Mapping[str, float]] = {}
|
||||
all_errors: list[float] = []
|
||||
for name, values in dict(holdout).items():
|
||||
absolute = np.abs(np.asarray(tuple(values), dtype=float))
|
||||
if absolute.size == 0 or not np.all(np.isfinite(absolute)):
|
||||
raise ValueError(f"holdout evidence is missing or invalid: {name}")
|
||||
mae = float(np.mean(absolute))
|
||||
p95 = float(np.percentile(absolute, 95.0))
|
||||
maximum = float(np.max(absolute))
|
||||
if (
|
||||
mae > math.radians(1.0)
|
||||
or p95 > math.radians(2.0)
|
||||
or maximum > math.radians(3.0)
|
||||
):
|
||||
raise ValueError(f"holdout quality failed: {name}")
|
||||
quality_by_joint[str(name)] = {
|
||||
"mae_rad": mae,
|
||||
"p95_rad": p95,
|
||||
"maximum_rad": maximum,
|
||||
}
|
||||
all_errors.extend(float(value) for value in absolute)
|
||||
if not quality_by_joint:
|
||||
raise ValueError("independent holdout evidence is required")
|
||||
normalized_curves = dict(
|
||||
curves if curves is not None else getattr(fit_result, "curves", {})
|
||||
)
|
||||
for name, curve in normalized_curves.items():
|
||||
diagnostics: dict[str, float] = {}
|
||||
for field_name in (
|
||||
"maximum_hysteresis_rad",
|
||||
"maximum_monotonic_correction_rad",
|
||||
):
|
||||
value = getattr(curve, field_name, None)
|
||||
if value is None:
|
||||
continue
|
||||
numeric = float(value)
|
||||
if not math.isfinite(numeric) or numeric < 0.0:
|
||||
raise ValueError(
|
||||
f"fit diagnostic is invalid: {name}.{field_name}"
|
||||
)
|
||||
diagnostics[field_name] = numeric
|
||||
if diagnostics:
|
||||
fit_diagnostics_by_joint[str(name)] = diagnostics
|
||||
offsets = dict(
|
||||
zero_offsets_rad
|
||||
if zero_offsets_rad is not None
|
||||
else getattr(fit_result, "zero_offsets_rad", {})
|
||||
)
|
||||
normalized_travel = dict(
|
||||
travel
|
||||
if travel is not None
|
||||
else getattr(
|
||||
fit_result,
|
||||
"travels_rad",
|
||||
getattr(fit_result, "travel", {}),
|
||||
)
|
||||
)
|
||||
if not normalized_curves or not offsets:
|
||||
raise ValueError("fit result is missing curves or zero offsets")
|
||||
result = CalibrationResult(
|
||||
profile_id=self.profile.key.profile_id,
|
||||
policy_version=ACQUISITION_POLICY_VERSION,
|
||||
curves=normalized_curves,
|
||||
zero_offsets_rad=offsets,
|
||||
travel=normalized_travel,
|
||||
coupling=dict(
|
||||
coupling
|
||||
if coupling is not None
|
||||
else getattr(fit_result, "mimic_fits", {})
|
||||
),
|
||||
transfers=dict(transfers or {}),
|
||||
quality={
|
||||
"passed": True,
|
||||
"training_cycles": TRAINING_CYCLES,
|
||||
"holdout_cycle": HOLDOUT_CYCLE,
|
||||
"holdout_by_joint": quality_by_joint,
|
||||
"holdout_sample_count": len(all_errors),
|
||||
# Direction-aware curves explicitly compensate repeatable
|
||||
# mechanical backlash. Hysteresis is therefore diagnostic
|
||||
# evidence, not a standalone rejection criterion; isolated
|
||||
# holdout accuracy remains the publication gate.
|
||||
"fit_diagnostics_by_joint": fit_diagnostics_by_joint,
|
||||
},
|
||||
metadata=dict(metadata or {}),
|
||||
)
|
||||
self.validate_result(result)
|
||||
return result
|
||||
|
||||
def validate_result(self, result: CalibrationResult) -> None:
|
||||
if result.profile_id != self.profile.key.profile_id:
|
||||
raise ValueError("calibration result belongs to another profile")
|
||||
if result.policy_version != ACQUISITION_POLICY_VERSION:
|
||||
raise ValueError("calibration result uses an obsolete policy")
|
||||
if not bool(result.quality.get("passed")):
|
||||
raise ValueError("calibration result is not publishable")
|
||||
if tuple(result.quality.get("training_cycles", ())) != TRAINING_CYCLES:
|
||||
raise ValueError("calibration result must use three training cycles")
|
||||
if int(result.quality.get("holdout_cycle", -1)) != HOLDOUT_CYCLE:
|
||||
raise ValueError("calibration result must use cycle four as holdout")
|
||||
|
||||
def _formal_speed(self, task: TaskSpec) -> float:
|
||||
if self.profile.command.unit == "rad":
|
||||
assert task.formal_speed is not None
|
||||
return float(task.formal_speed)
|
||||
return float(
|
||||
task.formal_speed_u8
|
||||
if task.formal_speed_u8 is not None
|
||||
else self.profile.motion.speed_parameters.get("formal_u8", 1.0)
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult",
|
||||
"HOLDOUT_CYCLE", "ScanUnit", "SweepQuality", "TRAINING_CYCLES",
|
||||
]
|
||||
@@ -24,6 +24,17 @@ def build_session_controller(
|
||||
return SessionController(registered.profile, evaluator, solver)
|
||||
|
||||
|
||||
def build_registered_runtime(
|
||||
profile_key: ProfileKey,
|
||||
*,
|
||||
registry: ProfileRegistry | None = None,
|
||||
):
|
||||
"""Assemble the same engine/adapter pair for any registered hand."""
|
||||
selected_registry = registry or get_default_registry()
|
||||
registered = selected_registry.get(profile_key)
|
||||
return registered.build_engine(), registered.build_sdk_adapter()
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
"""Select the profile first, then delegate to its reviewed CLI strategy."""
|
||||
selector = argparse.ArgumentParser(add_help=False)
|
||||
|
||||
@@ -635,6 +635,7 @@ def _fit_joint_curve(
|
||||
*,
|
||||
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
|
||||
preserve_direction_offset: bool = False,
|
||||
require_observed_domain_endpoints: bool = True,
|
||||
) -> tuple[dict[str, Any], float, float]:
|
||||
by_direction: dict[str, list[list[float]]] = {
|
||||
direction: [[] for _ in range(256)] for direction in DIRECTIONS
|
||||
@@ -655,10 +656,12 @@ def _fit_joint_curve(
|
||||
],
|
||||
dtype=int,
|
||||
)
|
||||
if (
|
||||
commands.size < 3
|
||||
or int(commands[0]) != 0
|
||||
or int(commands[-1]) != 255
|
||||
if commands.size < 3:
|
||||
raise ValueError(
|
||||
f"{direction} centre trajectory requires at least 3 commands"
|
||||
)
|
||||
if require_observed_domain_endpoints and (
|
||||
int(commands[0]) != 0 or int(commands[-1]) != 255
|
||||
):
|
||||
raise ValueError(
|
||||
f"{direction} centre trajectory requires commands 0 and 255"
|
||||
|
||||
@@ -0,0 +1,175 @@
|
||||
"""Read-only, model-independent URDF pose comparison; never a release gate.
|
||||
|
||||
Coordinates are URDF joint radians, NOT SDK feedback. Link origins are not
|
||||
fingertip contact points. A matching model is not a physical accuracy proof.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .storage import atomic_write_json
|
||||
|
||||
|
||||
def _vector(value):
|
||||
a = np.asarray([float(x) for x in value.split()])
|
||||
if a.shape != (3,) or not np.isfinite(a).all():
|
||||
raise ValueError("invalid URDF vector")
|
||||
return a
|
||||
|
||||
|
||||
class _Model:
|
||||
def __init__(self, path):
|
||||
self.path = Path(path).resolve(strict=True)
|
||||
root = ET.parse(self.path).getroot()
|
||||
self.links = {x.get("name"): ET.tostring(x) for x in root.findall("link")}
|
||||
self.joints = {x.get("name"): x for x in root.findall("joint")}
|
||||
if len(self.links) != len(root.findall("link")) or len(self.joints) != len(root.findall("joint")):
|
||||
raise ValueError("duplicate URDF names")
|
||||
self.parents = {}
|
||||
for name, j in self.joints.items():
|
||||
if j.get("type") not in {"fixed", "revolute", "continuous"}:
|
||||
raise ValueError(f"unsupported joint type: {name}")
|
||||
p, c = j.find("parent").get("link"), j.find("child").get("link")
|
||||
if p not in self.links or c not in self.links or c in self.parents:
|
||||
raise ValueError(f"invalid topology: {name}")
|
||||
self.parents[c] = name
|
||||
roots = set(self.links) - set(self.parents)
|
||||
if len(roots) != 1:
|
||||
raise ValueError("URDF must have one root")
|
||||
self.root = roots.pop()
|
||||
self.active = {n for n, j in self.joints.items()
|
||||
if j.get("type") != "fixed" and j.find("mimic") is None}
|
||||
|
||||
def bounds(self, name):
|
||||
j = self.joints[name]
|
||||
if j.get("type") == "continuous":
|
||||
return -math.pi, math.pi
|
||||
lim = j.find("limit")
|
||||
values = float(lim.get("lower")), float(lim.get("upper"))
|
||||
if not all(math.isfinite(v) for v in values) or values[0] > values[1]:
|
||||
raise ValueError(f"invalid limits: {name}")
|
||||
return values
|
||||
|
||||
def fk(self, values):
|
||||
angles, poses = {}, {self.root: np.eye(4)}
|
||||
|
||||
def angle(name, stack=()):
|
||||
if name in angles:
|
||||
return angles[name]
|
||||
if name in stack or name not in self.joints:
|
||||
raise ValueError("invalid mimic dependency")
|
||||
j = self.joints[name]
|
||||
mimic = j.find("mimic")
|
||||
if j.get("type") == "fixed":
|
||||
v = 0.
|
||||
elif mimic is None:
|
||||
v = float(values.get(name, 0.))
|
||||
else:
|
||||
v = (float(mimic.get("multiplier", "1")) *
|
||||
angle(mimic.get("joint"), (*stack, name)) + float(mimic.get("offset", "0")))
|
||||
if not math.isfinite(v):
|
||||
raise ValueError("non-finite joint coordinate")
|
||||
angles[name] = v
|
||||
return v
|
||||
|
||||
def pose(link, stack=()):
|
||||
if link in poses:
|
||||
return poses[link]
|
||||
if link in stack:
|
||||
raise ValueError("cyclic URDF topology")
|
||||
name = self.parents[link]
|
||||
j = self.joints[name]
|
||||
origin = j.find("origin")
|
||||
t = np.eye(4)
|
||||
if origin is not None:
|
||||
t[:3, 3] = _vector(origin.get("xyz", "0 0 0"))
|
||||
t[:3, :3] = Rotation.from_euler("xyz", _vector(origin.get("rpy", "0 0 0"))).as_matrix()
|
||||
q = angle(name)
|
||||
if j.get("type") != "fixed":
|
||||
a = j.find("axis")
|
||||
axis = _vector("1 0 0" if a is None else a.get("xyz", "1 0 0"))
|
||||
if np.linalg.norm(axis) < 1.e-12:
|
||||
raise ValueError("zero joint axis")
|
||||
t[:3, :3] = t[:3, :3] @ Rotation.from_rotvec(axis / np.linalg.norm(axis) * q).as_matrix()
|
||||
poses[link] = pose(j.find("parent").get("link"), (*stack, link)) @ t
|
||||
return poses[link]
|
||||
|
||||
for link in self.links:
|
||||
pose(link)
|
||||
return poses
|
||||
|
||||
|
||||
def compare_urdfs(reference, candidate):
|
||||
a, b = _Model(reference), _Model(candidate)
|
||||
if set(a.links) != set(b.links) or set(a.joints) != set(b.joints) or a.active != b.active:
|
||||
raise ValueError("models have different links/joints/actuators")
|
||||
for name in a.joints:
|
||||
x, y = a.joints[name], b.joints[name]
|
||||
if x.get("type") != y.get("type") or any(x.find(k).attrib != y.find(k).attrib for k in ("parent", "child")):
|
||||
raise ValueError("models have different topology")
|
||||
bounds = {}
|
||||
for name in sorted(a.active):
|
||||
al, au = a.bounds(name)
|
||||
bl, bu = b.bounds(name)
|
||||
bounds[name] = max(al, bl), min(au, bu)
|
||||
if bounds[name][0] > bounds[name][1]:
|
||||
raise ValueError(f"no common joint interval: {name}")
|
||||
neutral = {n: float(np.clip(0., lo, hi)) for n, (lo, hi) in bounds.items()}
|
||||
samples = {"neutral": neutral}
|
||||
if len(bounds) > 1:
|
||||
samples["combined_middle"] = {n: (lo + hi) / 2 for n, (lo, hi) in bounds.items()}
|
||||
samples["combined_upper"] = {n: hi for n, (lo, hi) in bounds.items()}
|
||||
for name, (lo, hi) in bounds.items():
|
||||
for label, q in (("lower", lo), ("middle", (lo + hi) / 2), ("upper", hi)):
|
||||
samples[f"{name}:{label}"] = {**neutral, name: q}
|
||||
errors = {link: {"maximum_origin_distance_mm": 0., "maximum_orientation_difference_deg": 0.}
|
||||
for link in sorted(a.links)}
|
||||
for q in samples.values():
|
||||
left, right = a.fk(q), b.fk(q)
|
||||
for link, item in errors.items():
|
||||
distance = float(np.linalg.norm(left[link][:3, 3] - right[link][:3, 3]) * 1000)
|
||||
rotation = float(np.degrees(Rotation.from_matrix(left[link][:3, :3].T @ right[link][:3, :3]).magnitude()))
|
||||
item["maximum_origin_distance_mm"] = max(item["maximum_origin_distance_mm"], distance)
|
||||
item["maximum_orientation_difference_deg"] = max(item["maximum_orientation_difference_deg"], rotation)
|
||||
return {
|
||||
"comparison_schema": 1,
|
||||
"scope": "same_urdf_joint_angles_not_sdk_feedback",
|
||||
"is_accuracy_certificate": False,
|
||||
"note": "Link origins are not fingertip contact points; never send these offline probe poses to hardware.",
|
||||
"reference": str(a.path), "candidate": str(b.path),
|
||||
"reference_sha256": hashlib.sha256(a.path.read_bytes()).hexdigest(),
|
||||
"candidate_sha256": hashlib.sha256(b.path.read_bytes()).hexdigest(),
|
||||
"link_elements_identical": a.links == b.links,
|
||||
"active_joint_ranges_rad": {n: {"reference": a.bounds(n), "candidate": b.bounds(n),
|
||||
"comparison_interval": bounds[n]} for n in bounds},
|
||||
"pose_count": len(samples), "probe_joint_positions_rad": samples,
|
||||
"link_frame_differences": errors,
|
||||
"maximum_origin_distance_mm": max(x["maximum_origin_distance_mm"] for x in errors.values()),
|
||||
"maximum_orientation_difference_deg": max(x["maximum_orientation_difference_deg"] for x in errors.values()),
|
||||
}
|
||||
|
||||
|
||||
def main(args=None):
|
||||
parser = argparse.ArgumentParser(description="离线 URDF 姿态对比，不驱动硬件、不改变标定结果")
|
||||
parser.add_argument("--reference", required=True)
|
||||
parser.add_argument("--candidate", required=True)
|
||||
parser.add_argument("--output", required=True)
|
||||
selected = parser.parse_args(args)
|
||||
target = Path(selected.output).resolve()
|
||||
if target.exists():
|
||||
parser.error("output must be a new file")
|
||||
result = compare_urdfs(selected.reference, selected.candidate)
|
||||
atomic_write_json(target, result)
|
||||
print(f"模型对比完成（非精度PASS）：最大连杆原点差 {result['maximum_origin_distance_mm']:.6f} mm；"
|
||||
f"最大方向差 {result['maximum_orientation_difference_deg']:.6f}°；报告 {target}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -17,7 +17,12 @@ def package_data_tree(root: str) -> list[tuple[str, list[str]]]:
|
||||
result = []
|
||||
for directory in directories:
|
||||
files = sorted(
|
||||
str(path) for path in directory.iterdir() if path.is_file()
|
||||
str(path)
|
||||
for path in directory.iterdir()
|
||||
if path.is_file()
|
||||
# Generated calibration products belong under calibration_output,
|
||||
# never in the installed immutable CAD input bundle.
|
||||
and "calibrated" not in path.stem.lower()
|
||||
)
|
||||
if files:
|
||||
result.append(
|
||||
@@ -51,6 +56,7 @@ setup(
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"compare_calibration_urdfs = linkerhand_calibration.urdf_comparison:main",
|
||||
(
|
||||
"hikrobot_camera_node = "
|
||||
"linkerhand_calibration.hikrobot_camera:main"
|
||||
|
||||
@@ -203,15 +203,15 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
|
||||
assert parameters["minimum_sweep_bins"] >= 32
|
||||
assert parameters["maximum_bin_gap"] <= 16
|
||||
assert parameters["automatic_sweep_retry_limit"] == 2
|
||||
assert parameters["automatic_fit_retry_limit"] == 2
|
||||
assert parameters["automatic_sweep_retry_limit"] == 1
|
||||
assert parameters["automatic_fit_retry_limit"] == 0
|
||||
assert parameters["motor_stall_timeout_seconds"] == 2.0
|
||||
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
|
||||
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
|
||||
assert parameters["automatic_motion_retry_limit"] == 2
|
||||
assert parameters["automatic_motion_retry_limit"] == 0
|
||||
assert parameters["provisional_warning_ratio"] == 1.25
|
||||
assert parameters["retry_speed_scales"] == [0.8, 0.6]
|
||||
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
|
||||
assert parameters["retry_speed_scales"] == [1.0]
|
||||
assert parameters["retry_endpoint_hold_seconds"] == [0.5]
|
||||
assert parameters["position_timeout_seconds"] >= 20.0
|
||||
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
||||
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
|
||||
|
||||
@@ -77,6 +77,7 @@ from linkerhand_calibration.urdf_zero import (
|
||||
get_zero_calibration_profile,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
from linkerhand_calibration.runtime import ACQUISITION_POLICY_VERSION
|
||||
|
||||
|
||||
REPO = Path(__file__).resolve().parents[3]
|
||||
@@ -963,6 +964,7 @@ def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> N
|
||||
json.dumps(
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
@@ -1180,6 +1182,7 @@ def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary(
|
||||
json.dumps(
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
@@ -1205,6 +1208,7 @@ def test_automatic_resume_skips_newer_attempt_without_start_checkpoint(
|
||||
json.dumps(
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
@@ -1251,6 +1255,7 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
||||
rows = [
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
"view_tags": {
|
||||
@@ -1344,6 +1349,7 @@ def test_full_resume_discards_all_old_tasks_after_start_position_change(
|
||||
rows = [
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"model": "G20",
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
@@ -1448,6 +1454,7 @@ def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks(
|
||||
rows = [
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"model": "G20",
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
@@ -2725,6 +2732,7 @@ def test_node_drops_imported_task_failing_hard_gates(
|
||||
rows = [
|
||||
{
|
||||
"kind": "session_start",
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
"hand_type": "right",
|
||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||
"view_tags": {
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
import json
|
||||
import re
|
||||
from types import SimpleNamespace
|
||||
import xml.etree.ElementTree as ET
|
||||
@@ -432,11 +433,13 @@ def test_l6_quality_gates_each_tag_separately_from_joined_frames(
|
||||
L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step)
|
||||
|
||||
|
||||
def test_l6_quality_failure_names_the_specific_tag_id(tmp_path: Path) -> None:
|
||||
def test_l6_low_tag_rate_is_diagnostic_when_samples_are_observable(tmp_path: Path) -> None:
|
||||
fake, step = _l6_sweep_quality_fake(tmp_path)
|
||||
fake.step_tag_quality_frames["pinky_dip"] = 190
|
||||
with pytest.raises(ValueError, match=r"tag_rate\[ID5/pinky_dip\]=0\.900"):
|
||||
L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step)
|
||||
L6ThreeCameraCalibrationNode._qualify_recording_step(fake, step)
|
||||
quality = json.loads(fake.raw_path.read_text().splitlines()[-1])
|
||||
assert quality["failures"] == []
|
||||
assert any(value.startswith("tag_rate=") for value in quality["warnings"])
|
||||
|
||||
|
||||
def test_l6_operator_progress_shows_exact_failed_sweep_metric() -> None:
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
"""Observable line residuals must not be confused with axial PnP drift."""
|
||||
|
||||
import inspect
|
||||
import math
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from linkerhand_calibration.models.g20.zero_solver import (
|
||||
_fit_axis_point_from_pose_trajectory, fit_joint_axis_measurement,
|
||||
)
|
||||
|
||||
|
||||
def axis_fit(*, axial_drift=0., transverse_drift=0., separate=False):
|
||||
axis = np.array([0., 0., 1.])
|
||||
point = np.array([.012, -.017, 0.])
|
||||
reference = np.array([.047, .009, .02])
|
||||
rows = []
|
||||
for index, angle in enumerate(np.linspace(0., 1.2, 128)):
|
||||
rotation = Rotation.from_rotvec(axis * angle)
|
||||
translation = point + rotation.apply(reference - point)
|
||||
translation += axis * axial_drift * math.sin(angle * 3.)
|
||||
translation[0] += transverse_drift * math.sin(angle * 11.)
|
||||
rows.append({
|
||||
'command_u8': 255 - 2 * index, 'direction': 'decreasing',
|
||||
'relative_translation_xyz_m': translation.tolist(),
|
||||
'relative_quaternion_xyzw': rotation.as_quat().tolist(),
|
||||
})
|
||||
diagnostics = {}
|
||||
fitted, rms, _ = _fit_axis_point_from_pose_trajectory(
|
||||
rows, zero_command_u8=255, axis_parent_xyz=axis,
|
||||
angle_axis_parent_xyz=axis, phase_reference_point_parent_xyz=point,
|
||||
view_normal_common_xyz=None, canonical_zero_direction='decreasing',
|
||||
allow_axial_translation=separate, residual_diagnostics=diagnostics,
|
||||
)
|
||||
return fitted, rms, diagnostics
|
||||
|
||||
|
||||
def test_axial_component_is_reported_but_not_used_as_transverse_fit_error():
|
||||
baseline, baseline_rms, _ = axis_fit(separate=True)
|
||||
observed, rms, diagnostic = axis_fit(axial_drift=.015, separate=True)
|
||||
assert observed == pytest.approx(baseline, abs=1.e-9)
|
||||
assert rms == pytest.approx(baseline_rms, abs=1.e-9)
|
||||
assert diagnostic['axial_rms_m'] > .005
|
||||
assert diagnostic['raw_rms_m'] > rms
|
||||
assert diagnostic['transverse_rms_m'] == pytest.approx(rms, abs=1.e-9)
|
||||
|
||||
|
||||
def test_transverse_model_error_is_not_discarded_by_projection():
|
||||
_, rms, diagnostic = axis_fit(transverse_drift=.01, separate=True)
|
||||
assert rms > .0015
|
||||
assert diagnostic['transverse_rms_m'] > .0015
|
||||
|
||||
|
||||
def test_legacy_default_does_not_silently_enable_projection():
|
||||
assert inspect.signature(fit_joint_axis_measurement).parameters[
|
||||
'separate_axial_residual'].default is False
|
||||
_, rms, diagnostic = axis_fit(axial_drift=.015)
|
||||
assert rms > .0015
|
||||
assert diagnostic['raw_rms_m'] == pytest.approx(rms)
|
||||
@@ -0,0 +1,342 @@
|
||||
"""Non-zero recovery through G20 geometry, not zero-only CAD fixtures."""
|
||||
|
||||
from dataclasses import replace
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from linkerhand_calibration.models.g20.profile import JointCurveFit
|
||||
from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel
|
||||
from linkerhand_calibration.models.o12.profile import (
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
GEOMETRIC_ZERO_JOINTS,
|
||||
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||
build_typed_profile,
|
||||
)
|
||||
from linkerhand_calibration.models.o12.zero import (
|
||||
AXIS_JOINTS, PHASE_PARENT, ZERO_OBSERVER, O12SpatialZeroError,
|
||||
full_hand_zero_profile, motor_index, solve_full_hand_zero,
|
||||
)
|
||||
from linkerhand_calibration.models.o12.kinematics import PASSIVE_SDK_SOURCE_BY_JOINT
|
||||
from test_o12_right_profile import SOURCE_URDF, _synthetic_records
|
||||
|
||||
|
||||
def pose(t):
|
||||
return {"translation_xyz_m": t[:3, 3].tolist(),
|
||||
"quaternion_xyzw": Rotation.from_matrix(t[:3, :3]).as_quat().tolist()}
|
||||
|
||||
|
||||
def transform(xyz, rpy):
|
||||
t = np.eye(4)
|
||||
t[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix()
|
||||
t[:3, 3] = xyz
|
||||
return t
|
||||
|
||||
|
||||
def observations(truth, *, coupled_passive=False):
|
||||
model = UrdfKinematicModel(SOURCE_URDF)
|
||||
common = transform([.03, -.02, .6], [.15, -.2, .3])
|
||||
angle = tuple(np.linspace(.7, 0., 256))
|
||||
curve = JointCurveFit(angle, angle, angle, {}, 0., 0., {})
|
||||
rows = {}
|
||||
for i, name in enumerate(AXIS_JOINTS):
|
||||
mount = transform([.013, -.011, .024], [.21, -.1 * i, .3])
|
||||
# Passive trajectories live in the parent Tag frame. A constant
|
||||
# arbitrary mount must not be mistaken for an encoder zero.
|
||||
parent = common @ transform([-.01, .015, -.02], [-.2, .11, -.15])
|
||||
rows[name] = []
|
||||
for cycle in range(4):
|
||||
for direction, indices in (("decreasing", range(255, -1, -4)),
|
||||
("increasing", range(3, 256, 4))):
|
||||
for index in indices:
|
||||
state = [255.] * 12
|
||||
# A passive axis is independently rotated here to test
|
||||
# the spatial kernel, not the model's SDK adapter.
|
||||
if name in CALIBRATED_ACTIVE_JOINTS:
|
||||
state[motor_index(name)] = index
|
||||
joint_angles = {name: angle[index]}
|
||||
sample_parent = parent
|
||||
if coupled_passive and name in PASSIVE_SDK_SOURCE_BY_JOINT:
|
||||
state[motor_index(name)] = index
|
||||
joint_angles[PASSIVE_SDK_SOURCE_BY_JOINT[name]] = angle[index]
|
||||
sample_parent = common @ model.link_transform(
|
||||
PHASE_PARENT[name], zero_offsets=truth, joint_angles=joint_angles,
|
||||
independent_mimic_angles=True,
|
||||
) @ transform([.01, .004, .014], [.2, -.1, .3])
|
||||
child = common @ model.link_transform(
|
||||
name, zero_offsets=truth, joint_angles=joint_angles,
|
||||
independent_mimic_angles=True,
|
||||
) @ mount
|
||||
relative = np.linalg.inv(sample_parent) @ child
|
||||
rows[name].append({
|
||||
"cycle": cycle, "direction": direction, "command_u8": index,
|
||||
"state_u8": state,
|
||||
"relative_quaternion_xyzw": pose(relative)["quaternion_xyzw"],
|
||||
"relative_translation_xyz_m": relative[:3, 3].tolist(),
|
||||
"parent_pose_common": pose(sample_parent), "child_pose_common": pose(child),
|
||||
"view_normal_common_xyz": common[:3, 0].tolist(),
|
||||
"camera_center_common_xyz_m": [0., 0., 0.],
|
||||
})
|
||||
return rows, {name: curve for name in AXIS_JOINTS}
|
||||
|
||||
|
||||
def test_profile_authorizes_every_observed_active_zero_without_endpoint_assumptions():
|
||||
zero = build_typed_profile().zero
|
||||
assert set(zero.direct_zero_joints) == GEOMETRIC_ZERO_JOINTS == set(ZERO_OBSERVER)
|
||||
assert CALIBRATED_ACTIVE_JOINTS - GEOMETRIC_ZERO_JOINTS == STATIC_ZERO_EXCLUDED_JOINTS
|
||||
assert STATIC_ZERO_EXCLUDED_JOINTS == {"thumb_mcp"}
|
||||
assert not zero.mechanical_endpoint_joints
|
||||
assert zero.cad_frozen_joints & CALIBRATED_ACTIVE_JOINTS == STATIC_ZERO_EXCLUDED_JOINTS
|
||||
from linkerhand_calibration.models.g20.zero_solver import get_right_19_thumb_zero_profile
|
||||
assert not get_right_19_thumb_zero_profile().accept_validated_zero_in_confidence_interval
|
||||
assert full_hand_zero_profile().accept_validated_zero_in_confidence_interval
|
||||
assert not get_right_19_thumb_zero_profile().project_axis_gauge_before_image
|
||||
assert full_hand_zero_profile().project_axis_gauge_before_image
|
||||
assert full_hand_zero_profile().hand.stable_cross_view_cone_bias
|
||||
model = UrdfKinematicModel(SOURCE_URDF)
|
||||
for child, parent in PHASE_PARENT.items():
|
||||
a, _ = model.axis_line(child, zero_offsets={}, joint_angles={})
|
||||
b, _ = model.axis_line(parent, zero_offsets={}, joint_angles={})
|
||||
assert abs(float(a @ b)) == pytest.approx(1., abs=1.e-8), (parent, child)
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def full_solution():
|
||||
truth = {name: (.04 if i % 2 else -.06) for i, name in enumerate(ZERO_OBSERVER)}
|
||||
rows, curves = observations(truth)
|
||||
result = solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
return truth, result
|
||||
|
||||
|
||||
def test_all_ten_observable_nonzero_offsets_recovered_with_arbitrary_mounts(full_solution):
|
||||
truth, result = full_solution
|
||||
assert result.passed
|
||||
assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4)
|
||||
assert len(result.axis_residual_diagnostics) == 4 * len(AXIS_JOINTS)
|
||||
assert all(item["axial_component_separated"]
|
||||
for item in result.axis_residual_diagnostics.values())
|
||||
|
||||
|
||||
def test_coupled_passive_parent_motion_is_not_mistaken_for_a_static_zero():
|
||||
truth = {name: .04 for name in ZERO_OBSERVER}
|
||||
rows, curves = observations(truth, coupled_passive=True)
|
||||
result = solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("move_base,remount", [(False, False), (True, False),
|
||||
(False, True), (True, True)])
|
||||
def test_end_on_phase_is_invariant_to_pre_capture_base_and_tag_changes(move_base, remount):
|
||||
# Previously the synthetic camera faced CAD X, perpendicular to the
|
||||
# finger Y axes; that never exercised the real side-camera image branch.
|
||||
truth = {name: .04 for name in ZERO_OBSERVER}
|
||||
rows, curves = observations(truth, coupled_passive=True)
|
||||
common = transform([.03, -.02, .6], [.15, -.2, .3])
|
||||
normal = common[:3, :3] @ np.array([.1, .99, .1])
|
||||
normal /= np.linalg.norm(normal)
|
||||
body = transform([.03, -.02, .01], [.07, -.08, .05]) if move_base else np.eye(4)
|
||||
parent_mount = transform([-.005, .004, .012], [-.1, .05, -.11]) if remount else np.eye(4)
|
||||
child_mount = transform([.008, -.005, .011], [.1, -.12, .2]) if remount else np.eye(4)
|
||||
def matrix(payload):
|
||||
t = np.eye(4)
|
||||
t[:3, :3] = Rotation.from_quat(payload["quaternion_xyzw"]).as_matrix()
|
||||
t[:3, 3] = payload["translation_xyz_m"]
|
||||
return t
|
||||
for samples in rows.values():
|
||||
for row in samples:
|
||||
parent = body @ matrix(row["parent_pose_common"]) @ parent_mount
|
||||
child = body @ matrix(row["child_pose_common"]) @ child_mount
|
||||
relative = np.linalg.inv(parent) @ child
|
||||
row.update(parent_pose_common=pose(parent), child_pose_common=pose(child),
|
||||
relative_translation_xyz_m=relative[:3, 3].tolist(),
|
||||
relative_quaternion_xyzw=Rotation.from_matrix(relative[:3, :3]).as_quat().tolist(),
|
||||
view_normal_common_xyz=normal.tolist())
|
||||
result = solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
assert result.passed
|
||||
assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4)
|
||||
|
||||
|
||||
def test_active_parallel_axis_direction_bias_does_not_become_a_zero():
|
||||
truth = {name: .04 for name in ZERO_OBSERVER}
|
||||
rows, curves = observations(truth, coupled_passive=True)
|
||||
common = transform([.03, -.02, .6], [.15, -.2, .3])
|
||||
normal = common[:3, :3] @ np.array([.1, .99, .1])
|
||||
normal /= np.linalg.norm(normal)
|
||||
for samples in rows.values():
|
||||
for row in samples:
|
||||
row["view_normal_common_xyz"] = normal.tolist()
|
||||
bias = Rotation.from_euler("xyz", [.06, -.04, .07])
|
||||
for name in ("index_pip", "middle_pip"):
|
||||
ref = Rotation.from_quat(rows[name][0]["relative_quaternion_xyzw"])
|
||||
for row in rows[name]:
|
||||
observed = Rotation.from_quat(row["relative_quaternion_xyzw"])
|
||||
delta = observed * ref.inv()
|
||||
changed = bias * delta * bias.inv() * ref
|
||||
row["relative_quaternion_xyzw"] = changed.as_quat().tolist()
|
||||
parent = Rotation.from_quat(row["parent_pose_common"]["quaternion_xyzw"])
|
||||
row["child_pose_common"]["quaternion_xyzw"] = (parent * changed).as_quat().tolist()
|
||||
result = solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
assert result.passed
|
||||
assert result.direct_offsets_rad == pytest.approx(truth, abs=3.e-4)
|
||||
|
||||
|
||||
def test_rotation_only_data_cannot_be_published_as_full_spatial_calibration():
|
||||
from linkerhand_calibration.models.o12.fitting import fit_o12_session
|
||||
with pytest.raises(O12SpatialZeroError, match="requires pose observations"):
|
||||
fit_o12_session(SOURCE_URDF, _synthetic_records(), require_full_hand_spatial_zero=True)
|
||||
|
||||
|
||||
def test_small_uncertain_zero_is_kept_only_when_frozen_zero_passes_holdout():
|
||||
truth = {name: .04 for name in ZERO_OBSERVER}
|
||||
rows, curves = observations({**truth, "index_mcp_roll": .003})
|
||||
for cycle, value in enumerate((.001, .005, .003, 0.)):
|
||||
changed, _ = observations({**truth, "index_mcp_roll": value})
|
||||
for name in rows:
|
||||
rows[name] = [r for r in rows[name] if r["cycle"] != cycle] + [
|
||||
r for r in changed[name] if r["cycle"] == cycle]
|
||||
result = solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
assert result.passed
|
||||
assert result.direct_offsets_rad["index_mcp_roll"] == 0.
|
||||
assert result.validation_error_by_joint_rad["index_mcp_pitch"] < 1.e-5
|
||||
changed, _ = observations({**truth, "index_mcp_roll": .15})
|
||||
for name in rows:
|
||||
rows[name] = [r for r in rows[name] if r["cycle"] != 3] + [
|
||||
r for r in changed[name] if r["cycle"] == 3]
|
||||
with pytest.raises(O12SpatialZeroError):
|
||||
solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
|
||||
|
||||
def test_independent_fourth_cycle_cannot_define_or_hide_a_bad_zero():
|
||||
truth = {name: .04 for name in ZERO_OBSERVER}
|
||||
rows, curves = observations(truth)
|
||||
moved, _ = observations({**truth, "index_pip": .18})
|
||||
for name in rows:
|
||||
rows[name] = [r for r in rows[name] if r["cycle"] < 3] + [r for r in moved[name] if r["cycle"] == 3]
|
||||
with pytest.raises(O12SpatialZeroError, match="spatial zero solve failed") as error:
|
||||
solve_full_hand_zero(SOURCE_URDF, rows, curves)
|
||||
assert error.value.diagnostics["passed"] is False
|
||||
|
||||
|
||||
def test_full_zero_writeback_and_ring_transfer_preserve_each_cad_frame(full_solution, tmp_path):
|
||||
from linkerhand_calibration.models.o12.fitting import fit_o12_session
|
||||
from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf
|
||||
from linkerhand_calibration.models.o12.artifacts import (
|
||||
build_o12_runtime_payload, validate_o12_runtime_payload_against_urdf,
|
||||
)
|
||||
truth, solved = full_solution
|
||||
offsets = {
|
||||
**truth,
|
||||
"thumb_mcp": 0.0,
|
||||
"ring_mcp_pitch": truth["pinky_mcp_pitch"],
|
||||
}
|
||||
fit = replace(fit_o12_session(SOURCE_URDF, _synthetic_records()),
|
||||
zero_offsets_rad=offsets, full_hand_zero_result=solved,
|
||||
zero_method_by_joint={
|
||||
n: (
|
||||
"source_cad_zero_profile_excluded"
|
||||
if n == "thumb_mcp"
|
||||
else "urdf_serial_axis_geometry"
|
||||
)
|
||||
for n in offsets
|
||||
})
|
||||
written = write_o12_corrected_urdf(source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path, serial_number="FULL", result=fit)
|
||||
before, after = UrdfKinematicModel(SOURCE_URDF), UrdfKinematicModel(written.path)
|
||||
for q in ({}, {n: .3 for n in offsets}):
|
||||
for name in offsets:
|
||||
assert after.link_transform(name, zero_offsets={}, joint_angles=q) == pytest.approx(
|
||||
before.link_transform(name, zero_offsets=offsets, joint_angles=q), abs=1.e-8)
|
||||
hashes = {k: "a" * 64 for k in build_typed_profile().artifacts.protected_input_fields}
|
||||
payload = build_o12_runtime_payload(serial_number="FULL", source_urdf=SOURCE_URDF,
|
||||
result=fit, protected_inputs=hashes, passed=True)
|
||||
validate_o12_runtime_payload_against_urdf(payload, written.path, source_urdf=SOURCE_URDF)
|
||||
assert payload["calibration_scope"] == "full_dynamic_except_thumb_mcp_static_zero"
|
||||
assert payload["quality"]["cad_static_joints_not_measured"] == ["thumb_mcp"]
|
||||
assert set(payload["quality"]["static_zero_exclusions"]) == {"thumb_mcp"}
|
||||
assert payload["joints"]["thumb_mcp"]["calibration_status"] == "measured_dynamic_cad_static"
|
||||
assert payload["joints"]["thumb_mcp"]["static_urdf_origin_offset_rad"] == 0.0
|
||||
assert payload["quality"]["full_hand_spatial_zero"]["passed"]
|
||||
invalid = replace(
|
||||
fit,
|
||||
zero_offsets_rad={**fit.zero_offsets_rad, "thumb_mcp": 0.01},
|
||||
)
|
||||
with pytest.raises(ValueError, match="retain immutable source-CAD"):
|
||||
write_o12_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path / "invalid",
|
||||
serial_number="INVALID",
|
||||
result=invalid,
|
||||
)
|
||||
with pytest.raises(ValueError, match="retain immutable source-CAD"):
|
||||
build_o12_runtime_payload(
|
||||
serial_number="INVALID",
|
||||
source_urdf=SOURCE_URDF,
|
||||
result=invalid,
|
||||
protected_inputs=hashes,
|
||||
passed=True,
|
||||
)
|
||||
|
||||
|
||||
def test_failed_spatial_solve_writes_evidence_without_replacing_published_result(monkeypatch, tmp_path):
|
||||
from linkerhand_calibration.models.o12 import pipeline
|
||||
import json
|
||||
previous = tmp_path / "previous"
|
||||
previous.mkdir()
|
||||
pointer = tmp_path / "latest_passed"
|
||||
pointer.symlink_to(previous, target_is_directory=True)
|
||||
diagnostic = {"passed": False, "stage": "axis_observation", "joint": "thumb_mcp"}
|
||||
def fail(*args, **kwargs):
|
||||
assert kwargs["require_full_hand_spatial_zero"]
|
||||
raise O12SpatialZeroError("unreliable geometry", diagnostic)
|
||||
monkeypatch.setattr(pipeline, "fit_o12_session", fail)
|
||||
session = tmp_path / "new"
|
||||
with pytest.raises(O12SpatialZeroError):
|
||||
pipeline.finalize_o12_session(session_dir=session, serial_number="FULL",
|
||||
source_urdf=SOURCE_URDF, protected_inputs={}, records=[], publish=True)
|
||||
assert pointer.resolve() == previous
|
||||
assert not list(session.glob("*.urdf"))
|
||||
assert json.loads((session / "spatial_zero_diagnostics.json").read_text()) == diagnostic
|
||||
|
||||
|
||||
def test_identifiable_failed_fit_exports_only_review_model(monkeypatch, tmp_path, full_solution):
|
||||
import json
|
||||
from dataclasses import asdict
|
||||
from linkerhand_calibration.models.o12 import pipeline
|
||||
from linkerhand_calibration.models.o12.fitting import fit_o12_session
|
||||
from linkerhand_calibration.models.o12.artifacts import build_o12_runtime_payload
|
||||
truth, valid_zero = full_solution
|
||||
invalid_zero = replace(valid_zero, passed=False,
|
||||
failure_reasons={"index_pip": "zero_phase_axis_line_residual_too_large"})
|
||||
fit = fit_o12_session(SOURCE_URDF, _synthetic_records())
|
||||
offsets = {
|
||||
**truth,
|
||||
"thumb_mcp": 0.0,
|
||||
"ring_mcp_pitch": truth["pinky_mcp_pitch"],
|
||||
}
|
||||
fit = replace(fit, zero_offsets_rad=offsets, full_hand_zero_result=invalid_zero)
|
||||
error = O12SpatialZeroError("unreliable geometry", {
|
||||
"passed": False, "stage": "spatial_solve", "result": asdict(invalid_zero)})
|
||||
error.review_fit = fit
|
||||
def fail(*args, **kwargs):
|
||||
raise error
|
||||
monkeypatch.setattr(pipeline, "fit_o12_session", fail)
|
||||
previous = tmp_path / "previous"
|
||||
previous.mkdir()
|
||||
pointer = tmp_path / "latest_passed"
|
||||
pointer.symlink_to(previous, target_is_directory=True)
|
||||
session = tmp_path / "new"
|
||||
with pytest.raises(O12SpatialZeroError, match="仅供复核"):
|
||||
pipeline.finalize_o12_session(session_dir=session, serial_number="FULL",
|
||||
source_urdf=SOURCE_URDF, protected_inputs={}, records=[], publish=True,
|
||||
timestamp="20260909_000000")
|
||||
assert pointer.resolve() == previous
|
||||
assert not list(session.glob("*.urdf"))
|
||||
assert not list(session.rglob("*calibration.json"))
|
||||
manifest = json.loads((session / "review_only/review_manifest.json").read_text())
|
||||
assert not manifest["publication_allowed"]
|
||||
assert not manifest["spatial_validation"]["result"]["passed"]
|
||||
assert "REVIEW_ONLY" in manifest["urdf"]
|
||||
assert (session / "review_only" / manifest["urdf"]).is_file()
|
||||
with pytest.raises(ValueError):
|
||||
build_o12_runtime_payload(serial_number="FULL", source_urdf=SOURCE_URDF,
|
||||
result=fit, protected_inputs={}, passed=True)
|
||||
@@ -0,0 +1,117 @@
|
||||
import copy
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
from linkerhand_calibration.models.o12 import observations as module
|
||||
from linkerhand_calibration.pnp import SquareTagPose
|
||||
|
||||
|
||||
def build_data(monkeypatch, *, wrong_holdout=False):
|
||||
"""Ambiguous initial poses: the wrong distal branch has lower image error."""
|
||||
rows=[]; pose_lookup={}; idx=0
|
||||
camera=R.from_euler('xyz',[.2,-.3,.4])
|
||||
mounts=[R.from_euler('xyz',angles) for angles in ([.1,.3,.2],[-.3,.5,-.4],[.2,.6,-.7])]
|
||||
def payload(rotation,t,error):
|
||||
return dict(quaternion_xyzw=(camera*rotation).as_quat().tolist(),
|
||||
translation_xyz_m=(camera.apply(t)+[0,0,1]).tolist(),reprojection_error_px=error)
|
||||
for cycle in range(4):
|
||||
for theta in np.linspace(0,.9,24):
|
||||
idx+=1
|
||||
driver=R.from_euler('z',theta)
|
||||
good=[payload(mounts[0],[0,0,0],.05),
|
||||
payload(driver*mounts[1],driver.apply([.03,0,0]),.05),
|
||||
payload(R.from_euler('z',2*theta)*mounts[2],driver.apply([.04,0,0])+R.from_euler('z',2*theta).apply([.02,0,0]),.08)]
|
||||
bad=payload(R.from_euler('x',2*theta)*R.from_euler('y',.8)*mounts[2],[0,0,0],.01)
|
||||
bad['translation_xyz_m']=good[2]['translation_xyz_m']
|
||||
# Holdout is allowed to disagree; it may never choose the training branch.
|
||||
if wrong_holdout and cycle==3:
|
||||
good[2]['quaternion_xyzw']=bad['quaternion_xyzw']
|
||||
evidence={'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','cycle':cycle,'direction':'decreasing','attempt':1,
|
||||
'image_stamp_ns':idx,'camera_matrix':np.eye(3).tolist(),
|
||||
'camera_matrix_source':module.MATRIX_SOURCE,'tag_size_m':.016,'roles':{}}
|
||||
for j,role in enumerate(module.THUMB_ROLES):
|
||||
marker=idx*10+j
|
||||
candidates=[good[j],good[j]] if j<2 else [bad,good[j]]
|
||||
pose_lookup[marker]=[SquareTagPose(tuple(p['quaternion_xyzw']),tuple(p['translation_xyz_m']),p['reprojection_error_px']) for p in candidates]
|
||||
evidence['roles'][role]={'corners_xy':[[marker,0]]*4,'selected':candidates[0],
|
||||
'maximum_reprojection_error_px':1.5}
|
||||
rows.append(evidence)
|
||||
for j,name in enumerate(('thumb_mcp','thumb_dip')):
|
||||
pa=evidence['roles'][module.THUMB_ROLES[j]]['selected']
|
||||
ch=evidence['roles'][module.THUMB_ROLES[j+1]]['selected']
|
||||
q,t=module._relative(pa,ch)
|
||||
rows.append({'kind':'o12_joint_sample','task_name':'thumb_mcp_dip_front',
|
||||
'joint':name,'cycle':cycle,'direction':'decreasing','attempt':1,
|
||||
'image_stamp_ns':idx,'parent_pose_common':pa,'child_pose_common':ch,
|
||||
'relative_quaternion_xyzw':q.tolist(),'relative_translation_xyz_m':t.tolist()})
|
||||
monkeypatch.setattr(module,'solve_square_tag_ippe',lambda xy,**kw:pose_lookup[xy[0][0]])
|
||||
return rows
|
||||
|
||||
|
||||
def test_complete_training_can_reconsider_wrong_initial_branch(monkeypatch):
|
||||
rows=build_data(monkeypatch);original=copy.deepcopy(rows)
|
||||
resolved,report=module.resolve_thumb_observations(rows)
|
||||
assert rows==original
|
||||
assert report['status']=='resolved' and report['selected_branches'][2]==1
|
||||
assert report['training_frames']==72 and report['holdout_frames']==24
|
||||
assert report['changed_joint_rows']>0
|
||||
assert resolved!=rows
|
||||
|
||||
|
||||
def test_holdout_cannot_change_training_branch_or_scores(monkeypatch):
|
||||
_,a=module.resolve_thumb_observations(build_data(monkeypatch))
|
||||
_,b=module.resolve_thumb_observations(build_data(monkeypatch,wrong_holdout=True))
|
||||
assert a['selected_branches']==b['selected_branches']
|
||||
assert a['hypotheses']==b['hypotheses']
|
||||
|
||||
|
||||
def test_legacy_projection_is_not_silently_relabelled(monkeypatch):
|
||||
rows=build_data(monkeypatch)
|
||||
for r in rows:r.pop('camera_matrix_source',None)
|
||||
result,report=module.resolve_thumb_observations(rows)
|
||||
assert report['status']=='legacy_projection_unverified' and result==rows
|
||||
assert module.resolve_thumb_observations([])[1]['status']=='legacy_no_corners'
|
||||
|
||||
|
||||
def test_missing_evidence_cannot_be_silently_dropped(monkeypatch):
|
||||
rows=build_data(monkeypatch)
|
||||
rows=[r for r in rows if not(r['kind']=='o12_pnp_candidate_frame' and r['image_stamp_ns']==20)]
|
||||
with pytest.raises(ValueError,match='lacks evidence'):module.resolve_thumb_observations(rows)
|
||||
|
||||
|
||||
def test_other_task_evidence_is_not_mixed_into_thumb(monkeypatch):
|
||||
rows=build_data(monkeypatch)
|
||||
rows.append({'kind':'o12_pnp_candidate_frame','task_name':'pinky_chain_side','image_stamp_ns':1})
|
||||
assert module.resolve_thumb_observations(rows)[1]['status']=='resolved'
|
||||
|
||||
|
||||
def test_partial_external_projection_cannot_finalize_whole_hand(tmp_path):
|
||||
from linkerhand_calibration.models.o12.pipeline import finalize_o12_session
|
||||
with pytest.raises(ValueError,match='diagnostic-only'):
|
||||
finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf='unused',
|
||||
protected_inputs={},records=[{'projection_reprocessing_scope':'thumb_only_external_projection'}],publish=False)
|
||||
|
||||
|
||||
def test_known_unverified_projection_cannot_publish_even_if_fit_passes(monkeypatch,tmp_path):
|
||||
from dataclasses import dataclass,field
|
||||
from linkerhand_calibration.models.o12 import pipeline
|
||||
from linkerhand_calibration.models.o12.zero import O12SpatialZeroError
|
||||
@dataclass
|
||||
class Spatial:
|
||||
passed: bool=True
|
||||
failure_reasons: dict=field(default_factory=dict)
|
||||
@dataclass
|
||||
class Fit:
|
||||
full_hand_zero_result: Spatial=field(default_factory=Spatial)
|
||||
zero_offsets_rad: dict=field(default_factory=dict)
|
||||
monkeypatch.setattr(pipeline,'fit_o12_session',lambda *a,**k:Fit())
|
||||
def no_review(**kwargs):
|
||||
raise RuntimeError('test omits review geometry')
|
||||
monkeypatch.setattr(pipeline,'write_o12_corrected_urdf',no_review)
|
||||
with pytest.raises(O12SpatialZeroError) as error:
|
||||
pipeline.finalize_o12_session(session_dir=tmp_path/'out',serial_number='test',source_urdf='unused',
|
||||
protected_inputs={},records=[{'kind':'o12_pnp_candidate_frame','task_name':'thumb_mcp_dip_front','image_stamp_ns':1}])
|
||||
assert error.value.diagnostics['stage']=='camera_projection'
|
||||
assert not error.value.review_fit.full_hand_zero_result.passed
|
||||
assert not (tmp_path/'latest_passed').exists()
|
||||
@@ -0,0 +1,49 @@
|
||||
"""Optional local-data replay: deterministic recovery, NOT physical certification.
|
||||
|
||||
Set O12_REPLAY_RAW and O12_REPLAY_REFERENCE to use a captured session and an
|
||||
independently reviewed model. The test never publishes or launches hardware.
|
||||
"""
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from linkerhand_calibration.models.o12.pipeline import finalize_o12_session, load_o12_raw_samples
|
||||
from linkerhand_calibration.models.o12.zero import O12SpatialZeroError
|
||||
from linkerhand_calibration.product import sha256_file
|
||||
from linkerhand_calibration.storage import atomic_write_json
|
||||
from linkerhand_calibration.urdf_comparison import compare_urdfs
|
||||
from test_o12_right_profile import SOURCE_URDF
|
||||
|
||||
|
||||
@pytest.mark.skipif(not os.environ.get("O12_REPLAY_RAW") or not os.environ.get("O12_REPLAY_REFERENCE"),
|
||||
reason="local raw recording/reference not supplied")
|
||||
def test_recorded_replay_matches_reviewed_geometry_without_overriding_quality(tmp_path):
|
||||
raw = Path(os.environ["O12_REPLAY_RAW"]).resolve(strict=True)
|
||||
reference = Path(os.environ["O12_REPLAY_REFERENCE"]).resolve(strict=True)
|
||||
output = Path(os.environ.get("O12_REPLAY_OUTPUT", str(tmp_path / "replay"))).resolve()
|
||||
assert not output.exists(), "use a new output directory"
|
||||
hashes = {str(p): sha256_file(p) for p in (raw, reference, SOURCE_URDF)}
|
||||
try:
|
||||
_, fit, correction = finalize_o12_session(
|
||||
session_dir=output, serial_number="O12_RIGHT_001", source_urdf=SOURCE_URDF,
|
||||
protected_inputs=hashes, records=load_o12_raw_samples(raw), publish=False,
|
||||
)
|
||||
candidate = correction.path
|
||||
assert fit.full_hand_zero_result.passed
|
||||
except O12SpatialZeroError as error:
|
||||
assert error.review_fit is not None
|
||||
assert not error.review_fit.full_hand_zero_result.passed
|
||||
assert error.review_fit.full_hand_zero_result.failure_reasons
|
||||
candidate = Path(error.diagnostics["review_urdf"])
|
||||
assert "REVIEW_ONLY" in candidate.name
|
||||
assert not list(output.glob("*calibration.json"))
|
||||
report = compare_urdfs(reference, candidate)
|
||||
atomic_write_json(output / "reference_geometry_comparison.json", report)
|
||||
assert report["maximum_origin_distance_mm"] < 1.e-5
|
||||
assert report["maximum_orientation_difference_deg"] < 1.e-5
|
||||
assert report["link_elements_identical"]
|
||||
for ranges in report["active_joint_ranges_rad"].values():
|
||||
assert ranges["reference"] == pytest.approx(ranges["candidate"], abs=1.e-10)
|
||||
assert all(sha256_file(p) == digest for p, digest in hashes.items())
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,156 @@
|
||||
"""Absolute zero and transfer tests, independent of curve-fit repeatability."""
|
||||
|
||||
from dataclasses import replace
|
||||
import math
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel
|
||||
from linkerhand_calibration.models.o12.fitting import (
|
||||
O12_THUMB_ROOT_AXIS_JOINTS, _fit_thumb_root_zero, fit_o12_session,
|
||||
)
|
||||
from linkerhand_calibration.models.o12.profile import build_typed_profile
|
||||
from linkerhand_calibration.models.o12.artifacts import (
|
||||
build_o12_runtime_payload, validate_o12_runtime_payload,
|
||||
validate_o12_runtime_payload_against_urdf,
|
||||
)
|
||||
from linkerhand_calibration.models.o12.urdf import write_o12_corrected_urdf
|
||||
from test_o12_right_profile import SOURCE_URDF, _synthetic_records
|
||||
|
||||
|
||||
def _transform(xyz, rpy):
|
||||
t = np.eye(4)
|
||||
t[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix()
|
||||
t[:3, 3] = xyz
|
||||
return t
|
||||
|
||||
|
||||
def _pose(t):
|
||||
return {"translation_xyz_m": t[:3, 3].tolist(),
|
||||
"quaternion_xyzw": Rotation.from_matrix(t[:3, :3]).as_quat().tolist()}
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def rotation_fit():
|
||||
return fit_o12_session(SOURCE_URDF, _synthetic_records())
|
||||
|
||||
|
||||
def _geometry_records(fit, offsets, camera_rpy):
|
||||
model = UrdfKinematicModel(SOURCE_URDF)
|
||||
tasks = {j: t for t in build_typed_profile().motion.tasks for j in t.joints}
|
||||
palm = _transform([.02, -.01, .7], camera_rpy)
|
||||
records = {}
|
||||
for i, name in enumerate(O12_THUMB_ROOT_AXIS_JOINTS):
|
||||
task = tasks[name]
|
||||
mounting = _transform([.012, -.009, .02], [.13 * i, -.24, .31])
|
||||
parent = palm @ _transform([-.01, .007, .003], [-.12, .1 * i, .2])
|
||||
rows = []
|
||||
for cycle in range(4):
|
||||
for direction, phases in (("decreasing", np.linspace(0, 1, 65)),
|
||||
("increasing", np.linspace(1, 0, 65))):
|
||||
for phase in phases:
|
||||
state = [0.] * 12
|
||||
state[task.command_index] = task.start_value + phase * (task.end_value - task.start_value)
|
||||
child = palm @ model.link_transform(
|
||||
name, zero_offsets=offsets,
|
||||
joint_angles={name: float(phase * fit.travels_rad[name])},
|
||||
) @ mounting
|
||||
relative = np.linalg.inv(parent) @ child
|
||||
rows.append({
|
||||
"cycle": cycle, "direction": direction,
|
||||
"feedback_rad": state[task.command_index], "state_rad": state,
|
||||
"relative_quaternion_xyzw": _pose(relative)["quaternion_xyzw"],
|
||||
"relative_translation_xyz_m": relative[:3, 3].tolist(),
|
||||
"parent_pose_common": _pose(parent), "child_pose_common": _pose(child),
|
||||
"view_normal_common_xyz": [0., 0., 1.],
|
||||
"camera_center_common_xyz_m": [0., 0., 0.],
|
||||
})
|
||||
records[name] = rows
|
||||
return records
|
||||
|
||||
|
||||
@pytest.mark.parametrize("camera_rpy", [[.2, -.3, .4], [-.3, .5, -.6]])
|
||||
def test_root_zeros_recover_known_geometry_with_arbitrary_tag_mounts(rotation_fit, camera_rpy, tmp_path):
|
||||
truth = {"thumb_cmc_roll": .08, "thumb_cmc_yaw": -.12}
|
||||
rows = _geometry_records(rotation_fit, truth, camera_rpy)
|
||||
solved = _fit_thumb_root_zero(SOURCE_URDF, rows, rotation_fit.curves,
|
||||
rotation_fit.feedback_domains_rad)
|
||||
assert solved.passed
|
||||
assert set(solved.direct_offsets_rad) == set(truth)
|
||||
for name, value in truth.items():
|
||||
assert solved.direct_offsets_rad[name] == pytest.approx(value, abs=1.e-5)
|
||||
result = replace(rotation_fit, zero_offsets_rad={
|
||||
**rotation_fit.zero_offsets_rad, **solved.direct_offsets_rad,
|
||||
})
|
||||
corrected = write_o12_corrected_urdf(source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path, serial_number="ROOT", result=result)
|
||||
actual, original = UrdfKinematicModel(corrected.path), UrdfKinematicModel(SOURCE_URDF)
|
||||
for state in ({}, {"thumb_cmc_roll": .72},
|
||||
{"thumb_cmc_roll": .36, "thumb_cmc_yaw": .4, "thumb_cmc_pitch": .3, "thumb_mcp": .5}):
|
||||
assert actual.link_transform("thumb_mcp", zero_offsets={}, joint_angles=state) == pytest.approx(
|
||||
original.link_transform("thumb_mcp", zero_offsets=truth, joint_angles=state), abs=1.e-5)
|
||||
|
||||
|
||||
def test_root_holdout_rejects_changed_camera_pose(rotation_fit):
|
||||
rows = _geometry_records(rotation_fit, {"thumb_cmc_roll": .08, "thumb_cmc_yaw": -.12}, [.2, -.3, .4])
|
||||
rotation = Rotation.from_rotvec([.15, .05, .1])
|
||||
for row in rows["thumb_cmc_yaw"]:
|
||||
if row["cycle"] == 3:
|
||||
for field in ("parent_pose_common", "child_pose_common"):
|
||||
p = row[field]
|
||||
p["translation_xyz_m"] = rotation.apply(p["translation_xyz_m"]).tolist()
|
||||
p["quaternion_xyzw"] = (rotation * Rotation.from_quat(p["quaternion_xyzw"])).as_quat().tolist()
|
||||
with pytest.raises(ValueError, match="spatial zero solve failed"):
|
||||
_fit_thumb_root_zero(SOURCE_URDF, rows, rotation_fit.curves,
|
||||
rotation_fit.feedback_domains_rad)
|
||||
|
||||
|
||||
def test_rotation_only_records_cannot_pass_production_static_solve():
|
||||
with pytest.raises(ValueError, match="absolute zero requires"):
|
||||
fit_o12_session(SOURCE_URDF, _synthetic_records(), require_thumb_root_spatial_zero=True)
|
||||
|
||||
|
||||
def test_ring_transfers_nonzero_correction_without_copying_geometry(rotation_fit, tmp_path):
|
||||
offsets = {**rotation_fit.zero_offsets_rad, "pinky_mcp_pitch": .06, "ring_mcp_pitch": .06}
|
||||
result = replace(rotation_fit, zero_offsets_rad=offsets)
|
||||
corrected = write_o12_corrected_urdf(source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path, serial_number="TRANSFER", result=result)
|
||||
original = ET.parse(SOURCE_URDF).getroot()
|
||||
actual = ET.parse(corrected.path).getroot()
|
||||
for name in ("pinky_mcp_pitch", "ring_mcp_pitch"):
|
||||
before = original.find(f"joint[@name='{name}']/origin")
|
||||
after = actual.find(f"joint[@name='{name}']/origin")
|
||||
assert before.get("xyz") == after.get("xyz")
|
||||
rb = Rotation.from_euler("xyz", [float(x) for x in before.get("rpy").split()])
|
||||
ra = Rotation.from_euler("xyz", [float(x) for x in after.get("rpy").split()])
|
||||
assert (rb.inv() * ra).as_rotvec() == pytest.approx([0., .06, 0.], abs=1.e-10)
|
||||
for name in ("ring_pip", "ring_dip"):
|
||||
assert ET.tostring(original.find(f"joint[@name='{name}']")) == ET.tostring(actual.find(f"joint[@name='{name}']"))
|
||||
hashes = {k: "a" * 64 for k in build_typed_profile().artifacts.protected_input_fields}
|
||||
payload = build_o12_runtime_payload(serial_number="TRANSFER", source_urdf=SOURCE_URDF,
|
||||
result=result, protected_inputs=hashes, passed=True)
|
||||
ring = payload["joints"]["ring_mcp_pitch"]
|
||||
pinky = payload["joints"]["pinky_mcp_pitch"]
|
||||
# Equal feedback must have equal corrections before the ring saturates.
|
||||
for i, value in enumerate(pinky["angle_rad"]):
|
||||
assert ring["angle_rad"][i] == pytest.approx(min(1.38, value))
|
||||
validate_o12_runtime_payload_against_urdf(payload, corrected.path, source_urdf=SOURCE_URDF)
|
||||
payload["joints"]["ring_mcp_pitch"]["static_urdf_origin_offset_rad"] = 0.
|
||||
with pytest.raises(ValueError, match="static zero differs"):
|
||||
validate_o12_runtime_payload(payload)
|
||||
|
||||
|
||||
def test_static_frame_validator_catches_correct_limits_but_wrong_origin(rotation_fit, tmp_path):
|
||||
corrected = write_o12_corrected_urdf(source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path, serial_number="FRAME", result=rotation_fit)
|
||||
hashes = {k: "a" * 64 for k in build_typed_profile().artifacts.protected_input_fields}
|
||||
payload = build_o12_runtime_payload(serial_number="FRAME", source_urdf=SOURCE_URDF,
|
||||
result=rotation_fit, protected_inputs=hashes, passed=True)
|
||||
tree = ET.parse(corrected.path)
|
||||
tree.getroot().find("joint[@name='thumb_cmc_pitch']/origin").set("rpy", "0 0.9259 -1.5708")
|
||||
tree.write(corrected.path)
|
||||
with pytest.raises(ValueError, match="thumb_cmc_pitch static origin disagrees"):
|
||||
validate_o12_runtime_payload_against_urdf(payload, corrected.path, source_urdf=SOURCE_URDF)
|
||||
@@ -0,0 +1,124 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
from linkerhand_calibration.models.o12.pnp import O12ThumbPoseTracker, THUMB_ROLES
|
||||
from linkerhand_calibration.pnp import SquareTagPose
|
||||
|
||||
|
||||
def pose(rotation, error=0.05):
|
||||
return SquareTagPose(tuple(rotation.as_quat()), (0., 0., 1.), error)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('remount', [False, True])
|
||||
@pytest.mark.parametrize('passive_travel', [12, 28, 45])
|
||||
def test_parallel_branch_disambiguation_without_angle_ratio(remount, passive_travel):
|
||||
tracker=O12ThumbPoseTracker()
|
||||
mounts=[R.identity()]*3 if not remount else [
|
||||
R.from_euler('xyz', angles) for angles in
|
||||
([.3,-.7,.2],[-.5,.4,.8],[.6,.2,-.4])]
|
||||
camera=R.from_euler('xyz',[.4,-.8,.7])
|
||||
baseline={role:(pose(camera*mount),) for role,mount in zip(THUMB_ROLES,mounts)}
|
||||
for i in range(8):
|
||||
selected,reason=tracker.select(baseline,stamp_ns=1_000_000_000+i*30_000_000)
|
||||
assert selected is not None
|
||||
driver=R.from_euler('z',20,degrees=True)
|
||||
follower=R.from_euler('z',passive_travel,degrees=True)
|
||||
wrong=R.from_euler('x',passive_travel,degrees=True)
|
||||
good_pose=pose(camera*driver*follower*mounts[2],.08)
|
||||
candidates={THUMB_ROLES[0]:baseline[THUMB_ROLES[0]],
|
||||
THUMB_ROLES[1]:(pose(camera*driver*mounts[1]),),
|
||||
THUMB_ROLES[2]:(pose(camera*driver*wrong*mounts[2]),good_pose)}
|
||||
selected,reason=tracker.select(candidates,stamp_ns=1_250_000_000)
|
||||
assert reason == ''
|
||||
assert selected[THUMB_ROLES[2]] == good_pose
|
||||
# Angles are observed candidates, not replaced with driver*mimic.
|
||||
assert tracker.coupled_rotation_pairs == ()
|
||||
|
||||
|
||||
def test_no_credible_parallel_candidate_does_not_stop_or_invent_pose():
|
||||
tracker=O12ThumbPoseTracker()
|
||||
baseline={role:(pose(R.identity()),) for role in THUMB_ROLES}
|
||||
for i in range(8):
|
||||
tracker.select(baseline,stamp_ns=i*30_000_000)
|
||||
bad=pose(R.from_euler('zx',[20,45],degrees=True))
|
||||
selected,reason=tracker.select({THUMB_ROLES[0]:baseline[THUMB_ROLES[0]],
|
||||
THUMB_ROLES[1]:(pose(R.from_euler('z',20,degrees=True)),),
|
||||
THUMB_ROLES[2]:(bad,)},stamp_ns=250_000_000)
|
||||
assert reason == ''
|
||||
assert selected[THUMB_ROLES[2]] == bad
|
||||
tracker.reset()
|
||||
assert tracker.reference is None
|
||||
|
||||
|
||||
def test_missing_tag_discards_observation_and_gap_reinitializes():
|
||||
tracker=O12ThumbPoseTracker()
|
||||
baseline={role:(pose(R.identity()),) for role in THUMB_ROLES}
|
||||
for i in range(8):
|
||||
tracker.select(baseline,stamp_ns=i*30_000_000)
|
||||
selected,reason=tracker.select({},stamp_ns=250_000_000)
|
||||
assert selected is None and reason == 'group_missing_pose_candidates'
|
||||
selected,_=tracker.select(baseline,stamp_ns=10_000_000_000)
|
||||
assert selected is None and tracker.reference is None
|
||||
|
||||
|
||||
def test_hook_only_applies_to_o12_thumb_task():
|
||||
from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode
|
||||
from linkerhand_calibration.models.l6.node import L6ThreeCameraCalibrationNode
|
||||
from linkerhand_calibration.models.o6.node import O6ThreeCameraCalibrationNode
|
||||
assert not hasattr(L6ThreeCameraCalibrationNode,'_select_articulated_capture_poses')
|
||||
assert not hasattr(O6ThreeCameraCalibrationNode,'_select_articulated_capture_poses')
|
||||
hook=O12ThreeCameraCalibrationNode._select_articulated_capture_poses
|
||||
assert hook(SimpleNamespace(),'side',None,(),{},0) is None
|
||||
assert hook(SimpleNamespace(),'front',SimpleNamespace(task_key='thumb_pitch_front'),(),{},0) is None
|
||||
|
||||
|
||||
def test_hook_records_corners_all_candidates_and_rejected_frames(monkeypatch, tmp_path):
|
||||
import json
|
||||
from linkerhand_calibration.models.o12 import node as module
|
||||
from linkerhand_calibration.models.o12.profile import build_typed_profile
|
||||
profile=build_typed_profile()
|
||||
front=next(v for v in profile.vision.views if v.name == 'front')
|
||||
good=pose(R.identity())
|
||||
high_error=pose(R.from_euler('y',.5),3.)
|
||||
monkeypatch.setattr(module,'solve_square_tag_ippe',lambda *a,**k:[good,high_error])
|
||||
raw=tmp_path/'raw.jsonl'
|
||||
node=SimpleNamespace(trackers={'front':SimpleNamespace(maximum_reprojection_error_px=1.5)},
|
||||
camera_matrices={'front':np.diag([500.,500.,1.])},tag_size_m=.016,
|
||||
raw_path=raw,_view=lambda view:front)
|
||||
step=SimpleNamespace(task_key='thumb_mcp_dip_front',cycle=0,direction='decreasing',attempt=1,phase='sweep')
|
||||
corners={role:np.array([[1.,1.],[2.,1.],[2.,2.],[1.,2.]]) for role in THUMB_ROLES}
|
||||
hook=module.O12ThreeCameraCalibrationNode._select_articulated_capture_poses
|
||||
for i in range(8):
|
||||
selected,reason=hook(node,'front',step,THUMB_ROLES,corners,i*30_000_000)
|
||||
assert selected is not None
|
||||
records=[json.loads(line) for line in raw.read_text().splitlines()]
|
||||
assert len(records)==8
|
||||
assert records[0]['roles']['thumb_dip']['selected'] is None
|
||||
assert len(records[-1]['roles']['thumb_dip']['candidates'])==2
|
||||
assert records[-1]['roles']['thumb_dip']['eligible_candidate_count']==1
|
||||
assert records[-1]['roles']['thumb_dip']['tag_id']==3
|
||||
assert records[-1]['camera_matrix']==node.camera_matrices['front'].tolist()
|
||||
assert records[-1]['roles']['thumb_mcp']['corners_xy']==corners['thumb_mcp'].tolist()
|
||||
|
||||
|
||||
def test_other_task_evidence_distinguishes_locked_reference_from_pixels(tmp_path):
|
||||
import json
|
||||
from linkerhand_calibration.models.o12.node import O12ThreeCameraCalibrationNode
|
||||
roles=('front_base','moving')
|
||||
poses={r:pose(R.identity()) for r in roles}
|
||||
node=SimpleNamespace(raw_path=tmp_path/'raw.jsonl',tag_size_m=.016,
|
||||
camera_matrices={'front':np.eye(3)},
|
||||
trackers={'front':SimpleNamespace(last_candidates_by_role={r:(p,) for r,p in poses.items()},maximum_reprojection_error_px=1.5)},
|
||||
_view=lambda v:SimpleNamespace(tags=[SimpleNamespace(role=r,tag_id=i) for i,r in enumerate(roles)]))
|
||||
step=SimpleNamespace(task_key='index_roll_front',cycle=0,direction='decreasing',attempt=1,phase='sweep')
|
||||
corners={r:np.zeros((4,2)) for r in roles}
|
||||
O12ThreeCameraCalibrationNode._record_capture_pose_evidence(node,'front',step,roles,corners,poses,1,locked_roles=('front_base',))
|
||||
record=json.loads(node.raw_path.read_text())
|
||||
assert record['camera_matrix_source']=='CameraInfo.P[:3,:3]'
|
||||
assert record['roles']['front_base']['observation_source']=='locked_reference'
|
||||
assert record['roles']['front_base']['candidates']==[]
|
||||
assert record['roles']['moving']['observation_source']=='image'
|
||||
@@ -208,7 +208,7 @@ def test_o6_motion_plan_uses_o6_specific_speed_tiers() -> None:
|
||||
steps = O6ThreeCameraCalibrationNode._build_steps(fake)
|
||||
assert steps[0].phase == "baseline"
|
||||
assert steps[0].speed_u8 == 80
|
||||
assert {step.speed_u8 for step in steps if step.phase == "preflight"} == {60}
|
||||
assert not [step for step in steps if step.phase == "preflight"]
|
||||
assert {
|
||||
step.speed_u8
|
||||
for step in steps
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
from types import SimpleNamespace
|
||||
import numpy as np
|
||||
import pytest
|
||||
import cv2
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
from linkerhand_calibration.core.geometry.camera import rectified_camera_matrix
|
||||
from linkerhand_calibration.pnp import solve_square_tag_ippe, square_object_points
|
||||
|
||||
|
||||
def test_rectified_corners_require_projection_not_raw_intrinsics():
|
||||
k=np.array([[3816.108989,0,620.129591],[0,3796.2534,570.26288],[0,0,1.]])
|
||||
p=np.array([[3792.732422,0,606.014017,0],[0,3825.369629,568.524418,0],[0,0,1,0.]])
|
||||
expected=R.from_euler('xyz',[.2,-.5,.3])
|
||||
t=np.array([.12,.05,1.])
|
||||
corners=cv2.projectPoints(square_object_points(.016),expected.as_rotvec(),t,p[:,:3],np.zeros(4))[0].reshape(4,2)
|
||||
candidates=solve_square_tag_ippe(corners,tag_size_m=.016,camera_matrix=rectified_camera_matrix(p))
|
||||
errors=[(expected.inv()*R.from_quat(c.quaternion_xyzw)).magnitude() for c in candidates]
|
||||
assert min(errors)<1e-6
|
||||
wrong=solve_square_tag_ippe(corners,tag_size_m=.016,camera_matrix=k)
|
||||
assert min((expected.inv()*R.from_quat(c.quaternion_xyzw)).magnitude() for c in wrong)>.001
|
||||
|
||||
|
||||
@pytest.mark.parametrize('bad',[[],[0.]*12,[float('nan')]*12])
|
||||
def test_invalid_p_cannot_silently_fall_back_to_k(bad):
|
||||
with pytest.raises(ValueError):rectified_camera_matrix(bad)
|
||||
|
||||
|
||||
def test_shared_camera_callback_uses_p_saves_provenance_and_invalidates(tmp_path):
|
||||
import json
|
||||
from linkerhand_calibration.models.l6.node import L6ThreeCameraCalibrationNode
|
||||
node=SimpleNamespace(camera_matrices={},image_sizes={},raw_path=tmp_path/'raw.jsonl')
|
||||
p=[500.,0,320,0,0,510,240,0,0,0,1,0]
|
||||
msg=SimpleNamespace(p=p,k=[600.,0,300,0,620,220,0,0,1],d=[.1]*5,r=np.eye(3).ravel().tolist(),width=640,height=480)
|
||||
callback=L6ThreeCameraCalibrationNode._camera_info_callback
|
||||
callback(node,'front',msg); callback(node,'front',msg)
|
||||
np.testing.assert_allclose(node.camera_matrices['front'],np.array(p).reshape(3,4)[:,:3])
|
||||
records=[json.loads(l) for l in node.raw_path.read_text().splitlines()]
|
||||
assert len(records)==1 and records[0]['raw_k']==msg.k
|
||||
assert records[0]['matrix_source']=='CameraInfo.P[:3,:3]'
|
||||
msg.p=[0.]*12
|
||||
callback(node,'front',msg)
|
||||
assert 'front' not in node.camera_matrices
|
||||
@@ -235,16 +235,14 @@ def test_refit_invalidates_all_derived_calibration_artifacts() -> None:
|
||||
def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None:
|
||||
plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4)
|
||||
|
||||
assert len(plan) == len(RIGHT_19_HAND_PROFILE.sweep_specs) * 10
|
||||
assert len(plan) == len(RIGHT_19_HAND_PROFILE.sweep_specs) * 8
|
||||
for spec_index, spec in enumerate(RIGHT_19_HAND_PROFILE.sweep_specs):
|
||||
task = plan[spec_index * 10 : (spec_index + 1) * 10]
|
||||
task = plan[spec_index * 8 : (spec_index + 1) * 8]
|
||||
assert all(item.spec == spec for item in task)
|
||||
assert [
|
||||
(item.precheck, item.cycle, item.direction)
|
||||
for item in task
|
||||
] == [
|
||||
(True, -1, DIRECTION_DECREASING),
|
||||
(True, -1, DIRECTION_INCREASING),
|
||||
*[
|
||||
(False, cycle, direction)
|
||||
for cycle in range(4)
|
||||
@@ -259,8 +257,6 @@ def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None:
|
||||
for left, right in zip(task, task[1:])
|
||||
]
|
||||
assert transitions == [
|
||||
"immediate_reverse",
|
||||
"immediate_reverse",
|
||||
"immediate_reverse",
|
||||
"cycle_reset",
|
||||
"immediate_reverse",
|
||||
@@ -271,7 +267,7 @@ def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None:
|
||||
]
|
||||
task_boundaries = [
|
||||
(plan[index], plan[index + 1])
|
||||
for index in range(9, len(plan) - 1, 10)
|
||||
for index in range(7, len(plan) - 1, 8)
|
||||
]
|
||||
assert all(
|
||||
_sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right)
|
||||
@@ -337,8 +333,8 @@ def test_right_19_complete_plan_has_no_unobserved_immediate_handoff() -> None:
|
||||
minimum_per_joint=1,
|
||||
)
|
||||
|
||||
# 16 tasks × (precheck out/back, precheck->formal, four formal reversals).
|
||||
assert immediate_boundaries == 16 * 6
|
||||
# Four formal decreasing/increasing pairs per task; no full-range precheck.
|
||||
assert immediate_boundaries == 16 * 4
|
||||
assert cycle_resets == 16 * 3
|
||||
assert task_changes == 15
|
||||
|
||||
@@ -356,8 +352,7 @@ def test_right_19_initializes_pnp_once_per_normal_task() -> None:
|
||||
]
|
||||
|
||||
assert len(reset_items) == len(RIGHT_19_HAND_PROFILE.sweep_specs)
|
||||
assert all(item.precheck for item in reset_items)
|
||||
assert all(item.cycle == -1 for item in reset_items)
|
||||
assert all(not item.precheck and item.cycle == 0 for item in reset_items)
|
||||
assert all(
|
||||
item.direction == DIRECTION_DECREASING for item in reset_items
|
||||
)
|
||||
@@ -2960,7 +2955,7 @@ def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None
|
||||
assert event["cycles"] == [1]
|
||||
|
||||
|
||||
def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> None:
|
||||
def test_recoverable_sweep_failure_retries_once_at_same_speed(tmp_path) -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||
item = SweepItem(spec, 0, DIRECTION_INCREASING)
|
||||
transitions: list[str] = []
|
||||
@@ -2975,8 +2970,8 @@ def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) ->
|
||||
_publish_hold_current=lambda: holds.append(True),
|
||||
_begin_return_baseline=lambda after: transitions.append(after),
|
||||
_pause=lambda reason: pauses.append(reason),
|
||||
retry_speed_scales=(0.8, 0.6, 0.5),
|
||||
retry_endpoint_hold_seconds=(0.75, 1.0, 1.25),
|
||||
retry_speed_scales=(1.0,),
|
||||
retry_endpoint_hold_seconds=(0.5,),
|
||||
)
|
||||
|
||||
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
||||
@@ -2985,22 +2980,15 @@ def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) ->
|
||||
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
||||
node, "sweep_bin_gap_too_large"
|
||||
)
|
||||
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
||||
node, "sweep_bin_gap_too_large"
|
||||
)
|
||||
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
||||
node, "sweep_bin_gap_too_large"
|
||||
)
|
||||
|
||||
assert transitions == ["retry_sweep", "retry_sweep", "retry_sweep"]
|
||||
assert transitions == ["retry_sweep"]
|
||||
assert pauses == ["sweep_bin_gap_too_large"]
|
||||
assert holds == []
|
||||
assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 3
|
||||
assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 1
|
||||
events = [
|
||||
json.loads(line)
|
||||
for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines()
|
||||
]
|
||||
assert [event["retry"] for event in events] == [1, 2, 3]
|
||||
assert [event["retry"] for event in events] == [1]
|
||||
|
||||
|
||||
def test_visibility_precheck_does_not_require_dense_feedback_bins(tmp_path) -> None:
|
||||
@@ -3209,7 +3197,7 @@ def test_formal_sweep_still_rejects_17_u8_feedback_gap(tmp_path) -> None:
|
||||
assert retries == ["sweep_bin_gap_too_large"]
|
||||
|
||||
|
||||
def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None:
|
||||
def test_first_provisional_fit_failure_stops_without_more_motion(tmp_path) -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 15)
|
||||
transitions: list[str] = []
|
||||
pauses: list[str] = []
|
||||
@@ -3240,10 +3228,9 @@ def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None:
|
||||
[{"joint": "thumb_ip", "metric": "rotation_orthogonal_rms_deg"}],
|
||||
)
|
||||
|
||||
assert prepared == [True]
|
||||
assert transitions == ["resume_sweep"]
|
||||
assert pauses == []
|
||||
assert node.paused_reason == "joint_fit_check_failed"
|
||||
assert prepared == []
|
||||
assert transitions == []
|
||||
assert pauses == ["joint_fit_check_failed"]
|
||||
|
||||
|
||||
def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry(
|
||||
@@ -3290,10 +3277,7 @@ def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry(
|
||||
assert node.fit_failure["directions_to_rescan"] == 0
|
||||
|
||||
|
||||
def test_near_threshold_provisional_failure_rescans_in_place(tmp_path) -> None:
|
||||
# In-band values are rejected by the final fit on the same records, so
|
||||
# the first in-band result reschedules the task here instead of warning
|
||||
# and deferring the rejection to the end of the session.
|
||||
def test_near_threshold_provisional_failure_stops_without_rescan(tmp_path) -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||
node = SimpleNamespace(
|
||||
raw_path=tmp_path / "raw_samples.jsonl",
|
||||
@@ -3333,11 +3317,11 @@ def test_near_threshold_provisional_failure_rescans_in_place(tmp_path) -> None:
|
||||
for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines()
|
||||
]
|
||||
kinds = [event["kind"] for event in events]
|
||||
assert "provisional_fit_warning_rescan" in kinds
|
||||
assert "provisional_fit_warning_retry_exhausted" in kinds
|
||||
assert "fit_failure" in kinds
|
||||
|
||||
|
||||
def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None:
|
||||
def test_motion_timeout_pauses_without_republishing(tmp_path) -> None:
|
||||
commands: list[list[int]] = []
|
||||
pauses: list[str] = []
|
||||
holds: list[bool] = []
|
||||
@@ -3363,12 +3347,12 @@ def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None:
|
||||
node, "return_baseline_timeout", now
|
||||
)
|
||||
|
||||
assert len(commands) == 2
|
||||
assert commands == []
|
||||
assert holds == []
|
||||
assert pauses == ["return_baseline_timeout"]
|
||||
assert pauses == ["return_baseline_timeout"] * 3
|
||||
|
||||
|
||||
def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None:
|
||||
def test_sweep_start_tag_timeout_does_not_block_motion(tmp_path) -> None:
|
||||
spec = next(
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
@@ -3379,6 +3363,8 @@ def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None:
|
||||
resets: list[tuple[object, bool]] = []
|
||||
diagnostic_resets: list[object] = []
|
||||
commands: list[list[int]] = []
|
||||
starts: list[float] = []
|
||||
pauses: list[str] = []
|
||||
start_frames = [object()]
|
||||
node = SimpleNamespace(
|
||||
profile=RIGHT_19_HAND_PROFILE,
|
||||
@@ -3406,22 +3392,18 @@ def test_sweep_start_tag_timeout_resets_pnp_before_retry(tmp_path) -> None:
|
||||
_publish_speed_profile=lambda profile: None,
|
||||
_publish_command=lambda command: commands.append(command),
|
||||
_reset_motion_progress=lambda now, error: None,
|
||||
_pause=lambda reason: pytest.fail(f"unexpected pause: {reason}"),
|
||||
_pause=lambda reason: pauses.append(reason),
|
||||
_begin_active_sweep=lambda now: starts.append(now),
|
||||
)
|
||||
|
||||
G20ThreeCameraCalibrationNode._retry_motion_or_pause(
|
||||
node, "sweep_start_tag_timeout", 20.0
|
||||
G20ThreeCameraCalibrationNode._handle_sweep_start_timeout(
|
||||
node, 20.0, True
|
||||
)
|
||||
|
||||
assert resets == [(runtime, True)]
|
||||
assert diagnostic_resets == [runtime]
|
||||
assert runtime.pnp_invalid_since is None
|
||||
assert runtime.pnp_reset_count == 1
|
||||
assert start_frames == []
|
||||
assert len(commands) == 1
|
||||
assert starts == [20.0]
|
||||
assert pauses == []
|
||||
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
|
||||
assert event["pnp_trackers_reset"] == ["side"]
|
||||
assert event["task_reference_preserved"] is True
|
||||
assert event["kind"] == "sweep_start_vision_timeout_warning"
|
||||
|
||||
|
||||
def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None:
|
||||
@@ -3453,7 +3435,7 @@ def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None:
|
||||
assert event["context"] == "sweep_motor_0"
|
||||
|
||||
|
||||
def test_slower_sweep_retry_extends_timeout_inversely() -> None:
|
||||
def test_same_speed_sweep_retry_keeps_timeout() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
|
||||
item = SweepItem(spec, 0, DIRECTION_DECREASING)
|
||||
key = (spec.motor_index, 0, DIRECTION_DECREASING)
|
||||
@@ -3466,11 +3448,11 @@ def test_slower_sweep_retry_extends_timeout_inversely() -> None:
|
||||
|
||||
assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
|
||||
node
|
||||
) == 112.5
|
||||
) == 90.0
|
||||
node.sweep_retry_counts[key] = 3
|
||||
assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
|
||||
node
|
||||
) == 180.0
|
||||
) == 90.0
|
||||
|
||||
|
||||
def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None:
|
||||
@@ -3726,7 +3708,7 @@ def test_thumb_yaw_zero_outlier_retries_only_localized_source_cycle(
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
]
|
||||
assert calls == ["prepare", "resume_sweep"]
|
||||
assert calls == ["pause:joint_fit_check_failed"]
|
||||
|
||||
|
||||
def test_previous_passed_joint_zero_offset_reads_formal_pointer(
|
||||
@@ -5327,7 +5309,7 @@ def _warning_band_node(tmp_path, attempts: dict) -> SimpleNamespace:
|
||||
return node
|
||||
|
||||
|
||||
def test_warning_band_failure_rescans_once_in_place(tmp_path) -> None:
|
||||
def test_warning_band_failure_stops_in_place(tmp_path) -> None:
|
||||
node = _warning_band_node(tmp_path, attempts={})
|
||||
failures = [
|
||||
{
|
||||
@@ -5343,22 +5325,19 @@ def test_warning_band_failure_rescans_once_in_place(tmp_path) -> None:
|
||||
node, node._spec, failures, allow_warning=True
|
||||
)
|
||||
|
||||
# The final fit rejects 0.779 against 0.75 on the same records, so the
|
||||
# first in-band result must rescan here instead of deferring the
|
||||
# rejection to the end of the session.
|
||||
assert paused is True
|
||||
assert "prepare_retry" in node._calls
|
||||
assert node._calls == ["pause_joint_fit_check_failed"]
|
||||
rows = [
|
||||
json.loads(line)
|
||||
for line in node.raw_path.read_text().splitlines()
|
||||
]
|
||||
kinds = {row["kind"] for row in rows}
|
||||
assert "provisional_fit_warning_rescan" in kinds
|
||||
assert "provisional_fit_warning_retry_exhausted" in kinds
|
||||
assert "provisional_fit_warning" not in kinds
|
||||
assert "fit_failure" in kinds
|
||||
|
||||
|
||||
def test_second_warning_band_result_retries_immediately(tmp_path) -> None:
|
||||
def test_second_warning_band_result_also_stops_without_motion(tmp_path) -> None:
|
||||
spec = next(
|
||||
item
|
||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
||||
@@ -5382,17 +5361,17 @@ def test_second_warning_band_result_retries_immediately(tmp_path) -> None:
|
||||
)
|
||||
|
||||
assert paused is True
|
||||
assert node._calls == ["prepare_retry", "return_resume_sweep"]
|
||||
assert node._calls == ["pause_joint_fit_check_failed"]
|
||||
rows = [
|
||||
json.loads(line)
|
||||
for line in node.raw_path.read_text().splitlines()
|
||||
]
|
||||
assert [row["kind"] for row in rows] == [
|
||||
"provisional_fit_warning_rescan",
|
||||
"provisional_fit_warning_retry_exhausted",
|
||||
"fit_failure",
|
||||
]
|
||||
assert rows[0]["attempt"] == 2
|
||||
assert rows[0]["attempt_limit"] == 3
|
||||
assert rows[0]["attempt_limit"] == 1
|
||||
|
||||
|
||||
def test_third_warning_band_result_stops_at_current_joint(tmp_path) -> None:
|
||||
@@ -5427,7 +5406,7 @@ def test_third_warning_band_result_stops_at_current_joint(tmp_path) -> None:
|
||||
"fit_failure",
|
||||
]
|
||||
assert rows[0]["attempt"] == 3
|
||||
assert rows[0]["attempt_limit"] == 3
|
||||
assert rows[0]["attempt_limit"] == 1
|
||||
|
||||
|
||||
def test_repeated_branch_clusters_stop_before_third_full_rescan(tmp_path) -> None:
|
||||
@@ -5459,7 +5438,7 @@ def test_repeated_branch_clusters_stop_before_third_full_rescan(tmp_path) -> Non
|
||||
allow_warning=True,
|
||||
)
|
||||
assert paused is True
|
||||
assert node._calls == ["prepare_retry", "return_resume_sweep"]
|
||||
assert node._calls == ["pause_joint_fit_check_failed"]
|
||||
|
||||
node._calls.clear()
|
||||
node.sweep_attempts[node._spec.key] = 2
|
||||
|
||||
@@ -0,0 +1,326 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_calibration.models import get_default_registry
|
||||
from linkerhand_calibration.core import (
|
||||
ArtifactPolicy,
|
||||
CalibrationProfile,
|
||||
CommandLayout,
|
||||
MeasurementPolicy,
|
||||
MeasurementSpec,
|
||||
MotionPolicy,
|
||||
ProfileKey,
|
||||
QualityPolicy,
|
||||
ScopePolicy,
|
||||
TagSpec,
|
||||
TaskSpec,
|
||||
ViewSpec,
|
||||
VisionRigSpec,
|
||||
ZeroSolvePolicy,
|
||||
)
|
||||
from linkerhand_calibration.core.urdf import (
|
||||
UrdfJointPatch,
|
||||
UrdfPatchSet,
|
||||
write_urdf_patches,
|
||||
)
|
||||
from linkerhand_calibration.runtime import (
|
||||
ACQUISITION_POLICY_VERSION,
|
||||
CalibrationEngine,
|
||||
)
|
||||
from linkerhand_calibration.runtime.adapters import ProfileSdkAdapter
|
||||
from linkerhand_calibration.core.geometry.rotation import fit_rotation_axis
|
||||
from linkerhand_calibration.models.g20.profile import JointCurveFit
|
||||
from linkerhand_calibration.models.l6.fitting import fit_coupling_model
|
||||
|
||||
|
||||
def test_all_product_profiles_use_unified_engine_and_sdk_adapter() -> None:
|
||||
registry = get_default_registry()
|
||||
products = {
|
||||
registered.profile.key.model: registered
|
||||
for registered in registry
|
||||
if registered.profile.key.layout
|
||||
in {"g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"}
|
||||
}
|
||||
assert set(products) == {"G20", "L6", "O6", "O12"}
|
||||
for registered in products.values():
|
||||
engine = registered.build_engine()
|
||||
adapter = registered.build_sdk_adapter()
|
||||
assert engine.checkpoint_token == ACQUISITION_POLICY_VERSION
|
||||
assert adapter.command_layout is registered.profile.command
|
||||
assert len(engine.scan_units()) == len(registered.profile.motion.tasks) * 8
|
||||
|
||||
|
||||
def test_only_o12_has_one_three_degree_mapping_probe() -> None:
|
||||
for registered in get_default_registry():
|
||||
if registered.profile.key.layout not in {
|
||||
"g20_right_19", "l6_right_8", "o6_right_8", "o12_right_16"
|
||||
}:
|
||||
continue
|
||||
engine = registered.build_engine()
|
||||
task = registered.profile.motion.tasks[0]
|
||||
probe = engine.mapping_probe_delta(task)
|
||||
if registered.profile.key.model == "O12":
|
||||
assert probe is not None
|
||||
assert 0.0 < abs(probe) <= math.radians(3.0)
|
||||
else:
|
||||
assert probe is None
|
||||
|
||||
|
||||
def test_short_vision_loss_and_low_ideal_rates_are_warnings() -> None:
|
||||
profile = next(
|
||||
item.profile for item in get_default_registry()
|
||||
if item.profile.key.layout == "o12_right_16"
|
||||
)
|
||||
result = CalibrationEngine(profile).evaluate_sweep(
|
||||
[index / 63.0 for index in range(64)],
|
||||
minimum_span=0.85,
|
||||
total_frames=90,
|
||||
joint_frame_rate=0.71,
|
||||
feedback_hz=12.0,
|
||||
detection_rate=0.72,
|
||||
bin_count=64,
|
||||
)
|
||||
assert result.passed
|
||||
assert any(value.startswith("tag_rate=") for value in result.warnings)
|
||||
assert any(value.startswith("joint_frame_rate=") for value in result.warnings)
|
||||
|
||||
|
||||
def test_observability_failure_gets_only_one_same_speed_rescan() -> None:
|
||||
profile = next(
|
||||
item.profile for item in get_default_registry()
|
||||
if item.profile.key.layout == "l6_right_8"
|
||||
)
|
||||
engine = CalibrationEngine(profile)
|
||||
failed = engine.evaluate_sweep(
|
||||
[index / 255.0 for index in range(20)],
|
||||
minimum_span=240.0 / 255.0,
|
||||
total_frames=200,
|
||||
joint_frame_rate=0.1,
|
||||
feedback_hz=5.0,
|
||||
detection_rate=0.1,
|
||||
)
|
||||
assert not failed.passed
|
||||
assert engine.retry_speed(40.0, 2) == 40.0
|
||||
with pytest.raises(ValueError, match="exactly one"):
|
||||
engine.retry_speed(40.0, 3)
|
||||
|
||||
|
||||
def test_o12_effective_endpoint_scale_is_not_an_internal_blind_spot() -> None:
|
||||
profile = next(
|
||||
item.profile for item in get_default_registry()
|
||||
if item.profile.key.layout == "o12_right_16"
|
||||
)
|
||||
result = CalibrationEngine(profile).evaluate_sweep(
|
||||
[index / 255.0 for index in range(20, 256)],
|
||||
minimum_span=0.85,
|
||||
total_frames=236,
|
||||
joint_frame_rate=1.0,
|
||||
feedback_hz=50.0,
|
||||
detection_rate=1.0,
|
||||
)
|
||||
assert result.passed
|
||||
assert result.metrics["gap_scope"] == "observed_feedback_span"
|
||||
assert result.metrics["maximum_bin_gap"] == 0
|
||||
|
||||
|
||||
def test_rotation_axis_uses_observed_endpoints_for_physical_feedback() -> None:
|
||||
commands = np.arange(25, 254, dtype=int)
|
||||
physical_angle = (253.0 - commands.astype(float)) / 228.0
|
||||
expected_axis = np.asarray([-0.25, 0.1, -0.96], dtype=float)
|
||||
expected_axis /= np.linalg.norm(expected_axis)
|
||||
vectors = physical_angle[:, None] * expected_axis[None, :]
|
||||
|
||||
axis = fit_rotation_axis(vectors, commands)
|
||||
projections = vectors @ axis
|
||||
|
||||
assert float(np.median(projections[commands <= 48])) > float(
|
||||
np.median(projections[commands >= 230])
|
||||
)
|
||||
|
||||
|
||||
def test_repeatable_hysteresis_is_diagnostic_when_holdout_passes() -> None:
|
||||
profile = next(
|
||||
item.profile for item in get_default_registry()
|
||||
if item.profile.key.layout == "o12_right_16"
|
||||
)
|
||||
curve = SimpleNamespace(
|
||||
maximum_hysteresis_rad=math.radians(4.2),
|
||||
maximum_monotonic_correction_rad=math.radians(0.2),
|
||||
)
|
||||
fit = SimpleNamespace(
|
||||
curves={"joint": curve},
|
||||
zero_offsets_rad={"joint": 0.0},
|
||||
travels_rad={"joint": 1.0},
|
||||
mimic_fits={},
|
||||
holdout_errors_rad={"joint": (0.0, math.radians(0.5))},
|
||||
)
|
||||
|
||||
result = CalibrationEngine(profile).result_from_fit(fit)
|
||||
|
||||
assert result.quality["passed"]
|
||||
assert result.quality["fit_diagnostics_by_joint"]["joint"][
|
||||
"maximum_hysteresis_rad"
|
||||
] == pytest.approx(math.radians(4.2))
|
||||
|
||||
|
||||
def test_directional_lookup_does_not_force_passive_curve_into_polynomial() -> None:
|
||||
source = tuple(float(value) for value in np.linspace(1.5, 0.0, 256))
|
||||
target = tuple(float(value) for value in 1.2 * np.asarray(source) ** 3)
|
||||
|
||||
def curve(values: tuple[float, ...]) -> JointCurveFit:
|
||||
return JointCurveFit(
|
||||
angle_rad=values,
|
||||
decreasing_rad=values,
|
||||
increasing_rad=tuple(value + 0.05 for value in values),
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.05,
|
||||
quality={},
|
||||
)
|
||||
|
||||
fit = fit_coupling_model(
|
||||
"source", "target", curve(source), curve(target),
|
||||
model="direction_aware_knots",
|
||||
minimum_multiplier=0.1,
|
||||
maximum_multiplier=3.0,
|
||||
)
|
||||
|
||||
assert fit.model == "direction_aware_knots"
|
||||
assert fit.urdf_mimic_policy == "endpoint_linear_fallback"
|
||||
assert fit.urdf_mimic_multiplier == pytest.approx(target[0] / source[0])
|
||||
assert fit.residual_max_rad == 0.0
|
||||
|
||||
|
||||
def test_o12_internal_blind_spot_larger_than_one_sixteenth_is_rejected() -> None:
|
||||
profile = next(
|
||||
item.profile for item in get_default_registry()
|
||||
if item.profile.key.layout == "o12_right_16"
|
||||
)
|
||||
values = [
|
||||
index / 255.0 for index in range(256)
|
||||
if not 100 <= index <= 130
|
||||
]
|
||||
result = CalibrationEngine(profile).evaluate_sweep(
|
||||
values,
|
||||
minimum_span=0.85,
|
||||
total_frames=len(values),
|
||||
joint_frame_rate=1.0,
|
||||
feedback_hz=50.0,
|
||||
detection_rate=1.0,
|
||||
)
|
||||
assert not result.passed
|
||||
assert any(value.startswith("maximum_gap=") for value in result.failures)
|
||||
|
||||
|
||||
def test_old_checkpoint_policy_is_intentionally_incompatible() -> None:
|
||||
profile = next(
|
||||
item.profile for item in get_default_registry()
|
||||
if item.profile.key.layout == "g20_right_19"
|
||||
)
|
||||
engine = CalibrationEngine(profile)
|
||||
assert not engine.resume_compatible({"profile_id": profile.key.profile_id})
|
||||
assert engine.resume_compatible({
|
||||
"profile_id": profile.key.profile_id,
|
||||
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||
})
|
||||
|
||||
|
||||
def test_removed_soft_stop_gates_are_not_in_live_nodes() -> None:
|
||||
package = Path(__file__).resolve().parents[1] / "linkerhand_calibration/models"
|
||||
live = "\n".join(
|
||||
(package / relative).read_text(encoding="utf-8")
|
||||
for relative in ("l6/node.py", "o12/node.py")
|
||||
)
|
||||
for removed in (
|
||||
"non_target_motor_moved",
|
||||
"o12_auxiliary_settle_timeout",
|
||||
"o12_sdk_feedback_coupling_hard_limit",
|
||||
"o12_roll_coupling_exceeded",
|
||||
):
|
||||
assert removed not in live
|
||||
|
||||
|
||||
def test_virtual_new_model_needs_only_adapter_profile_and_raw_urdf(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
joint = "finger_joint"
|
||||
profile = CalibrationProfile(
|
||||
key=ProfileKey("VIRTUAL", "right", "one_joint"),
|
||||
namespace="/virtual/right",
|
||||
command=CommandLayout(
|
||||
names=("finger",),
|
||||
baseline_u8=(255,),
|
||||
command_index_by_joint={joint: 0},
|
||||
urdf_joint_by_joint={joint: joint},
|
||||
),
|
||||
vision=VisionRigSpec(
|
||||
views=(ViewSpec("front", (
|
||||
TagSpec("base", 0, True), TagSpec("finger", 1),
|
||||
)),),
|
||||
common_frame="front",
|
||||
extrinsic_reference_view="front",
|
||||
),
|
||||
motion=MotionPolicy(tasks=(
|
||||
TaskSpec(
|
||||
"finger_sweep", "front", 0, (joint,),
|
||||
start_u8=255, end_u8=0, formal_speed_u8=40,
|
||||
),
|
||||
)),
|
||||
measurement=MeasurementPolicy(measurements={
|
||||
joint: MeasurementSpec(
|
||||
joint, "rotation", "front", "base", "finger"
|
||||
),
|
||||
}),
|
||||
zero=ZeroSolvePolicy(
|
||||
active_joints=frozenset({joint}),
|
||||
passive_joints=frozenset(),
|
||||
direct_zero_joints=(joint,),
|
||||
axis_joints=(joint,),
|
||||
mechanical_endpoint_joints=frozenset(),
|
||||
post_solve_endpoint_joints=frozenset(),
|
||||
mimic_source_by_joint={},
|
||||
cad_frozen_joints=frozenset(),
|
||||
),
|
||||
quality=QualityPolicy(
|
||||
(0, 1, 2), 3, frozenset({"holdout"}), isolated_holdout=True
|
||||
),
|
||||
scope=ScopePolicy(
|
||||
calibrate_joints={"full": frozenset({joint})},
|
||||
frozen_joints={"full": frozenset()},
|
||||
),
|
||||
artifacts=ArtifactPolicy(
|
||||
1, "virtual.json", "virtual.urdf", frozenset(),
|
||||
publish_corrected_urdf=True,
|
||||
),
|
||||
)
|
||||
engine = CalibrationEngine(profile)
|
||||
adapter = ProfileSdkAdapter(profile.command)
|
||||
assert len(engine.scan_units()) == 8
|
||||
assert adapter.parse_feedback(("finger",), (127.0,)) == (127.0,)
|
||||
|
||||
source = tmp_path / "virtual_raw.urdf"
|
||||
source.write_text(
|
||||
'<robot name="v"><link name="base"/><link name="tip"/>'
|
||||
'<joint name="finger_joint" type="revolute">'
|
||||
'<parent link="base"/><child link="tip"/>'
|
||||
'<origin xyz="0 0 0" rpy="0 0 0"/><axis xyz="0 0 1"/>'
|
||||
'<limit lower="0" upper="1" effort="1" velocity="1"/>'
|
||||
'</joint></robot>',
|
||||
encoding="utf-8",
|
||||
)
|
||||
output = write_urdf_patches(
|
||||
source_urdf=source,
|
||||
destination_urdf=tmp_path / "virtual.urdf",
|
||||
patches=UrdfPatchSet({
|
||||
joint: UrdfJointPatch(origin_rpy="0 0 0.01")
|
||||
}),
|
||||
)
|
||||
corrected = output.read_text(encoding="utf-8")
|
||||
assert 'rpy="0 0 0.01"' in corrected
|
||||
assert '<limit lower="0" upper="1" effort="1" velocity="1"/>' in corrected
|
||||
@@ -0,0 +1,59 @@
|
||||
import math
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from linkerhand_calibration.urdf_comparison import compare_urdfs, _Model, main
|
||||
|
||||
|
||||
def model(tmp_path, name, zero=0.):
|
||||
path = tmp_path / name
|
||||
path.write_text(f'''<robot name="test">
|
||||
<link name="base"/><link name="moving"/><link name="tip"/>
|
||||
<joint name="motor" type="revolute"><parent link="base"/><child link="moving"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 {zero}"/><axis xyz="0 0 1"/>
|
||||
<limit lower="0" upper="1" effort="1" velocity="1"/></joint>
|
||||
<joint name="tip_fixed" type="fixed"><parent link="moving"/><child link="tip"/>
|
||||
<origin xyz="0.1 0 0"/></joint></robot>''')
|
||||
return path
|
||||
|
||||
|
||||
def test_identical_geometry_has_zero_pose_difference(tmp_path):
|
||||
a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf")
|
||||
result = compare_urdfs(a, b)
|
||||
assert result["maximum_origin_distance_mm"] == pytest.approx(0.)
|
||||
assert result["maximum_orientation_difference_deg"] == pytest.approx(0.)
|
||||
assert not result["is_accuracy_certificate"]
|
||||
assert result["pose_count"] == 4
|
||||
assert result["scope"] == "same_urdf_joint_angles_not_sdk_feedback"
|
||||
|
||||
|
||||
def test_reports_actual_link_frame_shift_not_just_parameter_delta(tmp_path):
|
||||
a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf", .1)
|
||||
result = compare_urdfs(a, b)
|
||||
assert result["maximum_origin_distance_mm"] == pytest.approx(200 * math.sin(.05))
|
||||
assert result["maximum_orientation_difference_deg"] == pytest.approx(math.degrees(.1))
|
||||
|
||||
|
||||
def test_mimic_source_on_other_branch_is_resolved_recursively(tmp_path):
|
||||
path = model(tmp_path, "a.urdf")
|
||||
root = ET.parse(path).getroot()
|
||||
ET.SubElement(root, "link", name="follower_link")
|
||||
j = ET.SubElement(root, "joint", name="follower", type="revolute")
|
||||
ET.SubElement(j, "parent", link="base")
|
||||
ET.SubElement(j, "child", link="follower_link")
|
||||
ET.SubElement(j, "axis", xyz="0 0 1")
|
||||
ET.SubElement(j, "mimic", joint="motor", multiplier="0.5", offset="0.1")
|
||||
ET.ElementTree(root).write(path)
|
||||
poses = _Model(path).fk({"motor": .4})
|
||||
assert poses["follower_link"][0, 0] == pytest.approx(math.cos(.3))
|
||||
assert np.isfinite(poses["follower_link"]).all()
|
||||
|
||||
|
||||
def test_does_not_overwrite_existing_files(tmp_path):
|
||||
a, b = model(tmp_path, "a.urdf"), model(tmp_path, "b.urdf", .1)
|
||||
before = a.read_bytes()
|
||||
with pytest.raises(SystemExit):
|
||||
main(["--reference", str(a), "--candidate", str(b), "--output", str(a)])
|
||||
assert a.read_bytes() == before
|
||||
@@ -10,6 +10,7 @@ from linkerhand_calibration.core.urdf import (
|
||||
UrdfJointPatch,
|
||||
UrdfPatchSet,
|
||||
apply_urdf_patch_text,
|
||||
validate_urdf_mimic_ranges,
|
||||
write_urdf_patches,
|
||||
)
|
||||
|
||||
@@ -162,3 +163,37 @@ def test_patch_engine_copies_referenced_and_complete_mesh_bundle(
|
||||
assert (
|
||||
destination.parent / "meshes/auxiliary.stl"
|
||||
).read_bytes() == b"auxiliary"
|
||||
|
||||
|
||||
def test_mimic_range_validation_resolves_full_chain(tmp_path: Path) -> None:
|
||||
text = """<robot name="chain">
|
||||
<joint name="a" type="revolute"><limit lower="0" upper="1"/></joint>
|
||||
<joint name="b" type="revolute"><limit lower="0" upper="2.1"/><mimic joint="a" multiplier="2" offset="0"/></joint>
|
||||
<joint name="c" type="revolute"><limit lower="0" upper="1.1"/><mimic joint="b" multiplier="0.5" offset="0"/></joint>
|
||||
</robot>"""
|
||||
valid = tmp_path / "valid.urdf"
|
||||
valid.write_text(text, encoding="utf-8")
|
||||
ranges = validate_urdf_mimic_ranges(valid)
|
||||
assert ranges["b"] == pytest.approx((0.0, 2.0))
|
||||
assert ranges["c"] == pytest.approx((0.0, 1.0))
|
||||
|
||||
invalid = tmp_path / "invalid.urdf"
|
||||
invalid.write_text(text.replace('upper="1.1"', 'upper="0.9"'), encoding="utf-8")
|
||||
with pytest.raises(ValueError, match="c=0.100000000rad"):
|
||||
validate_urdf_mimic_ranges(invalid)
|
||||
|
||||
|
||||
def test_mimic_range_validation_never_enlarges_cad_excess(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
reference = tmp_path / "reference.urdf"
|
||||
reference.write_text(SOURCE_TEXT, encoding="utf-8")
|
||||
validate_urdf_mimic_ranges(reference, reference_urdf=reference)
|
||||
|
||||
worsened = tmp_path / "worsened.urdf"
|
||||
worsened.write_text(
|
||||
SOURCE_TEXT.replace('multiplier="1"', 'multiplier="1.1"', 1),
|
||||
encoding="utf-8",
|
||||
)
|
||||
with pytest.raises(ValueError, match="passive"):
|
||||
validate_urdf_mimic_ranges(worsened, reference_urdf=reference)
|
||||
|
||||
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
+1234
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user